Renamed packages and added skeleton for HWInterface

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

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)

src/ft24_control/description/blue_cone.xacro

@@ -12,7 +12,7 @@
       <visual>
         <geometry>
           <!-- <cylinder radius="0.112" length="0.3"/>  -->
-          <mesh filename="file://$(find dcaiti_control)/description/assets/blue_cone.dae"/>
+          <mesh filename="file://$(find ft24_control)/description/assets/blue_cone.dae"/>
         </geometry>
       </visual>
       <xacro:solid_cylinder_inertial 

src/ft24_control/description/real_robot_control.xacro

@@ -2,7 +2,7 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
     <ros2_control name="RealRobot" type="system">
         <hardware>
-            <plugin>diffdrive_arduino/DiffDriveArduino</plugin>
+            <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>
@@ -30,7 +30,7 @@
     </ros2_control>
     <gazebo>
         <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
-            <parameters>$(find dcaiti_control)/config/dcaiti_config.yml</parameters>
+            <parameters>$(find ft24_control)/config/dcaiti_config.yml</parameters>
         </plugin>
     </gazebo>
 </robot>

src/ft24_control/description/robot.urdf.xacro

@@ -1,5 +1,5 @@
 <?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro"  name="dcaiti_control">
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro"  name="ft24_control">
 
     <!-- Define parameters-->
     <xacro:arg name="use_ros2_control" default="true"/>

src/ft24_control/description/ros2_control.xacro

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

src/ft24_control/description/wheel.xacro

@@ -129,7 +129,7 @@
       <visual>
         <origin xyz="0 0 0" rpy="${-pi/2} ${(reflect * pi/2) + pi/2} 0"/>
         <geometry>
-          <mesh filename="file://$(find dcaiti_control)/description/assets/ft24_wheel_simplified.dae" />
+          <mesh filename="file://$(find ft24_control)/description/assets/ft24_wheel_simplified.dae" />
         </geometry>
       </visual>
       <xacro:solid_cylinder_inertial 
@@ -293,7 +293,7 @@
       <visual>
         <origin xyz="0 0 0" rpy="${-pi/2} ${(reflect * pi/2) + pi/2} 0"/>
         <geometry>
-          <mesh filename="file://$(find dcaiti_control)/description/assets/ft24_wheel_simplified.dae" />
+          <mesh filename="file://$(find ft24_control)/description/assets/ft24_wheel_simplified.dae" />
         </geometry>
       </visual>
       <xacro:solid_cylinder_inertial 

src/ft24_control/launch/launch_robot.py

@@ -26,7 +26,7 @@ def generate_launch_description():
     # Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
     # !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!
 
-    package_name='dcaiti_control' #<--- CHANGE ME
+    package_name='ft24_control' #<--- CHANGE ME
 
     use_ros2_control = LaunchConfiguration('use_ros2_control')
 
@@ -39,7 +39,7 @@ def generate_launch_description():
     robot_description = Command(['ros2 param get --hide-type /robot_state_publisher robot_description'])
 
     controller_params = os.path.join(
-        get_package_share_directory('dcaiti_control'), # <-- Replace with your package name
+        get_package_share_directory('ft24_control'), # <-- Replace with your package name
         'config',
         'dcaiti_config.yml'
         )

src/ft24_control/launch/launch_sim.py

@@ -22,7 +22,7 @@ def generate_launch_description():
     # Include the robot_state_publisher launch file, provided by our own package. Force sim time to be enabled
     # !!! MAKE SURE YOU SET THE PACKAGE NAME CORRECTLY !!!
 
-    package_name='dcaiti_control' #<--- CHANGE ME
+    package_name='ft24_control' #<--- CHANGE ME
 
     use_ros2_control = LaunchConfiguration('use_ros2_control')
     track = LaunchConfiguration('track')
@@ -96,7 +96,7 @@ def generate_launch_description():
     )
     
     imu_covariance_adder = Node(
-        package='dcaiti_control',
+        package='ft24_control',
         executable='imu_covariance_adder.py',
         name='imu_covariance_adder',
         output='screen',

src/ft24_control/launch/rsp.launch.py

@@ -20,12 +20,12 @@ def generate_launch_description():
     use_ros2_control = LaunchConfiguration('use_ros2_control')
 
     # Process the URDF file
-    pkg_path = os.path.join(get_package_share_directory('dcaiti_control'))
+    pkg_path = os.path.join(get_package_share_directory('ft24_control'))
     xacro_file = os.path.join(pkg_path,'description','robot.urdf.xacro')
     robot_description_config = Command(['xacro ', xacro_file, ' use_ros2_control:=', use_ros2_control, ' use_sim:=', use_sim_time])
     
     twist_mux_params = os.path.join(
-        get_package_share_directory('dcaiti_control'), # <-- Replace with your package name
+        get_package_share_directory('ft24_control'), # <-- Replace with your package name
         'config',
         'twist_mux.yml'
         )

src/ft24_control/package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>dcaiti_control </name>
+  <name>ft24_control </name>
   <version>0.0.0</version>
   <description>TODO: Package description</description>
   <maintainer email="my_email@email.com">MY NAME</maintainer>

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()
+
+
+
+
+
+

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()
+
+

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.

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`.

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" ;
+

src/ros2_control_can/fake_robot_hardware.xml

@@ -0,0 +1,9 @@
+<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>

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 <string>
+
+
+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

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 <hardware_interface/handle.hpp>
+#include <hardware_interface/hardware_info.hpp>
+#include <hardware_interface/system_interface.hpp>
+#include <rclcpp_lifecycle/state.hpp>
+#include <hardware_interface/types/hardware_interface_return_values.hpp>
+
+#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<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  std::vector<hardware_interface::CommandInterface> 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<std::chrono::system_clock> time_;
+  
+};
+
+
+#endif // ROS2_CONTROL_CAN_FAKE_ROBOT_H

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 <cstring>
+#include "rclcpp/rclcpp.hpp"
+
+#include <hardware_interface/handle.hpp>
+#include <hardware_interface/hardware_info.hpp>
+#include <hardware_interface/system_interface.hpp>
+#include <rclcpp_lifecycle/state.hpp>
+#include <hardware_interface/types/hardware_interface_return_values.hpp>
+
+#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<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  std::vector<hardware_interface::CommandInterface> 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<std::chrono::system_clock> time_;
+  
+};
+} // namespace ros2_control_can
+
+#endif // ROS2_CONTROL_CAN_REAL_ROBOT_H

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 <string>
+
+
+
+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

src/ros2_control_can/package.xml

@@ -0,0 +1,29 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>ros2_control_can</name>
+  <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>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>hardware_interface</depend>
+  <depend>controller_manager</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <!-- <depend>rclcpp_lifecyle</depend> -->
+
+
+<test_depend>ament_add_gmock</test_depend>
+<test_depend>hardware_interface</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+
+  
+</package>

src/ros2_control_can/robot_hardware.xml

@@ -0,0 +1,9 @@
+<library path="ros2_control_can">
+  <class name="ros2_control_can/Ros2ControlCAN"
+        type="ros2_control_can::Ros2ControlCAN"
+        base_class_type="hardware_interface::SystemInterface">
+    <description>
+      Hardware Interface for Differential Drive Arduino controller.
+    </description>
+  </class>
+</library>

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 <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
+)

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 <math.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)
+#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<hardware_interface::StateInterface> Ros2ControlCAN::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> Ros2ControlCAN::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;
+}
+
+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<double> 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
+)

src/ros2_control_can/src/wheel.cpp

@@ -0,0 +1,15 @@
+#include "ros2_control_can/wheel.h"
+
+#include <cmath>
+
+
+Wheel::Wheel(const std::string &wheel_name)
+{
+  setup(wheel_name);
+}
+
+
+void Wheel::setup(const std::string &wheel_name)
+{
+  name = wheel_name;
+}

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"