t.korjakow/ft24_control
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Big push

Browse Source
This commit is contained in:
wittenator 2023-12-02 19:41:01 +01:00
parent 8383aae8ff
commit be853eea93
22 changed files with 87620 additions and 387 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
Dockerfile Normal file
View File

@ -0,0 +1,14 @@
# build ROS2 Iron src in container and install depenedencies with rosdep
FROM ros:iron
ADD src /root/ros2_ws/src
RUN apt-get update && apt-get install -y \
python3-argcomplete \
python3-colcon-common-extensions \
python3-vcstool \
&& rm -rf /var/lib/apt/lists/*
CMD ["bash"]

10
docker-compose.yml Normal file
View File

@ -0,0 +1,10 @@
# docker compose file for the ROS2 Iron project
version: '3'
services:
ft24:
build: .
command: bash
volumes:
- ./src:/root/ros2_ws/src

0
src/ft24_control/config/dcaiti_config.yml → src/ft24_control/config/ft24_config.yml
View File

44
src/ft24_control/description/real_robot_control.xacro
View File

@ -3,34 +3,38 @@
<ros2_control name="RealRobot" type="system"> <ros2_control name="RealRobot" type="system">
<hardware> <hardware>
<plugin>ros2_control_can/Ros2ControlCAN</plugin> <plugin>ros2_control_can/Ros2ControlCAN</plugin>
<param name="left_wheel_name">left_wheel_joint</param> <param name="front_left_wheel_name">front_left_steer_joint</param>
<param name="right_wheel_name">right_wheel_joint</param> <param name="front_right_wheel_name">front_right_steer_joint</param>
<param name="loop_rate">30</param> <param name="rear_left_wheel_name">rear_left_wheel_joint</param>
<param name="device">/dev/ttyUSB0</param> <param name="rear_right_wheel_name">rear_right_wheel_joint</param>
<param name="baud_rate">57600</param> <param name="device">vcan0</param>
<param name="timeout">1000</param> <param name="loop_rate">50</param>
</hardware> </hardware>
<!-- Note everything below here is the same as the Gazebo one --> <!-- Note everything below here is the same as the Gazebo one -->
<joint name="left_wheel_joint"> <joint name="rear_right_wheel_joint">
<command_interface name="velocity"> <command_interface name="velocity"/>
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/>
</joint> </joint>
<joint name="right_wheel_joint"> <joint name="rear_left_wheel_joint">
<command_interface name="velocity"> <command_interface name="velocity"/>
<param name="min">-0.5</param>
<param name="max">0.5</param>
</command_interface>
<state_interface name="velocity"/>
<state_interface name="position"/> <state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="front_left_steer_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint>
<joint name="front_right_steer_joint">
<command_interface name="position"/>
<state_interface name="position"/>
<state_interface name="velocity"/>
</joint> </joint>
</ros2_control> </ros2_control>
<gazebo> <gazebo>
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control"> <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system">
<parameters>$(find ft24_control)/config/dcaiti_config.yml</parameters> <parameters>$(find ft24_control)/config/ft24_config.yml</parameters>
</plugin> </plugin>
</gazebo> </gazebo>
</robot> </robot>

2
src/ft24_control/description/ros2_control.xacro
View File

@ -27,7 +27,7 @@
</ros2_control> </ros2_control>
<gazebo> <gazebo>
<plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system"> <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system">
<parameters>$(find ft24_control)/config/dcaiti_config.yml</parameters> <parameters>$(find ft24_control)/config/ft24_config.yml</parameters>
<parameters>$(find ft24_control)/config/gaz_ros2_ctl_sim.yml</parameters> <parameters>$(find ft24_control)/config/gaz_ros2_ctl_sim.yml</parameters>
</plugin> </plugin>
</gazebo> </gazebo>

4
src/ft24_control/launch/launch_robot.py
View File

@ -41,7 +41,7 @@ def generate_launch_description():
controller_params = os.path.join( controller_params = os.path.join(
get_package_share_directory('ft24_control'), # <-- Replace with your package name get_package_share_directory('ft24_control'), # <-- Replace with your package name
'config', 'config',
'dcaiti_config.yml' 'ft24_config.yml'
) )
controller_manager = Node( controller_manager = Node(
@ -55,7 +55,7 @@ def generate_launch_description():
diff_drive_spawner = Node( diff_drive_spawner = Node(
package="controller_manager", package="controller_manager",
executable="spawner", executable="spawner",
arguments=["diff_cont"], arguments=["ackermann_steering_controller"],
) )
joint_broad_spawner = Node( joint_broad_spawner = Node(

3
src/ft24_control/launch/launch_sim.py
View File

@ -62,7 +62,6 @@ def generate_launch_description():
arguments=[ arguments=[
'/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock', '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
'/boxes@vision_msgs/msg/Detection2DArray@ignition.msgs.AnnotatedAxisAligned2DBox_V', '/boxes@vision_msgs/msg/Detection2DArray@ignition.msgs.AnnotatedAxisAligned2DBox_V',
'/lidar@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan',
'/lidar/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked', '/lidar/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked',
'lidar/imu@sensor_msgs/msg/Imu@gz.msgs.IMU' 'lidar/imu@sensor_msgs/msg/Imu@gz.msgs.IMU'
], ],
@ -135,6 +134,6 @@ def generate_launch_description():
spawn_entity, spawn_entity,
delayed_diff_drive_spawner, delayed_diff_drive_spawner,
delayed_joint_broad_spawner, delayed_joint_broad_spawner,
imu_covariance_adder, #imu_covariance_adder,
#ukf_node #ukf_node
]) ])

16
src/ft24_control/package.xml
View File

@ -3,9 +3,19 @@
<package format="3"> <package format="3">
<name>ft24_control </name> <name>ft24_control </name>
<version>0.0.0</version> <version>0.0.0</version>
<description>TODO: Package description</description> <description>This package houses all ros2_control related code</description>
<maintainer email="my_email@email.com">MY NAME</maintainer> <maintainer email="fake@email.com">Tim Korjakow</maintainer>
<license>TODO: License declaration</license> <license>Todo</license>
<depend>ros2_control</depend>
<depend>ros2_controllers</depend>
<depend>ros_gz</depend>
<depend>robot_localization</depend>
<depend>joint_state_publisher</depend>
<depend>twist_mux</depend>
<depend>gz_ros2_control</depend>
<depend>robot_state_publisher</depend>
<buildtool_depend>ament_cmake</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>

2
src/ft24_control/worlds/generated_worlds/AU2_skidpad.sdf
View File

@ -1,7 +1,7 @@
<?xml version="1.0" ?> <?xml version="1.0" ?>
<sdf version="1.6"> <sdf version="1.6">
<world name="empty"> <world name="empty">
<physics name="phys" type="dart"> <physics name="phys" type="ode">
<max_step_size>0.005</max_step_size> <max_step_size>0.005</max_step_size>
<real_time_factor>1.0</real_time_factor> <real_time_factor>1.0</real_time_factor>
<ode> <ode>

56
src/ros2_control_can/CMakeLists.txt
View File

@ -23,16 +23,41 @@ find_package(rclcpp REQUIRED)
find_package(rclcpp_lifecycle REQUIRED) find_package(rclcpp_lifecycle REQUIRED)
find_package(pluginlib REQUIRED) find_package(pluginlib REQUIRED)
set( GENERATED_CAN_HEADER
${CMAKE_CURRENT_SOURCE_DIR}/include/can1__main_ft24.h
)
set(
GENERATED_CAN_SRC
${CMAKE_CURRENT_SOURCE_DIR}/src/can1__main_ft24.cpp
)
add_custom_command(OUTPUT ${GENERATED_CAN_HEADER} ${GENERATED_CAN_SRC}
COMMAND python3 -m cantools generate_c_source --prune ${CMAKE_CURRENT_SOURCE_DIR}/dbc/CAN1-MainFT24.dbc
COMMAND mv ${CMAKE_CURRENT_SOURCE_DIR}/src/can1__main_ft24.h ${CMAKE_CURRENT_SOURCE_DIR}/include/can1__main_ft24.h
COMMAND mv ${CMAKE_CURRENT_SOURCE_DIR}/src/can1__main_ft24.c ${CMAKE_CURRENT_SOURCE_DIR}/src/can1__main_ft24.cpp
WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/src"
DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/dbc/CAN1-MainFT24.dbc"
PRE_BUILD
COMMENT "Generating CAN header files and code from DBC file"
VERBATIM
)
add_library(
ros2_control_can
SHARED
src/ros2_control_can.cpp
src/wheel.cpp
${GENERATED_CAN_HEADER}
${GENERATED_CAN_SRC}
)
add_library(ros2_control_can SHARED src/ros2_control_can.cpp src/wheel.cpp )
target_include_directories( target_include_directories(
ros2_control_can ros2_control_can
PRIVATE PRIVATE
include include
) )
ament_target_dependencies( ament_target_dependencies(
ros2_control_can ros2_control_can
hardware_interface hardware_interface
@ -42,41 +67,20 @@ ament_target_dependencies(
pluginlib pluginlib
) )
pluginlib_export_plugin_description_file(hardware_interface robot_hardware.xml) pluginlib_export_plugin_description_file(
add_library(fake_robot SHARED src/fake_robot.cpp src/wheel.cpp)
set_property(TARGET fake_robot PROPERTY POSITION_INDEPENDENT_CODE ON)
target_include_directories(
fake_robot
PRIVATE
include
)
ament_target_dependencies(
fake_robot
hardware_interface hardware_interface
controller_manager robot_hardware.xml
rclcpp
rclcpp_lifecycle
pluginlib
) )
pluginlib_export_plugin_description_file(hardware_interface fake_robot_hardware.xml)
install( install(
TARGETS ros2_control_can fake_robot TARGETS ros2_control_can
DESTINATION lib DESTINATION lib
) )
ament_export_libraries( ament_export_libraries(
ros2_control_can ros2_control_can
fake_robot
) )
ament_export_dependencies( ament_export_dependencies(
hardware_interface hardware_interface
controller_manager controller_manager

1726
src/ros2_control_can/dbc/CAN1-MainFT24.dbc Normal file
View File

File diff suppressed because it is too large Load Diff

93
src/ros2_control_can/dbc/FS23_DBC_DE_Berlin_TU_Car013.dbc
View File

@ -1,93 +0,0 @@
VERSION ""
NS_ :
NS_DESC_
CM_
BA_DEF_
BA_
VAL_
CAT_DEF_
CAT_
FILTER
BA_DEF_DEF_
EV_DATA_
ENVVAR_DATA_
SGTYPE_
SGTYPE_VAL_
BA_DEF_SGTYPE_
BA_SGTYPE_
SIG_TYPE_REF_
VAL_TABLE_
SIG_GROUP_
SIG_VALTYPE_
SIGTYPE_VALTYPE_
BO_TX_BU_
BA_DEF_REL_
BA_REL_
BA_DEF_DEF_REL_
BU_SG_REL_
BU_EV_REL_
BU_BO_REL_
SG_MUL_VAL_
BS_:
BU_: ABX RES FS_Datalogger
BO_ 1072 FS_Datalogger_Status: 6 FS_Datalogger
SG_ Current : 32|16@1+ (64,0) [0|4194240] "mA" ABX
SG_ Voltage : 16|16@1+ (16,0) [0|1048560] "mV" ABX
SG_ Status_Triggered_Current : 11|1@1- (1,0) [0|0] "" ABX
SG_ Status_Triggered_Voltage : 10|1@1- (1,0) [0|0] "" ABX
SG_ Status_Logging : 9|1@1- (1,0) [0|0] "" ABX
SG_ Status_Ready : 8|1@1- (1,0) [0|0] "" ABX
SG_ MsgCnt : 0|8@1+ (1,0) [0|255] "" ABX
BO_ 1298 DV_ContinuousMonitoring: 1 ABX
SG_ Pressure_charged : 5|1@1+ (1,0) [0|1] "bool" FS_Datalogger
SG_ PDU_comm_alive : 2|1@1+ (1,0) [0|1] "bool" FS_Datalogger
SG_ Position_sensors_closed : 4|1@1+ (1,0) [0|1] "bool" FS_Datalogger
SG_ AS_error : 3|1@1+ (1,0) [0|1] "bool" FS_Datalogger
SG_ AS_comm_alive : 1|1@1+ (1,0) [0|1] "bool" FS_Datalogger
SG_ SDC_opened : 0|1@1+ (1,0) [0|1] "bool" FS_Datalogger
BO_ 1280 DV_driving_dynamics_1: 8 ABX
SG_ Speed_actual : 0|8@1+ (1,0) [0|250] "km/h" FS_Datalogger
SG_ Speed_target : 8|8@1+ (1,0) [0|250] "km/h" FS_Datalogger
SG_ Steering_angle_actual : 16|8@1- (0.5,0) [-60|60] "°" FS_Datalogger
SG_ Steering_angle_target : 24|8@1- (0.5,0) [-60|60] "°" FS_Datalogger
SG_ Brake_hydr_actual : 32|8@1+ (1,0) [0|100] "%" FS_Datalogger
SG_ Brake_hydr_target : 40|8@1+ (1,0) [0|100] "%" FS_Datalogger
SG_ Motor_moment_actual : 48|8@1- (1,0) [-100|100] "%" FS_Datalogger
SG_ Motor_moment_target : 56|8@1- (1,0) [-100|100] "%" FS_Datalogger
BO_ 1281 DV_driving_dynamics_2: 6 ABX
SG_ Acceleration_longitudinal : 0|16@1- (0.00195313,0) [-50|50] "m/s^2" FS_Datalogger
SG_ Acceleration_lateral : 16|16@1- (0.00195313,0) [-50|50] "m/s^2" FS_Datalogger
SG_ Yaw_rate : 32|16@1- (0.0078125,0) [-200|200] "°/s" FS_Datalogger
BO_ 1282 DV_system_status: 5 ABX
SG_ AS_state : 0|3@1+ (1,0) [1|5] "" FS_Datalogger
SG_ EBS_state : 3|2@1+ (1,0) [1|3] "" FS_Datalogger
SG_ AMI_state : 5|3@1+ (1,0) [1|6] "" FS_Datalogger
SG_ Steering_state : 8|1@1+ (1,0) [0|1] "" FS_Datalogger
SG_ Service_brake_state : 9|2@1+ (1,0) [1|3] "" FS_Datalogger
SG_ Lap_counter : 11|4@1+ (1,0) [0|10] "" FS_Datalogger
SG_ Cones_count_actual : 15|8@1+ (1,0) [0|250] "" FS_Datalogger
SG_ Cones_count_all : 23|17@1+ (1,0) [0|131000] "" FS_Datalogger
BO_ 1297 DV_SEBSS_Pressures: 8 ABX
SG_ EBS_Pressure_tank_A : 0|16@1+ (0.1,0) [0|50] "Bar" FS_Datalogger
SG_ EBS_Pressure_tank_B : 16|16@1+ (0.1,0) [0|50] "Bar" FS_Datalogger
SG_ Brake_Pressure_front : 32|16@1+ (0.1,0) [0|200] "Bar" FS_Datalogger
SG_ Brake_Pressure_rear : 48|16@1+ (0.1,0) [0|200] "Bar" FS_Datalogger
VAL_ 1282 AS_state 1 "AS_state_off" 2 "AS_state_ready" 3 "AS_state_driving" 4 "AS_state_emergency_brake" 5 "AS_state_finish" ;
VAL_ 1282 EBS_state 1 "EBS_state_unavailable" 2 "EBS_state_armed" 3 "EBS_state_activated" ;
VAL_ 1282 AMI_state 1 "AMI_state_acceleration" 2 "AMI_state_skidpad" 3 "AMI_state_trackdrive" 4 "AMI_state_braketest" 5 "AMI_state_inspection" 6 "AMI_state_autocross" ;
VAL_ 1282 Service_brake_state 1 "Service_brake_state_disengaged" 2 "Service_brake_state_engaged" 3 "Service_brake_state_available" ;

9
src/ros2_control_can/fake_robot_hardware.xml
View File

@ -1,9 +0,0 @@
<library path="fake_robot">
<class name="fake_robot/FakeRobot"
type="FakeRobot"
base_class_type="hardware_interface::SystemInterface">
<description>
The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1.
</description>
</class>
</library>

51966
src/ros2_control_can/include/can1__main_ft24.h Normal file
View File

File diff suppressed because it is too large Load Diff

13
src/ros2_control_can/include/ros2_control_can/config.h
View File

@ -6,13 +6,12 @@
struct Config struct Config
{ {
std::string left_wheel_name = "left_wheel"; std::string front_left_wheel_name = "front_left_wheel";
std::string right_wheel_name = "right_wheel"; std::string front_right_wheel_name = "front_right_wheel";
float loop_rate = 30; std::string rear_left_wheel_name = "rear_left_wheel";
std::string device = "/dev/ttyUSB0"; std::string rear_right_wheel_name = "rear_right_wheel";
int baud_rate = 115200; float loop_rate = 50;
int timeout = 1000; std::string device = "vcan0";
int enc_counts_per_rev = 1920;
}; };

13
src/ros2_control_can/include/ros2_control_can/ros2_control_can.h
View File

@ -12,6 +12,7 @@
#include "config.h" #include "config.h"
#include "wheel.h" #include "wheel.h"
#include "can1__main_ft24.h"
using hardware_interface::CallbackReturn; using hardware_interface::CallbackReturn;
using hardware_interface::return_type; using hardware_interface::return_type;
@ -44,13 +45,21 @@ private:
Config cfg_; Config cfg_;
Wheel l_wheel_; Wheel f_l_wheel_;
Wheel r_wheel_; Wheel f_r_wheel_;
Wheel r_l_wheel_;
Wheel r_r_wheel_;
can1__main_ft24_jetson_tx_t tx_frame;
can1__main_ft24_jetson_rx_t rx_frame;
uint8_t can_buffer[64];
rclcpp::Logger logger_; rclcpp::Logger logger_;
std::chrono::time_point<std::chrono::system_clock> time_; std::chrono::time_point<std::chrono::system_clock> time_;
int socket_instance;
}; };
} // namespace ros2_control_can } // namespace ros2_control_can

1
src/ros2_control_can/include/ros2_control_can/wheel.h
View File

@ -16,6 +16,7 @@ class Wheel
double vel = 0; double vel = 0;
double eff = 0; double eff = 0;
double velSetPt = 0; double velSetPt = 0;
double steer = 0;
Wheel() = default; Wheel() = default;

10
src/ros2_control_can/package.xml
View File

@ -4,9 +4,12 @@
<version>0.0.1</version> <version>0.0.1</version>
<description>Provides an interface between ROS2_control and a CAN device.</description> <description>Provides an interface between ROS2_control and a CAN device.</description>
<maintainer email="josh.newans@gmail.com">Tim Korjakow</maintainer> <maintainer email="fake@email.com">Tim Korjakow</maintainer>
<author email="josh.newans@gmail.com">Tim Korjakow</author> <author>Tim Korjakow</author>
<license>BSD</license> <license>Todo</license>
<depend>ros2_control</depend>
<depend>ros2_controllers</depend>
<buildtool_depend>ament_cmake</buildtool_depend> <buildtool_depend>ament_cmake</buildtool_depend>
@ -18,7 +21,6 @@
<!-- <depend>rclcpp_lifecyle</depend> --> <!-- <depend>rclcpp_lifecyle</depend> -->
<test_depend>ament_add_gmock</test_depend>
<test_depend>hardware_interface</test_depend> <test_depend>hardware_interface</test_depend>
<export> <export>

33719
src/ros2_control_can/src/can1__main_ft24.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

129
src/ros2_control_can/src/fake_robot.cpp
View File

@ -1,129 +0,0 @@
#include "ros2_control_can/fake_robot.h"
#include "hardware_interface/types/hardware_interface_type_values.hpp"
#include "rclcpp/rclcpp.hpp"
#include <string>
CallbackReturn FakeRobot::on_init(const hardware_interface::HardwareInfo & info)
{
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
{
return CallbackReturn::ERROR;
}
RCLCPP_INFO(logger_, "Configuring Fakerobot...");
time_ = std::chrono::system_clock::now();
cfg_.left_wheel_name = info_.hardware_parameters["left_wheel_name"];
cfg_.right_wheel_name = info_.hardware_parameters["right_wheel_name"];
// Set up the wheels
// Note: It doesn't matter that we haven't set encoder counts per rev
// since the fake robot bypasses the encoder code completely
l_wheel_.setup(cfg_.left_wheel_name);
r_wheel_.setup(cfg_.right_wheel_name);
RCLCPP_INFO(logger_, "Finished Configuration Fakerobot");
return CallbackReturn::SUCCESS;
}
std::vector<hardware_interface::StateInterface> FakeRobot::export_state_interfaces()
{
// We need to set up a position and a velocity interface for each wheel
std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.vel));
state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_POSITION, &l_wheel_.pos));
state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.vel));
state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_POSITION, &r_wheel_.pos));
return state_interfaces;
}
std::vector<hardware_interface::CommandInterface> FakeRobot::export_command_interfaces()
{
// We need to set up a velocity command interface for each wheel
std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.emplace_back(hardware_interface::CommandInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.cmd));
command_interfaces.emplace_back(hardware_interface::CommandInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.cmd));
return command_interfaces;
}
#if OLD_ROS_CONTROL_SIGNATURE
return_type FakeRobot::read()
#else
return_type FakeRobot::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
#endif
{
// TODO fix chrono duration
// Calculate time delta
auto new_time = std::chrono::system_clock::now();
std::chrono::duration<double> diff = new_time - time_;
double deltaSeconds = diff.count();
time_ = new_time;
// Force the wheel position
l_wheel_.pos = l_wheel_.pos + l_wheel_.vel * deltaSeconds;
r_wheel_.pos = r_wheel_.pos + r_wheel_.vel * deltaSeconds;
return return_type::OK;
}
#if OLD_ROS_CONTROL_SIGNATURE
return_type FakeRobot::write()
#else
return_type FakeRobot::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */)
#endif
{
// Set the wheel velocities to directly match what is commanded
l_wheel_.vel = l_wheel_.cmd;
r_wheel_.vel = r_wheel_.cmd;
return return_type::OK;
}
#if OLD_ROS_CONTROL_SIGNATURE
CallbackReturn FakeRobot::on_activate(const rclcpp_lifecycle::State & /* previous_state */, const rclcpp_lifecycle::State & /* current_state */)
#else
CallbackReturn FakeRobot::on_activate(const rclcpp_lifecycle::State & /* previous_state */)
#endif
{
RCLCPP_INFO(logger_, "Starting Controller...");
return CallbackReturn::SUCCESS;
}
CallbackReturn FakeRobot::on_deactivate(const rclcpp_lifecycle::State & /* previous_state */)
{
RCLCPP_INFO(logger_, "Stopping Controller...");
return CallbackReturn::SUCCESS;
}
#include "pluginlib/class_list_macros.hpp"
PLUGINLIB_EXPORT_CLASS(
FakeRobot,
hardware_interface::SystemInterface
)

104
src/ros2_control_can/src/ros2_control_can.cpp
View File

@ -7,6 +7,13 @@
#include "rclcpp/rclcpp.hpp" #include "rclcpp/rclcpp.hpp"
#include <math.h> #include <math.h>
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <linux/can.h>
#include <linux/can/raw.h>
#define RADSTORPM(a)(a*60.0/(2.0*M_PI)) #define RADSTORPM(a)(a*60.0/(2.0*M_PI))
#define RPMSTORADS(a)(a*2.0*M_PI/60.0) #define RPMSTORADS(a)(a*2.0*M_PI/60.0)
#define REVSTORAD(a)(a*2.0*M_PI) #define REVSTORAD(a)(a*2.0*M_PI)
@ -31,19 +38,21 @@ CallbackReturn Ros2ControlCAN::on_init(const hardware_interface::HardwareInfo &
time_ = std::chrono::system_clock::now(); time_ = std::chrono::system_clock::now();
cfg_.left_wheel_name = info_.hardware_parameters["left_wheel_name"]; cfg_.front_left_wheel_name = info_.hardware_parameters["front_left_wheel_name"];
cfg_.right_wheel_name = info_.hardware_parameters["right_wheel_name"]; cfg_.front_right_wheel_name = info_.hardware_parameters["front_right_wheel_name"];
cfg_.rear_left_wheel_name = info_.hardware_parameters["rear_left_wheel_name"];
cfg_.rear_right_wheel_name = info_.hardware_parameters["rear_right_wheel_name"];
cfg_.loop_rate = std::stof(info_.hardware_parameters["loop_rate"]); cfg_.loop_rate = std::stof(info_.hardware_parameters["loop_rate"]);
cfg_.device = info_.hardware_parameters["device"]; cfg_.device = info_.hardware_parameters["device"];
cfg_.baud_rate = std::stoi(info_.hardware_parameters["baud_rate"]);
cfg_.timeout = std::stoi(info_.hardware_parameters["timeout"]);
// Set up the wheels // Set up the wheels
l_wheel_.setup(cfg_.left_wheel_name); f_l_wheel_.setup(cfg_.front_left_wheel_name);
r_wheel_.setup(cfg_.right_wheel_name); f_r_wheel_.setup(cfg_.front_right_wheel_name);
r_l_wheel_.setup(cfg_.front_left_wheel_name);
r_r_wheel_.setup(cfg_.front_right_wheel_name);
// Set up the Arduino // init tx frame
//arduino_.setup(cfg_.device, cfg_.baud_rate, cfg_.timeout); can1__main_ft24_jetson_tx_init(&tx_frame);
RCLCPP_INFO(logger_, "Finished Configuration"); RCLCPP_INFO(logger_, "Finished Configuration");
@ -56,10 +65,12 @@ std::vector<hardware_interface::StateInterface> Ros2ControlCAN::export_state_int
std::vector<hardware_interface::StateInterface> state_interfaces; std::vector<hardware_interface::StateInterface> state_interfaces;
state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.vel)); state_interfaces.emplace_back(hardware_interface::StateInterface(f_l_wheel_.name, hardware_interface::HW_IF_POSITION, &f_l_wheel_.steer));
state_interfaces.emplace_back(hardware_interface::StateInterface(l_wheel_.name, hardware_interface::HW_IF_POSITION, &l_wheel_.pos)); state_interfaces.emplace_back(hardware_interface::StateInterface(f_r_wheel_.name, hardware_interface::HW_IF_POSITION, &f_r_wheel_.steer));
state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.vel)); state_interfaces.emplace_back(hardware_interface::StateInterface(r_l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_l_wheel_.vel));
state_interfaces.emplace_back(hardware_interface::StateInterface(r_wheel_.name, hardware_interface::HW_IF_POSITION, &r_wheel_.pos)); state_interfaces.emplace_back(hardware_interface::StateInterface(r_r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_r_wheel_.vel));
state_interfaces.emplace_back(hardware_interface::StateInterface(r_l_wheel_.name, hardware_interface::HW_IF_POSITION, &r_l_wheel_.pos));
state_interfaces.emplace_back(hardware_interface::StateInterface(r_r_wheel_.name, hardware_interface::HW_IF_POSITION, &r_r_wheel_.pos));
return state_interfaces; return state_interfaces;
} }
@ -70,8 +81,10 @@ std::vector<hardware_interface::CommandInterface> Ros2ControlCAN::export_command
std::vector<hardware_interface::CommandInterface> command_interfaces; std::vector<hardware_interface::CommandInterface> command_interfaces;
command_interfaces.emplace_back(hardware_interface::CommandInterface(l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &l_wheel_.cmd)); command_interfaces.emplace_back(hardware_interface::CommandInterface(f_l_wheel_.name, hardware_interface::HW_IF_POSITION, &f_l_wheel_.steer));
command_interfaces.emplace_back(hardware_interface::CommandInterface(r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_wheel_.cmd)); command_interfaces.emplace_back(hardware_interface::CommandInterface(f_r_wheel_.name, hardware_interface::HW_IF_POSITION, &f_r_wheel_.steer));
command_interfaces.emplace_back(hardware_interface::CommandInterface(r_l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_l_wheel_.vel));
command_interfaces.emplace_back(hardware_interface::CommandInterface(r_r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_r_wheel_.vel));
return command_interfaces; return command_interfaces;
} }
@ -80,6 +93,32 @@ CallbackReturn Ros2ControlCAN::on_activate(const rclcpp_lifecycle::State & previ
{ {
RCLCPP_INFO(logger_, "Starting Controller..."); RCLCPP_INFO(logger_, "Starting Controller...");
if ((socket_instance = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
perror("Socket");
return CallbackReturn::ERROR;
}
struct ifreq ifr;
const char *cstr = cfg_.device.c_str();
strcpy(ifr.ifr_name, cstr );
ioctl(socket_instance, SIOCGIFINDEX, &ifr);
struct sockaddr_can addr;
memset(&addr, 0, sizeof(addr));
addr.can_family = AF_CAN;
addr.can_ifindex = ifr.ifr_ifindex;
if (bind(socket_instance, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
perror("Bind");
return CallbackReturn::ERROR;
}
struct can_filter rfilter[1];
rfilter[0].can_id = 0x550;
rfilter[0].can_mask = 0xFF0;
//rfilter[1].can_id = 0x200;
//rfilter[1].can_mask = 0x700;
setsockopt(socket_instance, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter));
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
@ -87,6 +126,11 @@ CallbackReturn Ros2ControlCAN::on_deactivate(const rclcpp_lifecycle::State & pre
{ {
RCLCPP_INFO(logger_, "Stopping Controller..."); RCLCPP_INFO(logger_, "Stopping Controller...");
if (close(socket_instance) < 0) {
perror("Close");
return CallbackReturn::ERROR;
}
return CallbackReturn::SUCCESS; return CallbackReturn::SUCCESS;
} }
@ -101,20 +145,17 @@ return_type Ros2ControlCAN::read(const rclcpp::Time & /* time */, const rclcpp::
double deltaSeconds = diff.count(); double deltaSeconds = diff.count();
time_ = new_time; time_ = new_time;
if (false) if (false)
{ {
return return_type::ERROR; return return_type::ERROR;
} }
//arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel, l_wheel_.pos, r_wheel_.pos);
// convert rpm to rad/s // convert rpm to rad/s
l_wheel_.vel = RPMSTORADS(l_wheel_.vel); //l_wheel_.vel = RPMSTORADS(l_wheel_.vel);
r_wheel_.vel = RPMSTORADS(r_wheel_.vel); //r_wheel_.vel = RPMSTORADS(r_wheel_.vel);
// convert rev to rad
l_wheel_.pos = REVSTORAD(l_wheel_.pos);
r_wheel_.pos = REVSTORAD(r_wheel_.pos);
return return_type::OK; return return_type::OK;
} }
@ -124,8 +165,27 @@ return_type Ros2ControlCAN::write(const rclcpp::Time & /* time */, const rclcpp:
{ {
return return_type::ERROR; return return_type::ERROR;
} }
// convert rad/s to rpm and write to motors
//arduino_.setMotorValues(RADSTORPM(l_wheel_.cmd), RADSTORPM(r_wheel_.cmd)); // send can frame
tx_frame.jetson_steering_angle = can1__main_ft24_jetson_tx_jetson_steering_angle_encode((f_l_wheel_.steer + f_r_wheel_.steer)/2.0);
// pack tx_frame
int tx_size = can1__main_ft24_jetson_tx_pack(can_buffer, &tx_frame, 64);
if (tx_size < 0) {
perror("Pack");
return return_type::ERROR;
}
// send tx_frame
struct can_frame frame;
frame.can_id = 0x550;
frame.can_dlc = tx_size;
memcpy(frame.data, can_buffer, tx_size);
if (::write(socket_instance, &frame, sizeof(struct can_frame)) < 0) {
perror("Write");
return return_type::ERROR;
}
return return_type::OK; return return_type::OK;
} }

59
start.bash
View File

@ -1,59 +0,0 @@
#!/bin/bash
DCAITI_PATH=.
# Define your windows, working dirs, and commands
declare -a windows=(
"performance"
"robot_control"
"remote_control"
"lidar"
# "slam"
"nav2"
)
declare -a work_dirs=(
$HOME
$DCAITI_PATH
$DCAITI_PATH/src/dcaitirobot/launch
$DCAITI_PATH
# $DCAITI_PATH/src/dcaitirobot/launch
$DCAITI_PATH/src/dcaiti_navigation/launch
)
declare -a commands=(
"htop"
"ros2 launch ft24_control launch_robot.py"
"ros2 launch remote_control_launch.py"
"ros2 launch velodyne velodyne-all-nodes-VLP16-launch.py"
# "ros2 launch launch_slam.py"
"ros2 launch start_nav2.py"
)
# Start tmux
tmux start-server
SESSION=dcaiti
# Create a new session without attaching yet
tmux new-session -d -s $SESSION
# Iterate over sessions array and create them
for index in ${!windows[*]}; do
window=${windows[$index]}
work_dir=${work_dirs[$index]}
command=${commands[$index]}
# If it's the first window, rename the existing window, otherwise create a new one
if [ $index -eq 0 ]; then
tmux rename-window -t $SESSION:0 $window
else
tmux new-window -t $SESSION -n $window
fi
# Switch to work_dir and execute the command
tmux send-keys -t $SESSION:$window "cd $work_dir" C-m
tmux send-keys -t $SESSION:$window "$command" C-m
done
# Finally attach to the tmux session
tmux attach-session -t $SESSION