diff --git a/README.md b/README.md index 19854cd..c9af8cf 100644 --- a/README.md +++ b/README.md @@ -8,8 +8,8 @@ The whole system (modules for robot control, remote control, lidar drivers and R The benefits of this modular setup lie in separation of the outputs of the different modules, enabling easier troubleshooting. Additionally, this allows terminating or restarting modules independently. For example, if the controller loses connection, this would be evident in the remote control window and the responsible module could be restarted without bringing the entire system down. ## Robot Control -The robot control is implemented in the `dcaiti_control` package and can be launched with \ -`ros2 launch dcaiti_control launch_robot.py`.\ +The robot control is implemented in the `ft24_control` package and can be launched with \ +`ros2 launch ft24_control launch_robot.py`.\ This launch file brings up the `ros2_control`-based nodes that serve as the interface between the physical robot and the ROS2 software stack. ## Remote Control diff --git a/src/dcaiti_control/CMakeLists.txt b/src/ft24_control/CMakeLists.txt similarity index 98% rename from src/dcaiti_control/CMakeLists.txt rename to src/ft24_control/CMakeLists.txt index ae34637..b29c22d 100644 --- a/src/dcaiti_control/CMakeLists.txt +++ b/src/ft24_control/CMakeLists.txt @@ -1,5 +1,5 @@ cmake_minimum_required(VERSION 3.5) -project(dcaiti_control) +project(ft24_control) # Default to C99 if(NOT CMAKE_C_STANDARD) diff --git a/src/dcaiti_control/LICENSE.md b/src/ft24_control/LICENSE.md similarity index 100% rename from src/dcaiti_control/LICENSE.md rename to src/ft24_control/LICENSE.md diff --git a/src/dcaiti_control/README.md b/src/ft24_control/README.md similarity index 100% rename from src/dcaiti_control/README.md rename to src/ft24_control/README.md diff --git a/src/dcaiti_control/config/dcaiti_config.yml b/src/ft24_control/config/dcaiti_config.yml similarity index 100% rename from src/dcaiti_control/config/dcaiti_config.yml rename to src/ft24_control/config/dcaiti_config.yml diff --git a/src/dcaiti_control/config/gaz_ros2_ctl_sim.yml b/src/ft24_control/config/gaz_ros2_ctl_sim.yml similarity index 100% rename from src/dcaiti_control/config/gaz_ros2_ctl_sim.yml rename to src/ft24_control/config/gaz_ros2_ctl_sim.yml diff --git a/src/dcaiti_control/config/gazebo_config.yml b/src/ft24_control/config/gazebo_config.yml similarity index 100% rename from src/dcaiti_control/config/gazebo_config.yml rename to src/ft24_control/config/gazebo_config.yml diff --git a/src/dcaiti_control/config/twist_mux.yml b/src/ft24_control/config/twist_mux.yml similarity index 100% rename from src/dcaiti_control/config/twist_mux.yml rename to src/ft24_control/config/twist_mux.yml diff --git a/src/dcaiti_control/config/ukf.yml b/src/ft24_control/config/ukf.yml similarity index 100% rename from src/dcaiti_control/config/ukf.yml rename to src/ft24_control/config/ukf.yml diff --git a/src/dcaiti_control/description/assets/ft24_wheel.dae b/src/ft24_control/description/assets/ft24_wheel.dae similarity index 100% rename from src/dcaiti_control/description/assets/ft24_wheel.dae rename to src/ft24_control/description/assets/ft24_wheel.dae diff --git a/src/dcaiti_control/description/assets/ft24_wheel_simplified.dae b/src/ft24_control/description/assets/ft24_wheel_simplified.dae similarity index 100% rename from src/dcaiti_control/description/assets/ft24_wheel_simplified.dae rename to src/ft24_control/description/assets/ft24_wheel_simplified.dae diff --git a/src/dcaiti_control/description/bbox_camera.xacro b/src/ft24_control/description/bbox_camera.xacro similarity index 100% rename from src/dcaiti_control/description/bbox_camera.xacro rename to src/ft24_control/description/bbox_camera.xacro diff --git a/src/dcaiti_control/description/blue_cone.xacro b/src/ft24_control/description/blue_cone.xacro similarity index 85% rename from src/dcaiti_control/description/blue_cone.xacro rename to src/ft24_control/description/blue_cone.xacro index cf90b90..31c25fd 100644 --- a/src/dcaiti_control/description/blue_cone.xacro +++ b/src/ft24_control/description/blue_cone.xacro @@ -12,7 +12,7 @@ - + - diffdrive_arduino/DiffDriveArduino + ros2_control_can/Ros2ControlCAN left_wheel_joint right_wheel_joint 30 @@ -30,7 +30,7 @@ - $(find dcaiti_control)/config/dcaiti_config.yml + $(find ft24_control)/config/dcaiti_config.yml \ No newline at end of file diff --git a/src/dcaiti_control/description/robot.urdf.xacro b/src/ft24_control/description/robot.urdf.xacro similarity index 96% rename from src/dcaiti_control/description/robot.urdf.xacro rename to src/ft24_control/description/robot.urdf.xacro index 606f84a..1d08e94 100644 --- a/src/dcaiti_control/description/robot.urdf.xacro +++ b/src/ft24_control/description/robot.urdf.xacro @@ -1,5 +1,5 @@ - + diff --git a/src/dcaiti_control/description/robot_core.xacro b/src/ft24_control/description/robot_core.xacro similarity index 100% rename from src/dcaiti_control/description/robot_core.xacro rename to src/ft24_control/description/robot_core.xacro diff --git a/src/dcaiti_control/description/ros2_control.xacro b/src/ft24_control/description/ros2_control.xacro similarity index 87% rename from src/dcaiti_control/description/ros2_control.xacro rename to src/ft24_control/description/ros2_control.xacro index d0c543d..2ca8827 100644 --- a/src/dcaiti_control/description/ros2_control.xacro +++ b/src/ft24_control/description/ros2_control.xacro @@ -27,8 +27,8 @@ - $(find dcaiti_control)/config/dcaiti_config.yml - $(find dcaiti_control)/config/gaz_ros2_ctl_sim.yml + $(find ft24_control)/config/dcaiti_config.yml + $(find ft24_control)/config/gaz_ros2_ctl_sim.yml \ No newline at end of file diff --git a/src/dcaiti_control/description/wheel.xacro b/src/ft24_control/description/wheel.xacro similarity index 98% rename from src/dcaiti_control/description/wheel.xacro rename to src/ft24_control/description/wheel.xacro index 16bfb8d..be0e2fa 100644 --- a/src/dcaiti_control/description/wheel.xacro +++ b/src/ft24_control/description/wheel.xacro @@ -129,7 +129,7 @@ - + - + - dcaiti_control + ft24_control 0.0.0 TODO: Package description MY NAME diff --git a/src/dcaiti_control/src/__init__.py b/src/ft24_control/src/__init__.py similarity index 100% rename from src/dcaiti_control/src/__init__.py rename to src/ft24_control/src/__init__.py diff --git a/src/dcaiti_control/src/imu_covariance_adder.py b/src/ft24_control/src/imu_covariance_adder.py similarity index 100% rename from src/dcaiti_control/src/imu_covariance_adder.py rename to src/ft24_control/src/imu_covariance_adder.py diff --git a/src/ft24_control/src/testf1.py b/src/ft24_control/src/testf1.py new file mode 100644 index 0000000..2cd0ece --- /dev/null +++ b/src/ft24_control/src/testf1.py @@ -0,0 +1,74 @@ +import cv2 +import numpy as np + +def draw_filled_track_boundaries(image, left_boundary, right_boundary): + # Combine left and right boundaries to create a single polygon + track_polygon = np.concatenate((left_boundary, right_boundary[::-1])) + + # Draw filled area between left and right boundaries + cv2.fillPoly(image, [track_polygon], color=(255, 255, 255)) + +def transform_boundaries(left_boundary, right_boundary, car_position, car_orientation): + # Homogeneous transformation matrix + transform_matrix = np.array([[np.cos(car_orientation), -np.sin(car_orientation), car_position[0]], + [np.sin(car_orientation), np.cos(car_orientation), car_position[1]], + [0, 0, 1]]) + + # Add homogeneous coordinate to boundaries + left_boundary_homogeneous = np.column_stack((left_boundary, np.ones(len(left_boundary)))) + right_boundary_homogeneous = np.column_stack((right_boundary, np.ones(len(right_boundary)))) + + # Apply transformation + left_boundary_transformed = np.dot(transform_matrix, left_boundary_homogeneous.T).T[:, :2].astype(np.int32) + right_boundary_transformed = np.dot(transform_matrix, right_boundary_homogeneous.T).T[:, :2].astype(np.int32) + + return left_boundary_transformed, right_boundary_transformed + +def main(): + # Image size and background color + image_size = (800, 600) + background_color = (0, 0, 0) # Black background + + # Create a black image + map_image = np.zeros((image_size[1], image_size[0], 3), dtype=np.uint8) + map_image[:] = background_color + + # Example curved track boundaries (replace with your own points) + num_points = 100 + theta = np.linspace(0, 2 * np.pi, num_points) + radius = 150 + + left_boundary_x = 400 + radius * np.cos(theta) + left_boundary_y = 300 + radius * np.sin(theta) + + right_boundary_x = 400 + radius * np.cos(theta - np.pi) + right_boundary_y = 500 + radius * np.sin(theta - np.pi) + + left_boundary = np.column_stack((left_boundary_x, left_boundary_y)).astype(np.int32) + right_boundary = np.column_stack((right_boundary_x, right_boundary_y)).astype(np.int32) + + # Car's global position and orientation (replace with your own values) + car_position = (100, 100) + car_orientation = np.pi / 4 # 45 degrees + + # Transform boundaries based on car's global position and orientation + left_boundary_transformed, right_boundary_transformed = transform_boundaries( + left_boundary, right_boundary, car_position, car_orientation + ) + + # Draw filled area between track boundaries on the image + draw_filled_track_boundaries(map_image, left_boundary_transformed, right_boundary_transformed) + + # Display the map image + cv2.imshow('Transformed Track Map', map_image) + cv2.waitKey(0) + cv2.destroyAllWindows() + +if __name__ == "__main__": + main() + + + + + + diff --git a/src/dcaiti_control/worlds/assets/blue_cone.dae b/src/ft24_control/worlds/assets/blue_cone.dae similarity index 100% rename from src/dcaiti_control/worlds/assets/blue_cone.dae rename to src/ft24_control/worlds/assets/blue_cone.dae diff --git a/src/dcaiti_control/worlds/assets/yellow_cone.dae b/src/ft24_control/worlds/assets/yellow_cone.dae similarity index 100% rename from src/dcaiti_control/worlds/assets/yellow_cone.dae rename to src/ft24_control/worlds/assets/yellow_cone.dae diff --git a/src/dcaiti_control/worlds/empty.sdf b/src/ft24_control/worlds/empty.sdf similarity index 100% rename from src/dcaiti_control/worlds/empty.sdf rename to src/ft24_control/worlds/empty.sdf diff --git a/src/dcaiti_control/worlds/empty.sdf.template b/src/ft24_control/worlds/empty.sdf.template similarity index 100% rename from src/dcaiti_control/worlds/empty.sdf.template rename to src/ft24_control/worlds/empty.sdf.template diff --git a/src/dcaiti_control/worlds/generate_track_world.py b/src/ft24_control/worlds/generate_track_world.py similarity index 100% rename from src/dcaiti_control/worlds/generate_track_world.py rename to src/ft24_control/worlds/generate_track_world.py diff --git a/src/dcaiti_control/worlds/generated_worlds/AU2_skidpad.sdf b/src/ft24_control/worlds/generated_worlds/AU2_skidpad.sdf similarity index 100% rename from src/dcaiti_control/worlds/generated_worlds/AU2_skidpad.sdf rename to src/ft24_control/worlds/generated_worlds/AU2_skidpad.sdf diff --git a/src/dcaiti_control/worlds/labeled_cone.sdf b/src/ft24_control/worlds/labeled_cone.sdf similarity index 100% rename from src/dcaiti_control/worlds/labeled_cone.sdf rename to src/ft24_control/worlds/labeled_cone.sdf diff --git a/src/dcaiti_control/worlds/tracks/AU2_skidpad.lyt b/src/ft24_control/worlds/tracks/AU2_skidpad.lyt similarity index 100% rename from src/dcaiti_control/worlds/tracks/AU2_skidpad.lyt rename to src/ft24_control/worlds/tracks/AU2_skidpad.lyt diff --git a/src/ros2_control_can/.DS_Store b/src/ros2_control_can/.DS_Store new file mode 100644 index 0000000..40fee54 Binary files /dev/null and b/src/ros2_control_can/.DS_Store differ diff --git a/src/ros2_control_can/CMakeLists.txt b/src/ros2_control_can/CMakeLists.txt new file mode 100644 index 0000000..67f7cff --- /dev/null +++ b/src/ros2_control_can/CMakeLists.txt @@ -0,0 +1,89 @@ +cmake_minimum_required(VERSION 3.5) +project(ros2_control_can) + +# Default to C99 +if(NOT CMAKE_C_STANDARD) + set(CMAKE_C_STANDARD 99) +endif() + + +# Default to C++14 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra) +endif() + +find_package(ament_cmake REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(controller_manager REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) +find_package(pluginlib REQUIRED) + + + + + +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 + controller_manager + rclcpp + rclcpp_lifecycle + 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 +) +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 + DESTINATION lib +) + +ament_export_libraries( + ros2_control_can + fake_robot +) +ament_export_dependencies( + hardware_interface + controller_manager + rclcpp + pluginlib +) + +ament_package() + + diff --git a/src/ros2_control_can/LICENSE b/src/ros2_control_can/LICENSE new file mode 100644 index 0000000..e3636e5 --- /dev/null +++ b/src/ros2_control_can/LICENSE @@ -0,0 +1,29 @@ +BSD 3-Clause License + +Copyright (c) 2020, Josh Newans +All rights reserved. + +Redistribution and use in source and binary forms, with or without +modification, are permitted provided that the following conditions are met: + +1. Redistributions of source code must retain the above copyright notice, this + list of conditions and the following disclaimer. + +2. Redistributions in binary form must reproduce the above copyright notice, + this list of conditions and the following disclaimer in the documentation + and/or other materials provided with the distribution. + +3. Neither the name of the copyright holder nor the names of its + contributors may be used to endorse or promote products derived from + this software without specific prior written permission. + +THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE +FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL +DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR +SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER +CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, +OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE +OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. diff --git a/src/ros2_control_can/README.md b/src/ros2_control_can/README.md new file mode 100644 index 0000000..bfa37a2 --- /dev/null +++ b/src/ros2_control_can/README.md @@ -0,0 +1,5 @@ +# ros2_control_can + +**Adds ROS2 Galactic and Humble support** + +This node is designed to provide an interface between a `diff_drive_controller` from `ros_control` and an Arduino running firmware from `ros_arduino_bridge`. diff --git a/src/ros2_control_can/dbc/FS23_DBC_DE_Berlin_TU_Car013.dbc b/src/ros2_control_can/dbc/FS23_DBC_DE_Berlin_TU_Car013.dbc new file mode 100644 index 0000000..200cc10 --- /dev/null +++ b/src/ros2_control_can/dbc/FS23_DBC_DE_Berlin_TU_Car013.dbc @@ -0,0 +1,93 @@ +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" ; + diff --git a/src/ros2_control_can/fake_robot_hardware.xml b/src/ros2_control_can/fake_robot_hardware.xml new file mode 100644 index 0000000..e0ae050 --- /dev/null +++ b/src/ros2_control_can/fake_robot_hardware.xml @@ -0,0 +1,9 @@ + + + + The ROS2 Control minimal robot protocol. This is example for start porting the robot from ROS 1. + + + diff --git a/src/ros2_control_can/include/.DS_Store b/src/ros2_control_can/include/.DS_Store new file mode 100644 index 0000000..aabfe3f Binary files /dev/null and b/src/ros2_control_can/include/.DS_Store differ diff --git a/src/ros2_control_can/include/ros2_control_can/.DS_Store b/src/ros2_control_can/include/ros2_control_can/.DS_Store new file mode 100644 index 0000000..f7f9506 Binary files /dev/null and b/src/ros2_control_can/include/ros2_control_can/.DS_Store differ diff --git a/src/ros2_control_can/include/ros2_control_can/config.h b/src/ros2_control_can/include/ros2_control_can/config.h new file mode 100644 index 0000000..faf51c1 --- /dev/null +++ b/src/ros2_control_can/include/ros2_control_can/config.h @@ -0,0 +1,19 @@ +#ifndef ROS2_CONTROL_CAN_CONFIG_H +#define ROS2_CONTROL_CAN_CONFIG_H + +#include + + +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; +}; + + +#endif // ROS2_CONTROL_CAN_CONFIG_H \ No newline at end of file diff --git a/src/ros2_control_can/include/ros2_control_can/fake_robot.h b/src/ros2_control_can/include/ros2_control_can/fake_robot.h new file mode 100644 index 0000000..96272b2 --- /dev/null +++ b/src/ros2_control_can/include/ros2_control_can/fake_robot.h @@ -0,0 +1,69 @@ +#ifndef ROS2_CONTROL_CAN_FAKE_ROBOT_H +#define ROS2_CONTROL_CAN_FAKE_ROBOT_H + + +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include + +#include "config.h" +#include "wheel.h" + +#if !OLD_ROS_CONTROL_SIGNATURE +using hardware_interface::CallbackReturn; +#endif +using hardware_interface::return_type; + +class FakeRobot : public hardware_interface::SystemInterface +{ + + +public: + FakeRobot(); + + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + #if OLD_ROS_CONTROL_SIGNATURE + CallbackReturn on_activate(const rclcpp_lifecycle::State & /* previous_state */, const rclcpp_lifecycle::State & /* current_state */) + #else + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + #endif + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + #if OLD_ROS_CONTROL_SIGNATURE + return_type read() override; + #else + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + #endif + + #if OLD_ROS_CONTROL_SIGNATURE + return_type write() override; + #else + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + #endif + + +private: + + Config cfg_; + + Wheel l_wheel_; + Wheel r_wheel_; + + rclcpp::Logger logger_; + + std::chrono::time_point time_; + +}; + + +#endif // ROS2_CONTROL_CAN_FAKE_ROBOT_H \ No newline at end of file diff --git a/src/ros2_control_can/include/ros2_control_can/ros2_control_can.h b/src/ros2_control_can/include/ros2_control_can/ros2_control_can.h new file mode 100644 index 0000000..700d7a4 --- /dev/null +++ b/src/ros2_control_can/include/ros2_control_can/ros2_control_can.h @@ -0,0 +1,57 @@ +#ifndef ROS2_CONTROL_CAN_REAL_ROBOT_H +#define ROS2_CONTROL_CAN_REAL_ROBOT_H + +#include +#include "rclcpp/rclcpp.hpp" + +#include +#include +#include +#include +#include + +#include "config.h" +#include "wheel.h" + +using hardware_interface::CallbackReturn; +using hardware_interface::return_type; + +namespace ros2_control_can +{ + +class Ros2ControlCAN : public hardware_interface::SystemInterface +{ + + +public: + Ros2ControlCAN(); + + CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + + return_type read(const rclcpp::Time & time, const rclcpp::Duration & period) override; + + return_type write(const rclcpp::Time & time, const rclcpp::Duration & period) override; + +private: + + Config cfg_; + + Wheel l_wheel_; + Wheel r_wheel_; + + rclcpp::Logger logger_; + + std::chrono::time_point time_; + +}; +} // namespace ros2_control_can + +#endif // ROS2_CONTROL_CAN_REAL_ROBOT_H \ No newline at end of file diff --git a/src/ros2_control_can/include/ros2_control_can/wheel.h b/src/ros2_control_can/include/ros2_control_can/wheel.h new file mode 100644 index 0000000..f4c5d59 --- /dev/null +++ b/src/ros2_control_can/include/ros2_control_can/wheel.h @@ -0,0 +1,33 @@ +#ifndef ROS2_CONTROL_CAN_WHEEL_H +#define ROS2_CONTROL_CAN_WHEEL_H + +#include + + + +class Wheel +{ + public: + + std::string name = ""; + int enc = 0; + double cmd = 0; + double pos = 0; + double vel = 0; + double eff = 0; + double velSetPt = 0; + + Wheel() = default; + + Wheel(const std::string &wheel_name); + + void setup(const std::string &wheel_name); + + double calcEncAngle(); + + + +}; + + +#endif // ROS2_CONTROL_CAN_WHEEL_H \ No newline at end of file diff --git a/src/ros2_control_can/package.xml b/src/ros2_control_can/package.xml new file mode 100644 index 0000000..06f8dc5 --- /dev/null +++ b/src/ros2_control_can/package.xml @@ -0,0 +1,29 @@ + + + ros2_control_can + 0.0.1 + Provides an interface between ROS2_control and a CAN device. + + Tim Korjakow + Tim Korjakow + BSD + + ament_cmake + + hardware_interface + controller_manager + pluginlib + rclcpp + rclcpp_lifecycle + + + +ament_add_gmock +hardware_interface + + + ament_cmake + + + + diff --git a/src/ros2_control_can/robot_hardware.xml b/src/ros2_control_can/robot_hardware.xml new file mode 100644 index 0000000..d8fd6d1 --- /dev/null +++ b/src/ros2_control_can/robot_hardware.xml @@ -0,0 +1,9 @@ + + + + Hardware Interface for Differential Drive Arduino controller. + + + diff --git a/src/ros2_control_can/src/.DS_Store b/src/ros2_control_can/src/.DS_Store new file mode 100644 index 0000000..0f8a2d8 Binary files /dev/null and b/src/ros2_control_can/src/.DS_Store differ diff --git a/src/ros2_control_can/src/fake_robot.cpp b/src/ros2_control_can/src/fake_robot.cpp new file mode 100644 index 0000000..3eeb863 --- /dev/null +++ b/src/ros2_control_can/src/fake_robot.cpp @@ -0,0 +1,129 @@ +#include "ros2_control_can/fake_robot.h" + + +#include "hardware_interface/types/hardware_interface_type_values.hpp" + +#include "rclcpp/rclcpp.hpp" +#include + + +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 FakeRobot::export_state_interfaces() +{ + // We need to set up a position and a velocity interface for each wheel + + std::vector 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 FakeRobot::export_command_interfaces() +{ + // We need to set up a velocity command interface for each wheel + + std::vector 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 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 +) \ No newline at end of file diff --git a/src/ros2_control_can/src/ros2_control_can.cpp b/src/ros2_control_can/src/ros2_control_can.cpp new file mode 100644 index 0000000..f403d77 --- /dev/null +++ b/src/ros2_control_can/src/ros2_control_can.cpp @@ -0,0 +1,140 @@ +#include "ros2_control_can/ros2_control_can.h" + + +#include "hardware_interface/types/hardware_interface_type_values.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + +#include "rclcpp/rclcpp.hpp" +#include + +#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) +#define RADSTOREV(a)(a/(2.0*M_PI)) + +namespace ros2_control_can +{ + + +Ros2ControlCAN::Ros2ControlCAN() + : logger_(rclcpp::get_logger("Ros2ControlCAN")) +{} + +CallbackReturn Ros2ControlCAN::on_init(const hardware_interface::HardwareInfo & info) +{ + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) +{ + 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_.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); + + // Set up the Arduino + //arduino_.setup(cfg_.device, cfg_.baud_rate, cfg_.timeout); + + RCLCPP_INFO(logger_, "Finished Configuration"); + + return CallbackReturn::SUCCESS; +} + +std::vector Ros2ControlCAN::export_state_interfaces() +{ + // We need to set up a position and a velocity interface for each wheel + + std::vector 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 Ros2ControlCAN::export_command_interfaces() +{ + // We need to set up a velocity command interface for each wheel + + std::vector 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; +} + +CallbackReturn Ros2ControlCAN::on_activate(const rclcpp_lifecycle::State & previous_state) +{ + RCLCPP_INFO(logger_, "Starting Controller..."); + + return CallbackReturn::SUCCESS; +} + +CallbackReturn Ros2ControlCAN::on_deactivate(const rclcpp_lifecycle::State & previous_state) +{ + RCLCPP_INFO(logger_, "Stopping Controller..."); + + return CallbackReturn::SUCCESS; +} + +return_type Ros2ControlCAN::read(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) +{ + + // TODO fix chrono duration + + // Calculate time delta + auto new_time = std::chrono::system_clock::now(); + std::chrono::duration diff = new_time - time_; + 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); + + + // convert rev to rad + l_wheel_.pos = REVSTORAD(l_wheel_.pos); + r_wheel_.pos = REVSTORAD(r_wheel_.pos); + return return_type::OK; +} + +return_type Ros2ControlCAN::write(const rclcpp::Time & /* time */, const rclcpp::Duration & /* period */) +{ + if (false) + { + return return_type::ERROR; + } + // convert rad/s to rpm and write to motors + //arduino_.setMotorValues(RADSTORPM(l_wheel_.cmd), RADSTORPM(r_wheel_.cmd)); + + return return_type::OK; +} + +} // namespace ros2_control_can + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + ros2_control_can::Ros2ControlCAN, + hardware_interface::SystemInterface +) diff --git a/src/ros2_control_can/src/wheel.cpp b/src/ros2_control_can/src/wheel.cpp new file mode 100644 index 0000000..420de1d --- /dev/null +++ b/src/ros2_control_can/src/wheel.cpp @@ -0,0 +1,15 @@ +#include "ros2_control_can/wheel.h" + +#include + + +Wheel::Wheel(const std::string &wheel_name) +{ + setup(wheel_name); +} + + +void Wheel::setup(const std::string &wheel_name) +{ + name = wheel_name; +} \ No newline at end of file diff --git a/start.bash b/start.bash index dff5998..0938e0d 100755 --- a/start.bash +++ b/start.bash @@ -21,7 +21,7 @@ declare -a work_dirs=( ) declare -a commands=( "htop" - "ros2 launch dcaiti_control launch_robot.py" + "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"