From 8383aae8ffc52ecf749646fdf77d3634568ec3e3 Mon Sep 17 00:00:00 2001
From: wittenator <9154515+wittenator@users.noreply.github.com>
Date: Mon, 6 Nov 2023 15:50:45 +0100
Subject: [PATCH] Renamed packages and added skeleton for HWInterface
---
README.md | 4 +-
.../CMakeLists.txt | 2 +-
.../LICENSE.md | 0
.../README.md | 0
.../config/dcaiti_config.yml | 0
.../config/gaz_ros2_ctl_sim.yml | 0
.../config/gazebo_config.yml | 0
.../config/twist_mux.yml | 0
.../config/ukf.yml | 0
.../description/assets/ft24_wheel.dae | 0
.../assets/ft24_wheel_simplified.dae | 0
.../description/bbox_camera.xacro | 0
.../description/blue_cone.xacro | 2 +-
.../description/colours.xacro | 0
.../description/imu.xacro | 0
.../description/inertial_macros.xacro | 0
.../description/lidar.xacro | 0
.../description/real_robot_control.xacro | 4 +-
.../description/robot.urdf.xacro | 2 +-
.../description/robot_core.xacro | 0
.../description/ros2_control.xacro | 4 +-
.../description/wheel.xacro | 4 +-
.../env-hooks/ft24_control.dsv.in} | 0
.../ft24_control}/__init__.py | 0
.../launch/launch_robot.py | 4 +-
.../launch/launch_sim.py | 4 +-
.../launch/rsp.launch.py | 4 +-
.../package.xml | 2 +-
.../src/__init__.py | 0
.../src/imu_covariance_adder.py | 0
src/ft24_control/src/testf1.py | 74 +++++++++
.../worlds/assets/blue_cone.dae | 0
.../worlds/assets/yellow_cone.dae | 0
.../worlds/empty.sdf | 0
.../worlds/empty.sdf.template | 0
.../worlds/generate_track_world.py | 0
.../worlds/generated_worlds/AU2_skidpad.sdf | 0
.../worlds/labeled_cone.sdf | 0
.../worlds/tracks/AU2_skidpad.lyt | Bin
src/ros2_control_can/.DS_Store | Bin 0 -> 6148 bytes
src/ros2_control_can/CMakeLists.txt | 89 +++++++++++
src/ros2_control_can/LICENSE | 29 ++++
src/ros2_control_can/README.md | 5 +
.../dbc/FS23_DBC_DE_Berlin_TU_Car013.dbc | 93 ++++++++++++
src/ros2_control_can/fake_robot_hardware.xml | 9 ++
src/ros2_control_can/include/.DS_Store | Bin 0 -> 6148 bytes
.../include/ros2_control_can/.DS_Store | Bin 0 -> 6148 bytes
.../include/ros2_control_can/config.h | 19 +++
.../include/ros2_control_can/fake_robot.h | 69 +++++++++
.../ros2_control_can/ros2_control_can.h | 57 +++++++
.../include/ros2_control_can/wheel.h | 33 +++++
src/ros2_control_can/package.xml | 29 ++++
src/ros2_control_can/robot_hardware.xml | 9 ++
src/ros2_control_can/src/.DS_Store | Bin 0 -> 6148 bytes
src/ros2_control_can/src/fake_robot.cpp | 129 ++++++++++++++++
src/ros2_control_can/src/ros2_control_can.cpp | 140 ++++++++++++++++++
src/ros2_control_can/src/wheel.cpp | 15 ++
start.bash | 2 +-
58 files changed, 818 insertions(+), 19 deletions(-)
rename src/{dcaiti_control => ft24_control}/CMakeLists.txt (98%)
rename src/{dcaiti_control => ft24_control}/LICENSE.md (100%)
rename src/{dcaiti_control => ft24_control}/README.md (100%)
rename src/{dcaiti_control => ft24_control}/config/dcaiti_config.yml (100%)
rename src/{dcaiti_control => ft24_control}/config/gaz_ros2_ctl_sim.yml (100%)
rename src/{dcaiti_control => ft24_control}/config/gazebo_config.yml (100%)
rename src/{dcaiti_control => ft24_control}/config/twist_mux.yml (100%)
rename src/{dcaiti_control => ft24_control}/config/ukf.yml (100%)
rename src/{dcaiti_control => ft24_control}/description/assets/ft24_wheel.dae (100%)
rename src/{dcaiti_control => ft24_control}/description/assets/ft24_wheel_simplified.dae (100%)
rename src/{dcaiti_control => ft24_control}/description/bbox_camera.xacro (100%)
rename src/{dcaiti_control => ft24_control}/description/blue_cone.xacro (85%)
rename src/{dcaiti_control => ft24_control}/description/colours.xacro (100%)
rename src/{dcaiti_control => ft24_control}/description/imu.xacro (100%)
rename src/{dcaiti_control => ft24_control}/description/inertial_macros.xacro (100%)
rename src/{dcaiti_control => ft24_control}/description/lidar.xacro (100%)
rename src/{dcaiti_control => ft24_control}/description/real_robot_control.xacro (90%)
rename src/{dcaiti_control => ft24_control}/description/robot.urdf.xacro (96%)
rename src/{dcaiti_control => ft24_control}/description/robot_core.xacro (100%)
rename src/{dcaiti_control => ft24_control}/description/ros2_control.xacro (87%)
rename src/{dcaiti_control => ft24_control}/description/wheel.xacro (98%)
rename src/{dcaiti_control/env-hooks/dcaiti_control.dsv.in => ft24_control/env-hooks/ft24_control.dsv.in} (100%)
rename src/{dcaiti_control/dcaiti_control => ft24_control/ft24_control}/__init__.py (100%)
rename src/{dcaiti_control => ft24_control}/launch/launch_robot.py (95%)
rename src/{dcaiti_control => ft24_control}/launch/launch_sim.py (98%)
rename src/{dcaiti_control => ft24_control}/launch/rsp.launch.py (93%)
rename src/{dcaiti_control => ft24_control}/package.xml (94%)
rename src/{dcaiti_control => ft24_control}/src/__init__.py (100%)
rename src/{dcaiti_control => ft24_control}/src/imu_covariance_adder.py (100%)
create mode 100644 src/ft24_control/src/testf1.py
rename src/{dcaiti_control => ft24_control}/worlds/assets/blue_cone.dae (100%)
rename src/{dcaiti_control => ft24_control}/worlds/assets/yellow_cone.dae (100%)
rename src/{dcaiti_control => ft24_control}/worlds/empty.sdf (100%)
rename src/{dcaiti_control => ft24_control}/worlds/empty.sdf.template (100%)
rename src/{dcaiti_control => ft24_control}/worlds/generate_track_world.py (100%)
rename src/{dcaiti_control => ft24_control}/worlds/generated_worlds/AU2_skidpad.sdf (100%)
rename src/{dcaiti_control => ft24_control}/worlds/labeled_cone.sdf (100%)
rename src/{dcaiti_control => ft24_control}/worlds/tracks/AU2_skidpad.lyt (100%)
create mode 100644 src/ros2_control_can/.DS_Store
create mode 100644 src/ros2_control_can/CMakeLists.txt
create mode 100644 src/ros2_control_can/LICENSE
create mode 100644 src/ros2_control_can/README.md
create mode 100644 src/ros2_control_can/dbc/FS23_DBC_DE_Berlin_TU_Car013.dbc
create mode 100644 src/ros2_control_can/fake_robot_hardware.xml
create mode 100644 src/ros2_control_can/include/.DS_Store
create mode 100644 src/ros2_control_can/include/ros2_control_can/.DS_Store
create mode 100644 src/ros2_control_can/include/ros2_control_can/config.h
create mode 100644 src/ros2_control_can/include/ros2_control_can/fake_robot.h
create mode 100644 src/ros2_control_can/include/ros2_control_can/ros2_control_can.h
create mode 100644 src/ros2_control_can/include/ros2_control_can/wheel.h
create mode 100644 src/ros2_control_can/package.xml
create mode 100644 src/ros2_control_can/robot_hardware.xml
create mode 100644 src/ros2_control_can/src/.DS_Store
create mode 100644 src/ros2_control_can/src/fake_robot.cpp
create mode 100644 src/ros2_control_can/src/ros2_control_can.cpp
create mode 100644 src/ros2_control_can/src/wheel.cpp
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 0000000000000000000000000000000000000000..40fee54d6273d46f17d8bc4c234805a9fe55d6b2
GIT binary patch
literal 6148
zcmeHK%Wl&^6upy##v!7L5QuJ&EU}G3feI3_Nz-Ie30Tw!7Jz~so5rf+iEM`;MUk?G
zf8ZC`@+JHWD>(BgvfaRf1wz%_Xy(l0o|(+?^^Avz*kIw`BWe+mg(R?c71o(FD5W16BhA)l5K1M1)(%qS|3#%`BJ
zt7FfJ@joJ3fE$#&KmFO;Tc&fpzOUXq3e#+V{~N1RYgcyaR^4h^Z=5M#I+>d-(t(@2
z61C@?N8Wka^wWX&W!xxJR~9}&$#}VagraSic=|}NpU77)Z5~9N#a7UxfBmrGEB#l
z)l^lJ8wHF4MuA@ni2cDv64=+cQ7E?#Wbz6Ctf5&N%HlIYaBPizjT?pNfiXn|Dk{@g
z45sL4x3yni<3^#P6VsOure|jQhQj3R7~hue#QF+NZWJ&I6cwndYD=8|qu<~EizHJs
z3K#`0l>)3bbcO>gNuRA37AMYH8|f{QFyXgRC@IMFaV!hsDBeSohCW*qfPIY{g=m49
O4*@BI$&3R3RDqwO8Wnp0
literal 0
HcmV?d00001
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 0000000000000000000000000000000000000000..aabfe3f32f19e107e01023e358173f95b8a0d77b
GIT binary patch
literal 6148
zcmeHKO;6iE5S)l1P6*zyAIT<@uGZ?Llntp_(T0A
z{X6u{?goJZ5+@W@Gtumu%+8GMx7MzQh*W!+ekQ6DQ2=GE)G)jtJkL5K74KO9DqAC^
z9%Xb(UFyt4i(?)c;I&(!6svZL-_!Mrv6d+fF;kHFmYbhRZW2WEtFjN>c(4NNO%;++
z|HfDNlicK4f3UUnS5+3OiyxMPrC>R@h<=$-G>8Yoemm}+^S={g@?>hq$!XRdb?ToE
zOfiVFqT90xS=vR&_p_`>&8TgLMcTKyk(&r=L9J6?8;`fYv>JM+xjSj-ar0}lp?6!Y
z$)pynu5a!gcYf#Bg}JlOLJIG)m5&~$a1Y}RT-`>!A~(eqqNkOn6$FI=VL%v|4+i}1
z6fDoj?Z{mS1H!-@F~IwShBAg8ONVyrz+hVdU=?mFu=xX#bF|0MW9bkx5am*VE>-y!
zL%DSLwT}xumJVGyDc^i3e`V!66lJfD`r3w*3LVNQ32-s((tV}ko5yR%B}<|TX%pU(`i{|U3$N(N-TU5xa*ieKl7xB9@wj=oaQkl$znzdu&~
z7fhG`gaOWMlii02y>$kh0cYSB1GYaDs9~A0O>hns@~OI}AsmZyyO3Q)DbNwh4KH
zgcJ&<(2%bfA%!zuWWO?Fn=pkdsoPk))wdo)WrPSgfWCBAIGs{NAUp~g=d9zz%pZ-
RkP*Z`1R@RII0Ju`fuH&NlH&jX
literal 0
HcmV?d00001
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 0000000000000000000000000000000000000000..0f8a2d85caee15963fc1d4a301bfea29c9ad3204
GIT binary patch
literal 6148
zcmeH~QEL-H6ot=dz*<2nBKRWgFX-Z%k86miU{xskT6dd>!K4YvM)7U^MgBgY>UU=b
zN!qk|lOp#nch26~Id|_IvNIV;p}!hFl@28B;fB3^nk{C}%`e%l+;ayEF~?YEn(0KR
zv~I3|D{#{luy6N)YArO>O6%Y6R8uW=$hSP#{PkpZ+Dv8*qiv~pjvt;!<4$@6d#uk~
zLo$u^O(Rx;&8*uq{ko9p;nqwG){_Qrx7QP%T*z{tN@{(LRqW#ag!^o5F4uDBZS!We
zY!`6e=~zEdsO2qFmHJ416EvI>FNp_K(Bh>&5RcHsdTTf}t7J{|g}$0fOC71N_b_It
zoid!kwec@ev(+nl8=~DRm#`LODXDPROy1_1Tsw|DclDiGM_iLD^#9GM_1^=b+l^O%
zJ=<*WUP5nO0aw5k_^W`e4+U;mW$Y5hrGq9O0l*&J(YUui2*xQgRvEj5Ji$Vuf{7aP
z5hElz{Y4&EW$Y3rx(tRI3DzwQr=rHP0se(0{?`Y*|;uYq|oH;INsP+{1}hMv%+q`Dr1+B9?bs;
NL>ats1^%f5e*heilxqM0
literal 0
HcmV?d00001
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"