Big push
This commit is contained in:
wittenator
parent
8383aae8ff
commit
be853eea93
14
Dockerfile
Normal file
14
Dockerfile
Normal 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
10
docker-compose.yml
Normal 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
|
||||
@ -3,34 +3,38 @@
|
||||
<ros2_control name="RealRobot" type="system">
|
||||
<hardware>
|
||||
<plugin>ros2_control_can/Ros2ControlCAN</plugin>
|
||||
<param name="left_wheel_name">left_wheel_joint</param>
|
||||
<param name="right_wheel_name">right_wheel_joint</param>
|
||||
<param name="loop_rate">30</param>
|
||||
<param name="device">/dev/ttyUSB0</param>
|
||||
<param name="baud_rate">57600</param>
|
||||
<param name="timeout">1000</param>
|
||||
<param name="front_left_wheel_name">front_left_steer_joint</param>
|
||||
<param name="front_right_wheel_name">front_right_steer_joint</param>
|
||||
<param name="rear_left_wheel_name">rear_left_wheel_joint</param>
|
||||
<param name="rear_right_wheel_name">rear_right_wheel_joint</param>
|
||||
<param name="device">vcan0</param>
|
||||
<param name="loop_rate">50</param>
|
||||
</hardware>
|
||||
<!-- Note everything below here is the same as the Gazebo one -->
|
||||
<joint name="left_wheel_joint">
|
||||
<command_interface name="velocity">
|
||||
<param name="min">-0.5</param>
|
||||
<param name="max">0.5</param>
|
||||
</command_interface>
|
||||
<state_interface name="velocity"/>
|
||||
<joint name="rear_right_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<state_interface name="position"/>
|
||||
<state_interface name="velocity"/>
|
||||
</joint>
|
||||
<joint name="right_wheel_joint">
|
||||
<command_interface name="velocity">
|
||||
<param name="min">-0.5</param>
|
||||
<param name="max">0.5</param>
|
||||
</command_interface>
|
||||
<state_interface name="velocity"/>
|
||||
<joint name="rear_left_wheel_joint">
|
||||
<command_interface name="velocity"/>
|
||||
<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>
|
||||
</ros2_control>
|
||||
<gazebo>
|
||||
<plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
|
||||
<parameters>$(find ft24_control)/config/dcaiti_config.yml</parameters>
|
||||
<plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system">
|
||||
<parameters>$(find ft24_control)/config/ft24_config.yml</parameters>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
</robot>
|
||||
@ -27,7 +27,7 @@
|
||||
</ros2_control>
|
||||
<gazebo>
|
||||
<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>
|
||||
</plugin>
|
||||
</gazebo>
|
||||
|
||||
@ -41,7 +41,7 @@ def generate_launch_description():
|
||||
controller_params = os.path.join(
|
||||
get_package_share_directory('ft24_control'), # <-- Replace with your package name
|
||||
'config',
|
||||
'dcaiti_config.yml'
|
||||
'ft24_config.yml'
|
||||
)
|
||||
|
||||
controller_manager = Node(
|
||||
@ -55,7 +55,7 @@ def generate_launch_description():
|
||||
diff_drive_spawner = Node(
|
||||
package="controller_manager",
|
||||
executable="spawner",
|
||||
arguments=["diff_cont"],
|
||||
arguments=["ackermann_steering_controller"],
|
||||
)
|
||||
|
||||
joint_broad_spawner = Node(
|
||||
|
||||
@ -62,7 +62,6 @@ def generate_launch_description():
|
||||
arguments=[
|
||||
'/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
|
||||
'/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/imu@sensor_msgs/msg/Imu@gz.msgs.IMU'
|
||||
],
|
||||
@ -135,6 +134,6 @@ def generate_launch_description():
|
||||
spawn_entity,
|
||||
delayed_diff_drive_spawner,
|
||||
delayed_joint_broad_spawner,
|
||||
imu_covariance_adder,
|
||||
#imu_covariance_adder,
|
||||
#ukf_node
|
||||
])
|
||||
|
||||
@ -3,9 +3,19 @@
|
||||
<package format="3">
|
||||
<name>ft24_control </name>
|
||||
<version>0.0.0</version>
|
||||
<description>TODO: Package description</description>
|
||||
<maintainer email="my_email@email.com">MY NAME</maintainer>
|
||||
<license>TODO: License declaration</license>
|
||||
<description>This package houses all ros2_control related code</description>
|
||||
<maintainer email="fake@email.com">Tim Korjakow</maintainer>
|
||||
<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>
|
||||
|
||||
|
||||
@ -1,7 +1,7 @@
|
||||
<?xml version="1.0" ?>
|
||||
<sdf version="1.6">
|
||||
<world name="empty">
|
||||
<physics name="phys" type="dart">
|
||||
<physics name="phys" type="ode">
|
||||
<max_step_size>0.005</max_step_size>
|
||||
<real_time_factor>1.0</real_time_factor>
|
||||
<ode>
|
||||
|
||||
@ -23,16 +23,41 @@ find_package(rclcpp REQUIRED)
|
||||
find_package(rclcpp_lifecycle 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(
|
||||
ros2_control_can
|
||||
PRIVATE
|
||||
include
|
||||
)
|
||||
|
||||
ament_target_dependencies(
|
||||
ros2_control_can
|
||||
hardware_interface
|
||||
@ -42,41 +67,20 @@ ament_target_dependencies(
|
||||
pluginlib
|
||||
)
|
||||
|
||||
pluginlib_export_plugin_description_file(hardware_interface robot_hardware.xml)
|
||||
|
||||
|
||||
|
||||
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
|
||||
pluginlib_export_plugin_description_file(
|
||||
hardware_interface
|
||||
robot_hardware.xml
|
||||
)
|
||||
ament_target_dependencies(
|
||||
fake_robot
|
||||
hardware_interface
|
||||
controller_manager
|
||||
rclcpp
|
||||
rclcpp_lifecycle
|
||||
pluginlib
|
||||
)
|
||||
|
||||
pluginlib_export_plugin_description_file(hardware_interface fake_robot_hardware.xml)
|
||||
|
||||
|
||||
|
||||
|
||||
install(
|
||||
TARGETS ros2_control_can fake_robot
|
||||
TARGETS ros2_control_can
|
||||
DESTINATION lib
|
||||
)
|
||||
|
||||
ament_export_libraries(
|
||||
ros2_control_can
|
||||
fake_robot
|
||||
)
|
||||
|
||||
ament_export_dependencies(
|
||||
hardware_interface
|
||||
controller_manager
|
||||
|
||||
1726
src/ros2_control_can/dbc/CAN1-MainFT24.dbc
Normal file
1726
src/ros2_control_can/dbc/CAN1-MainFT24.dbc
Normal file
File diff suppressed because it is too large
Load Diff
@ -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" ;
|
||||
|
||||
@ -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
51966
src/ros2_control_can/include/can1__main_ft24.h
Normal file
File diff suppressed because it is too large
Load Diff
@ -6,13 +6,12 @@
|
||||
|
||||
struct Config
|
||||
{
|
||||
std::string left_wheel_name = "left_wheel";
|
||||
std::string right_wheel_name = "right_wheel";
|
||||
float loop_rate = 30;
|
||||
std::string device = "/dev/ttyUSB0";
|
||||
int baud_rate = 115200;
|
||||
int timeout = 1000;
|
||||
int enc_counts_per_rev = 1920;
|
||||
std::string front_left_wheel_name = "front_left_wheel";
|
||||
std::string front_right_wheel_name = "front_right_wheel";
|
||||
std::string rear_left_wheel_name = "rear_left_wheel";
|
||||
std::string rear_right_wheel_name = "rear_right_wheel";
|
||||
float loop_rate = 50;
|
||||
std::string device = "vcan0";
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -12,6 +12,7 @@
|
||||
|
||||
#include "config.h"
|
||||
#include "wheel.h"
|
||||
#include "can1__main_ft24.h"
|
||||
|
||||
using hardware_interface::CallbackReturn;
|
||||
using hardware_interface::return_type;
|
||||
@ -44,12 +45,20 @@ private:
|
||||
|
||||
Config cfg_;
|
||||
|
||||
Wheel l_wheel_;
|
||||
Wheel r_wheel_;
|
||||
Wheel f_l_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_;
|
||||
|
||||
std::chrono::time_point<std::chrono::system_clock> time_;
|
||||
|
||||
int socket_instance;
|
||||
|
||||
};
|
||||
} // namespace ros2_control_can
|
||||
|
||||
@ -16,6 +16,7 @@ class Wheel
|
||||
double vel = 0;
|
||||
double eff = 0;
|
||||
double velSetPt = 0;
|
||||
double steer = 0;
|
||||
|
||||
Wheel() = default;
|
||||
|
||||
|
||||
@ -4,9 +4,12 @@
|
||||
<version>0.0.1</version>
|
||||
<description>Provides an interface between ROS2_control and a CAN device.</description>
|
||||
|
||||
<maintainer email="josh.newans@gmail.com">Tim Korjakow</maintainer>
|
||||
<author email="josh.newans@gmail.com">Tim Korjakow</author>
|
||||
<license>BSD</license>
|
||||
<maintainer email="fake@email.com">Tim Korjakow</maintainer>
|
||||
<author>Tim Korjakow</author>
|
||||
<license>Todo</license>
|
||||
|
||||
<depend>ros2_control</depend>
|
||||
<depend>ros2_controllers</depend>
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
@ -18,7 +21,6 @@
|
||||
<!-- <depend>rclcpp_lifecyle</depend> -->
|
||||
|
||||
|
||||
<test_depend>ament_add_gmock</test_depend>
|
||||
<test_depend>hardware_interface</test_depend>
|
||||
|
||||
<export>
|
||||
|
||||
33719
src/ros2_control_can/src/can1__main_ft24.cpp
Normal file
33719
src/ros2_control_can/src/can1__main_ft24.cpp
Normal file
File diff suppressed because it is too large
Load Diff
@ -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
|
||||
)
|
||||
@ -7,6 +7,13 @@
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#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 RPMSTORADS(a)(a*2.0*M_PI/60.0)
|
||||
#define REVSTORAD(a)(a*2.0*M_PI)
|
||||
@ -23,27 +30,29 @@ Ros2ControlCAN::Ros2ControlCAN()
|
||||
CallbackReturn Ros2ControlCAN::on_init(const hardware_interface::HardwareInfo & info)
|
||||
{
|
||||
if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS)
|
||||
{
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
{
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
RCLCPP_INFO(logger_, "Configuring...");
|
||||
|
||||
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"];
|
||||
cfg_.front_left_wheel_name = info_.hardware_parameters["front_left_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_.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
|
||||
l_wheel_.setup(cfg_.left_wheel_name);
|
||||
r_wheel_.setup(cfg_.right_wheel_name);
|
||||
f_l_wheel_.setup(cfg_.front_left_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
|
||||
//arduino_.setup(cfg_.device, cfg_.baud_rate, cfg_.timeout);
|
||||
// init tx frame
|
||||
can1__main_ft24_jetson_tx_init(&tx_frame);
|
||||
|
||||
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;
|
||||
|
||||
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));
|
||||
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(f_r_wheel_.name, hardware_interface::HW_IF_POSITION, &f_r_wheel_.steer));
|
||||
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_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;
|
||||
}
|
||||
@ -70,8 +81,10 @@ std::vector<hardware_interface::CommandInterface> Ros2ControlCAN::export_command
|
||||
|
||||
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));
|
||||
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(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;
|
||||
}
|
||||
@ -80,13 +93,44 @@ CallbackReturn Ros2ControlCAN::on_activate(const rclcpp_lifecycle::State & previ
|
||||
{
|
||||
RCLCPP_INFO(logger_, "Starting Controller...");
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
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;
|
||||
}
|
||||
|
||||
CallbackReturn Ros2ControlCAN::on_deactivate(const rclcpp_lifecycle::State & previous_state)
|
||||
{
|
||||
RCLCPP_INFO(logger_, "Stopping Controller...");
|
||||
|
||||
if (close(socket_instance) < 0) {
|
||||
perror("Close");
|
||||
return CallbackReturn::ERROR;
|
||||
}
|
||||
|
||||
return CallbackReturn::SUCCESS;
|
||||
}
|
||||
|
||||
@ -101,20 +145,17 @@ return_type Ros2ControlCAN::read(const rclcpp::Time & /* time */, const rclcpp::
|
||||
double deltaSeconds = diff.count();
|
||||
time_ = new_time;
|
||||
|
||||
|
||||
if (false)
|
||||
{
|
||||
return return_type::ERROR;
|
||||
}
|
||||
|
||||
//arduino_.readEncoderValues(l_wheel_.vel, r_wheel_.vel, l_wheel_.pos, r_wheel_.pos);
|
||||
// convert rpm to rad/s
|
||||
l_wheel_.vel = RPMSTORADS(l_wheel_.vel);
|
||||
r_wheel_.vel = RPMSTORADS(r_wheel_.vel);
|
||||
//l_wheel_.vel = RPMSTORADS(l_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;
|
||||
}
|
||||
|
||||
@ -124,8 +165,27 @@ return_type Ros2ControlCAN::write(const rclcpp::Time & /* time */, const rclcpp:
|
||||
{
|
||||
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;
|
||||
}
|
||||
|
||||
59
start.bash
59
start.bash
@ -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
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user