Added working CAN and first draft of sim with chrono

Dockerfile

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

README.md

@@ -1,25 +1,29 @@
-# (At least) teach the robot to drive remotely
+# Temporary FT24 control repo
 
-## Start Script
-The whole system (modules for robot control, remote control, lidar drivers and ROS interface, as well as SLAM & navigation) can be launched by executing `start.bash`, which is located at the root of the project directory.
+## Initial setup
+To install all necessary dependencies, run:
+```
+rosdep install --from-paths src -y --ignore-src
+```
 
-`start.bash` creates a tmux session with a separate window for each module, plus an additional window running `htop` for monitoring performance. In each window, the appropriate launch file for the respective module is executed.
+Then build this package via the normal ROS2 build process:
+```
+colcon build --symlink-install
+source install/setup.bash 
+```
 
-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.
+## Simulation Start
+For the Gazebo simulation, run:
+```
+ros2 launch ft24_control launch_sim.py
+```
+
+You can control the car with the keyboard by publishing Twist commands by opening a separate terminal and running:
+```
+ros2 run teleop_twist_keyboard teleop_twist_keyboard --ros-args -r /cmd_vel:=/ackermann_steering_controller/reference_unstamped
+```
 
 ## Robot Control
-The robot control is implemented in the `dcaiti_control` package and can be launched with \
-`ros2 launch dcaiti_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
-The launch file `ros2 launch src/dcaitirobot/launch/remote_control_launch.py` launches the nodes required to interface the PS3 controller and publish its commands as `Twist` messages in ROS.
-
-## Lidar
-`ros2 launch velodyne velodyne-all-nodes-VLP16-launch.py` is provided by the `velodyne` package and brings up the velodyne driver as well as nodes required to publish ROS2 messages of type `PointCloud2` and `LaserScan`.
-
-**Note**\
-In order to bring the lidar to the same subnet as the Jetson within the provided router, we modified the lidar's IP from `192.168.4.200` to `192.168.1.200`.
-
-## SLAM and Nav2
-`ros2 launch src/dcaiti_navigation/launch/start_nav2.py` will start `slam_toolbox` (plus several nodes/transforms required for `slam_toolbox` to work with our lidar output), multiple `Nav2` packages as well `explore_lite` which provides autonomous exploration support for `Nav2`.
+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.

docker-compose.yml

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

src/dcaiti_control/CMakeLists.txt

@@ -1,41 +0,0 @@
-cmake_minimum_required(VERSION 3.5)
-project(dcaiti_control)
-
-# 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 -Wpedantic)
-endif()
-
-# find dependencies
-find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
-
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  # the following line skips the linter which checks for copyrights
-  # uncomment the line when a copyright and license is not present in all source files
-  #set(ament_cmake_copyright_FOUND TRUE)
-  # the following line skips cpplint (only works in a git repo)
-  # uncomment the line when this package is not in a git repo
-  #set(ament_cmake_cpplint_FOUND TRUE)
-  ament_lint_auto_find_test_dependencies()
-endif()
-
-install(
-  DIRECTORY config description launch worlds
-  DESTINATION share/${PROJECT_NAME}
-)
-
-ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.dsv.in")
-ament_package()

src/dcaiti_control/description/imu.xacro

@@ -1,12 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
-    <gazebo reference="laser_frame">
-        <material>Gazebo/Red</material>
-        <sensor name="imu_sensor" type="imu">
-          <always_on>1</always_on>
-          <update_rate>100</update_rate>
-          <visualize>true</visualize>
-          <topic>/lidar/imu</topic>
-      </sensor>
-    </gazebo>  
-</robot>

src/dcaiti_control/description/real_robot_control.xacro

@@ -1,36 +0,0 @@
-<?xml version="1.0"?>
-<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
-    <ros2_control name="RealRobot" type="system">
-        <hardware>
-            <plugin>diffdrive_arduino/DiffDriveArduino</plugin>
-            <param name="left_wheel_name">left_wheel_joint</param>
-            <param name="right_wheel_name">right_wheel_joint</param>
-            <param name="loop_rate">30</param>
-            <param name="device">/dev/ttyUSB0</param>
-            <param name="baud_rate">57600</param>
-            <param name="timeout">1000</param>
-        </hardware>
-        <!-- Note everything below here is the same as the Gazebo one -->
-        <joint name="left_wheel_joint">
-            <command_interface name="velocity">
-                <param name="min">-0.5</param>
-                <param name="max">0.5</param>
-            </command_interface>
-            <state_interface name="velocity"/>
-            <state_interface name="position"/>
-        </joint>
-        <joint name="right_wheel_joint">
-            <command_interface name="velocity">
-                <param name="min">-0.5</param>
-                <param name="max">0.5</param>
-            </command_interface>
-            <state_interface name="velocity"/>
-            <state_interface name="position"/>
-        </joint>
-    </ros2_control>
-    <gazebo>
-        <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
-            <parameters>$(find dcaiti_control)/config/dcaiti_config.yml</parameters>
-        </plugin>
-    </gazebo>
-</robot>

src/dcaiti_control/package.xml

@@ -1,18 +0,0 @@
-<?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>
-  <version>0.0.0</version>
-  <description>TODO: Package description</description>
-  <maintainer email="my_email@email.com">MY NAME</maintainer>
-  <license>TODO: License declaration</license>
-
-  <buildtool_depend>ament_cmake</buildtool_depend>
-
-  <test_depend>ament_lint_auto</test_depend>
-  <test_depend>ament_lint_common</test_depend>
-
-  <export>
-    <build_type>ament_cmake</build_type>
-  </export>
-</package>

src/ft24_control/CMakeLists.txt

@@ -0,0 +1,85 @@
+cmake_minimum_required(VERSION 3.5)
+project(ft24_control)
+
+# 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 -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(ignition-cmake2 REQUIRED)
+find_package(ignition-msgs5 REQUIRED)
+find_package(ignition-transport8 REQUIRED)
+find_package(ignition-math6 REQUIRED)
+find_package(ignition-common3 REQUIRED)
+
+
+find_package(ignition-plugin1 REQUIRED COMPONENTS register)
+set(IGN_PLUGIN_VER ${ignition-plugin1_VERSION_MAJOR})
+
+find_package(ignition-gazebo6 REQUIRED)
+# Add sources for each plugin to be registered.
+add_library(SetLinkStatePlugin src/SetLinkStatePlugin.cpp)
+
+target_include_directories(
+  SetLinkStatePlugin
+  PRIVATE
+  include
+)
+
+set_property(TARGET SetLinkStatePlugin PROPERTY CXX_STANDARD 17)
+target_link_libraries(SetLinkStatePlugin
+  PRIVATE ignition-plugin${IGN_PLUGIN_VER}::ignition-plugin${IGN_PLUGIN_VER}
+  PRIVATE ignition-gazebo6::ignition-gazebo6)
+
+add_executable(SetPos src/external_pose_set.cpp)
+
+target_include_directories(
+  SetPos
+  PRIVATE
+  include
+)
+
+target_link_libraries(SetPos ${IGNITION-MSGS_LIBRARIES} ${IGNITION-TRANSPORT_LIBRARIES} ${IGNITION-COMMON_LIBRARIES} ${IGNITION-MATH_LIBRARIES})
+
+install(TARGETS SetPos DESTINATION lib/${PROJECT_NAME})
+
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  # the following line skips the linter which checks for copyrights
+  # uncomment the line when a copyright and license is not present in all source files
+  #set(ament_cmake_copyright_FOUND TRUE)
+  # the following line skips cpplint (only works in a git repo)
+  # uncomment the line when this package is not in a git repo
+  #set(ament_cmake_cpplint_FOUND TRUE)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(
+  DIRECTORY config description launch worlds ft24_control
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_python_install_package(${PROJECT_NAME})
+ 
+# Install Python executables
+install(PROGRAMS
+  ft24_control/imu_covariance_adder.py
+  ft24_control/joy.py
+  ft24_control/twist.py
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+ament_environment_hooks("${CMAKE_CURRENT_SOURCE_DIR}/env-hooks/${PROJECT_NAME}.dsv.in")
+ament_package()

src/ft24_control/config/ft24_config.yml

@@ -15,12 +15,13 @@ ackermann_steering_controller:
     base_frame_id: base_link
     publish_limited_velocity: true
     in_chained_mode: false
+    enable_odom_tf: false
 
     reference_timeout: 2.0
     front_steering: true
     open_loop: false
     velocity_rolling_window_size: 10
-    position_feedback: true
+    position_feedback: false
     use_stamped_vel: false
     rear_wheels_names: [rear_right_wheel_joint, rear_left_wheel_joint]
     front_wheels_names: [front_right_steer_joint, front_left_steer_joint]

src/ft24_control/config/ukf.yml

@@ -0,0 +1,202 @@
+### ukf config file ###
+ukf_filter_node:
+    ros__parameters:
+        # The frequency, in Hz, at which the filter will output a position estimate. Note that the filter will not begin
+        # computation until it receives at least one message from one of the inputs. It will then run continuously at the
+        # frequency specified here, regardless of whether it receives more measurements. Defaults to 30 if unspecified.
+        frequency: 30.0
+
+        # The period, in seconds, after which we consider a sensor to have timed out. In this event, we carry out a predict
+        # cycle on the EKF without correcting it. This parameter can be thought of as the minimum frequency with which the
+        # filter will generate new output. Defaults to 1 / frequency if not specified.
+        sensor_timeout: 0.1
+
+        # ekf_localization_node and ukf_localization_node both use a 3D omnidirectional motion model. If this parameter is
+        # set to true, no 3D information will be used in your state estimate. Use this if you are operating in a planar
+        # environment and want to ignore the effect of small variations in the ground plane that might otherwise be detected
+        # by, for example, an IMU. Defaults to false if unspecified.
+        two_d_mode: true
+
+        # Use this parameter to provide an offset to the transform generated by ekf_localization_node. This can be used for
+        # future dating the transform, which is required for interaction with some other packages. Defaults to 0.0 if
+        # unspecified.
+        transform_time_offset: 0.0
+
+        # Use this parameter to provide specify how long the tf listener should wait for a transform to become available. 
+        # Defaults to 0.0 if unspecified.
+        transform_timeout: 0.0
+
+        # If you're having trouble, try setting this to true, and then echo the /diagnostics_agg topic to see if the node is
+        # unhappy with any settings or data.
+        print_diagnostics: true
+
+        # Debug settings. Not for the faint of heart. Outputs a ludicrous amount of information to the file specified by
+        # debug_out_file. I hope you like matrices! Please note that setting this to true will have strongly deleterious
+        # effects on the performance of the node. Defaults to false if unspecified.
+        debug: false
+
+        # Whether we'll allow old measurements to cause a re-publication of the updated state
+        permit_corrected_publication: false
+
+        # Whether to publish the acceleration state. Defaults to false if unspecified.
+        publish_acceleration: false
+
+        # Whether to broadcast the transformation over the /tf topic. Defaults to true if unspecified.
+        publish_tf: true
+
+        # REP-105 (http://www.ros.org/reps/rep-0105.html) specifies four principal coordinate frames: base_link, odom, map, and
+        # earth. base_link is the coordinate frame that is affixed to the robot. Both odom and map are world-fixed frames.
+        # The robot's position in the odom frame will drift over time, but is accurate in the short term and should be
+        # continuous. The odom frame is therefore the best frame for executing local motion plans. The map frame, like the odom
+        # frame, is a world-fixed coordinate frame, and while it contains the most globally accurate position estimate for your
+        # robot, it is subject to discrete jumps, e.g., due to the fusion of GPS data or a correction from a map-based
+        # localization node. The earth frame is used to relate multiple map frames by giving them a common reference frame.
+        # ekf_localization_node and ukf_localization_node are not concerned with the earth frame.
+        # Here is how to use the following settings:
+        # 1. Set the map_frame, odom_frame, and base_link frames to the appropriate frame names for your system.
+        #     1a. If your system does not have a map_frame, just remove it, and make sure "world_frame" is set to the value of
+        #         odom_frame.
+        # 2. If you are fusing continuous position data such as wheel encoder odometry, visual odometry, or IMU data, set
+        #   "world_frame" to your odom_frame value. This is the default behavior for robot_localization's state estimation nodes.
+        # 3. If you are fusing global absolute position data that is subject to discrete jumps (e.g., GPS or position updates
+        # from landmark observations) then:
+        #     3a. Set your "world_frame" to your map_frame value
+        #     3b. MAKE SURE something else is generating the odom->base_link transform. Note that this can even be another state
+        #         estimation node from robot_localization! However, that instance should *not* fuse the global data.
+        odom_frame: odom            # Defaults to "odom" if unspecified
+        base_link_frame: base_link  # Defaults to "base_link" if unspecified
+        world_frame: odom           # Defaults to the value of odom_frame if unspecified
+
+        # The filter accepts an arbitrary number of inputs from each input message type (nav_msgs/Odometry,
+        # geometry_msgs/PoseWithCovarianceStamped, geometry_msgs/TwistWithCovarianceStamped,
+        # sensor_msgs/Imu). To add an input, simply append the next number in the sequence to its "base" name, e.g., odom0,
+        # odom1, twist0, twist1, imu0, imu1, imu2, etc. The value should be the topic name. These parameters obviously have no
+        # default values, and must be specified.
+        odom0: /ackermann_steering_controller/odometry
+
+        # Each sensor reading updates some or all of the filter's state. These options give you greater control over which
+        # values from each measurement are fed to the filter. For example, if you have an odometry message as input, but only
+        # want to use its Z position value, then set the entire vector to false, except for the third entry. The order of the
+        # values is x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Note that not some message types
+        # do not provide some of the state variables estimated by the filter. For example, a TwistWithCovarianceStamped message
+        # has no pose information, so the first six values would be meaningless in that case. Each vector defaults to all false
+        # if unspecified, effectively making this parameter required for each sensor.
+        odom0_config: [false,  false,  false,
+                       false, false, false,
+                       true, false, false,
+                       false, false, true,
+                       false, false, false]
+
+        # If you have high-frequency data or are running with a low frequency parameter value, then you may want to increase
+        # the size of the subscription queue so that more measurements are fused.
+        odom0_queue_size: 2
+
+        # [ADVANCED] When measuring one pose variable with two sensors, a situation can arise in which both sensors under-
+        # report their covariances. This can lead to the filter rapidly jumping back and forth between each measurement as they
+        # arrive. In these cases, it often makes sense to (a) correct the measurement covariances, or (b) if velocity is also
+        # measured by one of the sensors, let one sensor measure pose, and the other velocity. However, doing (a) or (b) isn't
+        # always feasible, and so we expose the differential parameter. When differential mode is enabled, all absolute pose
+        # data is converted to velocity data by differentiating the absolute pose measurements. These velocities are then
+        # integrated as usual. NOTE: this only applies to sensors that provide pose measurements; setting differential to true
+        # for twist measurements has no effect.
+        odom0_differential: false
+
+        # [ADVANCED] When the node starts, if this parameter is true, then the first measurement is treated as a "zero point"
+        # for all future measurements. While you can achieve the same effect with the differential paremeter, the key
+        # difference is that the relative parameter doesn't cause the measurement to be converted to a velocity before
+        # integrating it. If you simply want your measurements to start at 0 for a given sensor, set this to true.
+        odom0_relative: false
+
+        # [ADVANCED] If your data is subject to outliers, use these threshold settings, expressed as Mahalanobis distances, to
+        # control how far away from the current vehicle state a sensor measurement is permitted to be. Each defaults to
+        # numeric_limits<double>::max() if unspecified. It is strongly recommended that these parameters be removed if not
+        # required. Data is specified at the level of pose and twist variables, rather than for each variable in isolation.
+        # For messages that have both pose and twist data, the parameter specifies to which part of the message we are applying
+        # the thresholds.
+        odom0_pose_rejection_threshold: 5.0
+        odom0_twist_rejection_threshold: 1.0
+
+        imu0: lidar/imu_covariance
+        imu0_config: [false, false, false,
+                      true,  true,  true,
+                      false, false, false,
+                      true,  true,  true,
+                      true,  true,  true]
+        imu0_differential: false
+        imu0_relative: true
+        imu0_queue_size: 5
+        imu0_pose_rejection_threshold: 0.8                 # Note the difference in parameter names
+        imu0_twist_rejection_threshold: 0.8                #
+        imu0_linear_acceleration_rejection_threshold: 0.8  #
+
+        # [ADVANCED] Some IMUs automatically remove acceleration due to gravity, and others don't. If yours doesn't, please set
+        # this to true, and *make sure* your data conforms to REP-103, specifically, that the data is in ENU frame.
+        imu0_remove_gravitational_acceleration: true
+
+        # [ADVANCED]  The EKF and UKF models follow a standard predict/correct cycle. During prediction, if there is no
+        # acceleration reference, the velocity at time t+1 is simply predicted to be the same as the velocity at time t. During
+        # correction, this predicted value is fused with the measured value to produce the new velocity estimate. This can be
+        # problematic, as the final velocity will effectively be a weighted average of the old velocity and the new one. When
+        # this velocity is the integrated into a new pose, the result can be sluggish covergence. This effect is especially
+        # noticeable with LIDAR data during rotations. To get around it, users can try inflating the process_noise_covariance
+        # for the velocity variable in question, or decrease the  variance of the variable in question in the measurement
+        # itself. In addition, users can also take advantage of the control command being issued to the robot at the time we
+        # make the prediction. If control is used, it will get converted into an acceleration term, which will be used during
+        # predicition. Note that if an acceleration measurement for the variable in question is available from one of the
+        # inputs, the control term will be ignored.
+        # Whether or not we use the control input during predicition. Defaults to false.
+        use_control: false
+
+        # Whether the input (assumed to be cmd_vel) is a geometry_msgs/Twist or geometry_msgs/TwistStamped message. Defaults to
+        # false.
+        stamped_control: false
+
+        # The last issued control command will be used in prediction for this period. Defaults to 0.2.
+        control_timeout: 0.2
+
+        # Which velocities are being controlled. Order is vx, vy, vz, vroll, vpitch, vyaw.
+        control_config: [true, false, false, false, false, true]
+
+        # Places limits on how large the acceleration term will be. Should match your robot's kinematics.
+        acceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 3.4]
+
+        # Acceleration and deceleration limits are not always the same for robots.
+        deceleration_limits: [1.3, 0.0, 0.0, 0.0, 0.0, 4.5]
+
+        # If your robot cannot instantaneously reach its acceleration limit, the permitted change can be controlled with these
+        # gains
+        acceleration_gains: [0.8, 0.0, 0.0, 0.0, 0.0, 0.9]
+
+        # If your robot cannot instantaneously reach its deceleration limit, the permitted change can be controlled with these
+        # gains
+        deceleration_gains: [1.0, 0.0, 0.0, 0.0, 0.0, 1.0]
+
+        # [ADVANCED] The process noise covariance matrix can be difficult to tune, and can vary for each application, so it is
+        # exposed as a configuration parameter. This matrix represents the noise we add to the total error after each
+        # prediction step. The better the omnidirectional motion model matches your system, the smaller these values can be.
+        # However, if users find that a given variable is slow to converge, one approach is to increase the
+        # process_noise_covariance diagonal value for the variable in question, which will cause the filter's predicted error
+        # to be larger, which will cause the filter to trust the incoming measurement more during correction. The values are
+        # ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the matrix below if
+        # unspecified.
+        # Note: the specification of covariance matrices can be cumbersome, so all matrix parameters in this package support
+        # both full specification or specification of only the diagonal values.
+
+
+        # [ADVANCED] This represents the initial value for the state estimate error covariance matrix. Setting a diagonal
+        # value (variance) to a large value will result in rapid convergence for initial measurements of the variable in
+        # question. Users should take care not to use large values for variables that will not be measured directly. The values
+        # are ordered as x, y, z, roll, pitch, yaw, vx, vy, vz, vroll, vpitch, vyaw, ax, ay, az. Defaults to the diagonal values below
+        # if unspecified. In this example, we specify only the diagonal of the matrix.
+
+        # [ADVANCED, UKF ONLY] The alpha and kappa variables control the spread of the sigma points. Unless you are familiar
+        # with UKFs, it's probably a good idea to leave these alone.
+        # Defaults to 0.001 if unspecified.
+        alpha: 0.001
+        # Defaults to 0 if unspecified.
+        kappa: 0.0
+
+        # [ADVANCED, UKF ONLY] The beta variable relates to the distribution of the state vector. Again, it's probably best to
+        # leave this alone if you're uncertain. Defaults to 2 if unspecified.
+        beta: 2.0
+        use_sime_time: true

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/imu.xacro

@@ -0,0 +1,59 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
+    <gazebo reference="laser_frame">
+        <material>Gazebo/Red</material>
+        <sensor name="imu_sensor" type="imu">
+          <ignition_frame_id>laser_frame</ignition_frame_id>
+          <always_on>1</always_on>
+          <update_rate>50</update_rate>
+          <visualize>false</visualize>
+          <topic>/lidar/imu</topic>
+          <angular_velocity>
+            <x>
+              <noise>
+                <type>gaussian</type>
+                <mean>0.0</mean>
+                <stddev>0.01</stddev>
+              </noise>
+            </x> 
+            <y>
+              <noise>
+                <type>gaussian</type>
+                <mean>0.0</mean>
+                <stddev>0.01</stddev>
+              </noise>
+            </y>
+            <z>
+              <noise>
+                <type>gaussian</type>
+                <mean>0.0</mean>
+                <stddev>0.01</stddev>
+              </noise>
+            </z> 
+          </angular_velocity>
+          <linear_acceleration>
+            <x>
+              <noise>
+                <type>gaussian</type>
+                <mean>0.0</mean>
+                <stddev>0.01</stddev>
+              </noise>
+            </x> 
+            <y>
+              <noise>
+                <type>gaussian</type>
+                <mean>0.0</mean>
+                <stddev>0.01</stddev>
+              </noise>
+            </y>
+            <z>
+              <noise>
+                <type>gaussian</type>
+                <mean>0.0</mean>
+                <stddev>0.01</stddev>
+              </noise>
+            </z>
+          </linear_acceleration>
+      </sensor>
+    </gazebo>  
+</robot>

src/ft24_control/description/real_robot_control.xacro

@@ -0,0 +1,36 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
+    <ros2_control name="RealRobot" type="system">
+        <hardware>
+            <plugin>ros2_control_can/Ros2ControlCAN</plugin>
+            <param name="front_left_wheel_name">front_left_steer_joint</param>
+            <param name="front_right_wheel_name">front_right_steer_joint</param>
+            <param name="rear_left_wheel_name">rear_left_wheel_joint</param>
+            <param name="rear_right_wheel_name">rear_right_wheel_joint</param>
+            <param name="device">vcan0</param>
+            <param name="loop_rate">50</param>
+        </hardware>
+        <!-- Note everything below here is the same as the Gazebo one -->
+        <joint name="rear_right_wheel_joint">
+            <command_interface name="velocity"/>
+            <state_interface name="velocity"/>
+        </joint>
+        <joint name="rear_left_wheel_joint">
+            <command_interface name="velocity"/>
+            <state_interface name="velocity"/>
+        </joint>
+        <joint name="front_left_steer_joint">
+            <command_interface name="position"/>
+            <state_interface name="position"/>
+        </joint>
+        <joint name="front_right_steer_joint">
+            <command_interface name="position"/>
+            <state_interface name="position"/>
+        </joint>
+    </ros2_control>
+    <gazebo>
+        <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system">
+            <parameters>$(find ft24_control)/config/ft24_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

@@ -2,33 +2,29 @@
 <robot xmlns:xacro="http://www.ros.org/wiki/xacro" >
     <ros2_control name="GazeboSimSystem" type="system">
         <hardware>
-            <plugin>ign_ros2_control/IgnitionSystem</plugin>
+            <plugin>gz_ros2_control/GazeboSimSystem</plugin>
         </hardware>
         <joint name="rear_right_wheel_joint">
             <command_interface name="velocity"/>
-            <state_interface name="position"/>
             <state_interface name="velocity"/>
         </joint>
         <joint name="rear_left_wheel_joint">
             <command_interface name="velocity"/>
-            <state_interface name="position"/>
             <state_interface name="velocity"/>
         </joint>
         <joint name="front_left_steer_joint">
             <command_interface name="position"/>
             <state_interface name="position"/>
-            <state_interface name="velocity"/>
         </joint>
         <joint name="front_right_steer_joint">
             <command_interface name="position"/>
             <state_interface name="position"/>
-            <state_interface name="velocity"/>
         </joint>
     </ros2_control>
     <gazebo>
-    <plugin filename="libign_ros2_control-system.so" name="ign_ros2_control::IgnitionROS2ControlPlugin">
-        <parameters>$(find dcaiti_control)/config/dcaiti_config.yml</parameters>
-        <parameters>$(find dcaiti_control)/config/gaz_ros2_ctl_sim.yml</parameters>
-    </plugin>
-</gazebo>
+        <plugin name="gz_ros2_control::GazeboSimROS2ControlPlugin" filename="libgz_ros2_control-system">
+            <parameters>$(find ft24_control)/config/ft24_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 
@@ -322,6 +322,14 @@
 
     <gazebo reference="${name}_wheel_link">
       <material>Gazebo/Red</material>
+      <collision>
+        <friction>      
+          <ode>      
+            <mu>0</mu>
+            <mu2>0</mu2>
+          </ode>      
+        </friction>      
+      </collision>
     </gazebo>
   </xacro:macro>
 </robot>

src/ft24_control/ft24_control/external_pos_setting.py

@@ -0,0 +1,84 @@
+#!/usr/bin/env python3
+
+import math
+import time
+
+# https://github.com/srmainwaring/python-ignition
+from ignition.msgs.header_pb2 import Header
+from ignition.msgs.pose_pb2 import Pose
+from ignition.msgs.quaternion_pb2 import Quaternion
+from ignition.msgs.vector3d_pb2 import Vector3d
+from ignition.transport import Node
+
+# https://github.com/srmainwaring/python-ignition
+import ignition.math
+
+# Settings
+world_name = "empty"
+model_name = "iris"
+
+timeout_ms = 10
+update_rate_hz = 30.0
+
+# offsets - The ArduPilot z position may be zeroed to the ground plane,
+#           Gazebo will set pose for the base link geometric centre
+pose_offset_x = 0.0
+pose_offset_y = 0.0
+pose_offset_z = 0.17
+
+
+def main():
+
+    connection_string = "udp:127.0.0.1:14550"
+
+    # Connect to the Vehicle.
+    print("Connecting to vehicle on: {}".format(connection_string))
+
+    try:
+        # create a transport node
+        node = Node()
+
+        # set service details
+        service = "/world/{}/set_pose".format(world_name)
+        reqtype = "ignition.msgs.Pose" 
+        reptype = "ignition.msgs.Boolean" 
+
+        # configure update loop
+        now_s = time.time()
+        start_time_s = now_s
+
+        # update_rate_hz = 1.0
+        update_period_s = 1.0 / update_rate_hz
+        last_update_time_s = now_s
+
+        sim_time_s = now_s - start_time_s
+
+        while True:
+            now_s = time.time()
+            time_s = now_s - start_time_s
+            if now_s - last_update_time_s >= update_period_s:
+                last_update_time_s = now_s
+
+                # check we have valid data
+
+                # create request message
+                vector3d_msg = Vector3d()
+                vector3d_msg.x = 0.01 + pose_offset_x
+                vector3d_msg.y = 0.01+ pose_offset_y
+                vector3d_msg.z =  pose_offset_z
+
+                quat_msg = Quaternion()
+
+                pose_msg = Pose()
+                pose_msg.name = model_name
+                pose_msg.position.CopyFrom(vector3d_msg)
+                pose_msg.orientation.CopyFrom(quat_msg)
+
+                # submit request (blocking)
+                result = node.request(service, pose_msg, timeout_ms, reptype)
+
+                if not result:
+                    print("[{:.1f}] update failed".format(time_s))
+
+    except KeyboardInterrupt:
+        pass

src/ft24_control/ft24_control/imu_covariance_adder.py

@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import Imu
+
+class ImuCovarianceNode(Node):
+    def __init__(self):
+        super().__init__('imu_covariance_node')
+        
+        self.declare_parameter('publish_topic', '/lidar/imu_covariance').value
+        self.declare_parameter('subscribe_topic', '/lidar/imu').value
+        self.declare_parameter('linear_acceleration_covariance', [0.0] * 9)
+        self.declare_parameter('angular_velocity_covariance', [0.0] * 9)
+        self.declare_parameter('orientation_covariance', [0.0] * 9)
+
+        self.orientation_covariance = self.get_parameter('orientation_covariance').value
+        self.linear_acceleration_covariance = self.get_parameter('linear_acceleration_covariance').value
+        self.angular_velocity_covariance = self.get_parameter('angular_velocity_covariance').value
+        self.publish_topic = self.get_parameter('publish_topic').value
+        self.subscribe_topic = self.get_parameter('subscribe_topic').value
+
+        self.publisher = self.create_publisher(Imu, self.publish_topic, 10)
+
+
+        self.subscription = self.create_subscription(
+            Imu,
+            self.subscribe_topic,
+            self.imu_callback,
+            10)
+
+    def imu_callback(self, msg):
+        # Update covariances in-place
+        msg.orientation_covariance = self.orientation_covariance
+        msg.linear_acceleration_covariance = self.linear_acceleration_covariance
+        msg.angular_velocity_covariance = self.angular_velocity_covariance
+
+        self.publisher.publish(msg)
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    imu_covariance_node = ImuCovarianceNode()
+
+    rclpy.spin(imu_covariance_node)
+
+    imu_covariance_node.destroy_node()
+    rclpy.shutdown()
+
+if __name__ == '__main__':
+    main()

src/ft24_control/ft24_control/joy.py

@@ -0,0 +1,276 @@
+from math import atan2, degrees, sqrt
+from struct import Struct
+from time import sleep
+from typing import List
+
+import rclpy
+from rclpy.context import Context
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from sensor_msgs.msg import Joy
+
+
+class JSEvent(object):
+    """A joystick even class (struct)
+
+        struct js_event_t
+        {
+            uint32_t time;
+            int16_t value;
+            uint8_t type;
+            uint8_t id;
+        };
+
+    Takes in raw bytes from a file.read(JS_EVENT_SIZE) call.
+
+    Also defines AXIS and BUTTON ids
+    """
+
+    JS_EVENT_SIZE = 8  # 64 bits is 8 bytes
+    EVENT_BUTTON = 0x01
+    EVENT_AXIS = 0x02
+    EVENT_INIT = 0x80
+    MAX_AXIS_COUNT = 27
+    MAX_BUTTON_COUNT = 18
+    AXIS_COUNT = 26
+    BUTTON_COUNT = 17
+
+    AXIS_LEFT_STICK_HORIZONTAL = 0
+    AXIS_LEFT_STICK_VERTICAL = 1
+    AXIS_RIGHT_STICK_HORIZONTAL = 2
+    AXIS_RIGHT_STICK_VERTICAL = 3
+    AXIS_DPAD_UP = 8
+    AXIS_DPAD_RIGHT = 9
+    AXIS_DPAD_DOWN = 10
+    AXIS_DPAD_LEFT = 11  # who knows what the left value should actually be...
+    AXIS_LEFT_TRIGGER = 12
+    AXIS_RIGHT_TRIGGER = 13
+    AXIS_LEFT_BUMPER = 14
+    AXIS_RIGHT_BUMPER = 15
+    AXIS_TRIANGLE = 16
+    AXIS_CIRCLE = 17
+    AXIS_X = 18
+    AXIS_SQUARE = 19
+    AXIS_ACCEL_X = 23  # note: left is positive, right is negative
+    AXIS_ACCEL_Y = 24  # note: back is positive, forward is negative
+    AXIS_ACCEL_Z = 25  # note: can't tell what sign is what
+
+    BUTTON_SELECT = 0
+    BUTTON_LEFT_JOYSTICK = 1
+    BUTTON_RIGHT_JOYSTICK = 2
+    BUTTON_START = 3
+    BUTTON_DPAD_UP = 4
+    BUTTON_DPAD_RIGHT = 5
+    BUTTON_DPAD_DOWN = 6
+    BUTTON_DPAD_LEFT = 7
+    BUTTON_LEFT_TRIGGER = 8
+    BUTTON_RIGHT_TRIGGER = 9
+    BUTTON_LEFT_BUMPER = 10
+    BUTTON_RIGHT_BUMPER = 11
+    BUTTON_TRIANGLE = 12
+    BUTTON_CIRCLE = 13
+    BUTTON_X = 14
+    BUTTON_SQUARE = 15
+    BUTTON_PS3 = 16
+
+    def __init__(self, event_struct):
+        # c.f. https://docs.python.org/3/library/struct.html#format-characters
+        s = Struct("< I h B B")
+        self.time, self.value, self.type, self.id = s.unpack(event_struct)
+        # also c.f. Struct.pack()
+
+        # ignore non-input events
+        self.type = self.type & ~self.EVENT_INIT
+
+    def __repr__(self):
+        struct = "struct js_event_t\n{"
+        # struct += '\n\tuint32_t time : {}'.format(self.time)
+        struct += "\n\tint16_t value : {}".format(self.value)
+        struct += "\n\tuint8_t type  : {}".format(hex(self.type))
+        struct += "\n\tuint8_t id    : {}".format(self.id)
+        struct += "\n};\n"
+        return struct
+
+
+class Joystick(object):
+    def __init__(self, js_file="/dev/input/js1"):
+        # note names beginning with `__` are enforced as private
+        self.file = open(js_file, "rb")
+        # since we're using ID's as indices, account for 0 based indices
+        self.__axis_values = [0] * (JSEvent.MAX_AXIS_COUNT + 1)
+        # since we're using ID's as indices, account for 0 based indices
+        self.__button_values = [0] * (JSEvent.MAX_BUTTON_COUNT + 1)
+
+        self.update_id = -1
+        self.AxisUpdate = False
+        self.ButtonUpdate = False
+        self.event = None
+
+    def __del__(self):
+        self.file.close()
+
+    def __enter__(self):
+        return self
+
+    # __enter__ and __exit__ allow the context manager syntax
+    # with Joystick() as js:
+    # ...
+    def __exit__(self, exc_type, exc_value, traceback):
+        self.file.close()
+
+    def Update(self):
+        self.event = JSEvent(self.file.read(JSEvent.JS_EVENT_SIZE))
+
+        if self.event.type == JSEvent.EVENT_AXIS:
+            self.update_id = self.event.id
+            self.AxisUpdate = True
+            self.ButtonUpdate = False
+            self.__axis_values[self.event.id] = self.event.value
+
+        if self.event.type == JSEvent.EVENT_BUTTON:
+            self.update_id = self.event.id
+            self.AxisUpdate = False
+            self.ButtonUpdate = True
+            self.__button_values[self.event.id] = self.event.value
+
+    def getButtonState(self, button_id):
+        return self.__button_values[button_id]
+
+    def getAxisState(self, axis_id):
+        return self.__axis_values[axis_id]
+
+
+class JoystickController(Joystick):
+    """An implementation specific controller that inherits a basic Joystick. Adds the capability to
+    check for specific updates that you care about and then do something based on that.
+    """
+
+    def __init__(self, js_file="/dev/input/js0"):
+        Joystick.__init__(self, js_file)
+        # direction gives the direction of travel. Defined in terms of r and theta
+        self.direction_vector = (0, 0)
+        # rotation tells the robot to rotate x degrees
+        self.rotation_vector = (0, 0)
+
+    def hasUpdate(self):
+        return self.ButtonUpdate or self.AxisUpdate
+
+    def performUpdates(self):
+        care_about_buttons = [
+            JSEvent.BUTTON_SELECT,
+            JSEvent.BUTTON_LEFT_JOYSTICK,
+            JSEvent.BUTTON_RIGHT_JOYSTICK,
+            JSEvent.BUTTON_START,
+            JSEvent.BUTTON_DPAD_UP,
+            JSEvent.BUTTON_DPAD_RIGHT,
+            JSEvent.BUTTON_DPAD_DOWN,
+            JSEvent.BUTTON_DPAD_LEFT,
+            JSEvent.BUTTON_LEFT_TRIGGER,
+            JSEvent.BUTTON_RIGHT_TRIGGER,
+            JSEvent.BUTTON_LEFT_BUMPER,
+            JSEvent.BUTTON_RIGHT_BUMPER,
+            JSEvent.BUTTON_TRIANGLE,
+            JSEvent.BUTTON_CIRCLE,
+            JSEvent.BUTTON_X,
+            JSEvent.BUTTON_SQUARE,
+            JSEvent.BUTTON_PS3,
+        ]
+        care_about_axes = [
+            JSEvent.AXIS_LEFT_STICK_HORIZONTAL,
+            JSEvent.AXIS_LEFT_STICK_VERTICAL,
+            JSEvent.AXIS_RIGHT_STICK_HORIZONTAL,
+            JSEvent.AXIS_RIGHT_STICK_VERTICAL,
+            JSEvent.AXIS_DPAD_UP,
+            JSEvent.AXIS_DPAD_LEFT,
+            JSEvent.AXIS_DPAD_RIGHT,
+            JSEvent.AXIS_DPAD_DOWN,
+            JSEvent.AXIS_LEFT_TRIGGER,
+            JSEvent.AXIS_RIGHT_TRIGGER,
+            JSEvent.AXIS_LEFT_BUMPER,
+            JSEvent.AXIS_RIGHT_BUMPER,
+            JSEvent.AXIS_TRIANGLE,
+            JSEvent.AXIS_CIRCLE,
+            JSEvent.AXIS_X,
+            JSEvent.AXIS_SQUARE,
+            JSEvent.AXIS_ACCEL_X,
+            JSEvent.AXIS_ACCEL_Y,
+            JSEvent.AXIS_ACCEL_Z,
+        ]
+
+        if self.ButtonUpdate and self.update_id in care_about_buttons:
+            self.__button_update()
+
+        if self.AxisUpdate and self.update_id in care_about_axes:
+            self.__axis_update()
+
+    def __button_update(self):
+        pass
+
+    def __axis_update(self):
+        pass
+   
+class JoyNode(Node):
+    def __init__(
+        self,
+        node_name: str,
+        *,
+        context: Context = None,
+        cli_args: List[str] = None,
+        namespace: str = None,
+        use_global_arguments: bool = True,
+        enable_rosout: bool = True,
+        start_parameter_services: bool = True,
+        parameter_overrides: List[Parameter] = None,
+        allow_undeclared_parameters: bool = False,
+        automatically_declare_parameters_from_overrides: bool = False,
+    ) -> None:
+        super().__init__(
+            node_name,
+            context=context,
+            cli_args=cli_args,
+            namespace=namespace,
+            use_global_arguments=use_global_arguments,
+            enable_rosout=enable_rosout,
+            start_parameter_services=start_parameter_services,
+            parameter_overrides=parameter_overrides,
+            allow_undeclared_parameters=allow_undeclared_parameters,
+            automatically_declare_parameters_from_overrides=automatically_declare_parameters_from_overrides,
+        )
+
+        self.pub = self.create_publisher(Joy, "/joy", 1)
+
+    def run(self):
+        msg = Joy()
+        msg.axes = [0.0, 0.0]
+        msg.buttons = [0]
+        with JoystickController("/dev/input/js0") as controller:
+            while rclpy.ok():
+                controller.Update()
+                sleep(0.0001)
+
+                if controller.hasUpdate():
+                    hor = controller.getAxisState(JSEvent.AXIS_LEFT_STICK_HORIZONTAL)
+                    ver = controller.getAxisState(JSEvent.AXIS_LEFT_STICK_VERTICAL)
+
+                    button_change_mode = controller.getButtonState(JSEvent.BUTTON_CIRCLE) # it says circle but it actually is dpad up
+                    self.get_logger().info(str(button_change_mode))
+
+                    hor /= 2**15
+                    ver /= 2**15
+
+                    msg.axes[JSEvent.AXIS_LEFT_STICK_HORIZONTAL] = hor
+                    msg.axes[JSEvent.AXIS_LEFT_STICK_VERTICAL] = ver
+                    msg.buttons[0] = button_change_mode
+
+                    self.pub.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = JoyNode("test")
+    node.run()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/ft24_control/ft24_control/testf1.py

@@ -0,0 +1,108 @@
+import numpy as np
+import cv2
+import matplotlib.pyplot as plt
+
+def generate_straight(length, width, start, direction=(1, 0)):
+    direction = np.array(direction)
+    norm_dir = direction / np.linalg.norm(direction)
+    left_edge = [start + norm_dir * width / 2 * np.array([-1, 1]) * i for i in range(length)]
+    right_edge = [start + norm_dir * width / 2 * np.array([1, -1]) * i for i in range(length)]
+    return left_edge, right_edge
+
+def generate_hairpin(radius, width, center, entry_angle=0):
+    angles = np.linspace(entry_angle, entry_angle + np.pi, 180)
+    left_edge = [center + (radius + width / 2) * np.array([np.cos(a), np.sin(a)]) for a in angles]
+    right_edge = [center + (radius - width / 2) * np.array([np.cos(a), np.sin(a)]) for a in angles]
+    return left_edge, right_edge
+
+def generate_chicane(segment_lengths, width, start, direction=(1, 0)):
+    direction = np.array(direction)
+    norm_dir = direction / np.linalg.norm(direction)
+    orth_dir = np.array([-norm_dir[1], norm_dir[0]])
+    
+    points = [start]
+    for i, length in enumerate(segment_lengths):
+        start += norm_dir * length if i % 2 == 0 else orth_dir * width * ((i % 4) - 2)
+        points.append(start)
+    
+    left_edge = [p + orth_dir * width / 2 for p in points]
+    right_edge = [p - orth_dir * width / 2 for p in points]
+    
+    return left_edge, right_edge
+
+def generate_track():
+    # Define the track dimensions and segments
+    straight_length = 100
+    hairpin_radius = 20
+    chicane_segment_lengths = [20, 10, 20]
+    track_width = 5
+
+    # Generate track segments
+    straight_start = np.array([0, 0])
+    straight_end = np.array([straight_length, 0])
+    left_straight, right_straight = generate_straight(straight_length, track_width, straight_start)
+    
+    hairpin_center = straight_end + np.array([0, -hairpin_radius])
+    left_hairpin, right_hairpin = generate_hairpin(hairpin_radius, track_width, hairpin_center)
+    
+    chicane_start = left_hairpin[-1]
+    left_chicane, right_chicane = generate_chicane(chicane_segment_lengths, track_width, chicane_start)
+
+    # Combine track segments
+    left_track = left_straight + left_hairpin[::-1] + left_chicane
+    right_track = right_straight + right_hairpin[::-1] + right_chicane
+
+    # move center to (100, 100)
+    left_track = left_track + np.array([100, 100])
+    right_track = right_track + np.array([100, 100])
+
+    return left_track, right_track
+
+def convert_to_grid_coordinates(cone, map_size_meters, map_resolution):
+    x, y = cone
+    grid_x = int(x / map_resolution)
+    grid_y = int(y / map_resolution)
+    return grid_x, grid_y
+
+def create_occupancy_grid(left_cones, right_cones, map_size_meters, map_resolution):
+    map_size_cells = int(map_size_meters / map_resolution)
+    grid_image = np.full((map_size_cells, map_size_cells), -1, dtype=np.int8)
+
+    max_weight = 100
+    min_weight = 50
+    weight_decrement = 5
+    cones = []
+
+    for cone in np.concatenate([left_cones, right_cones], axis=0):
+        left_x, left_y = convert_to_grid_coordinates(cone, map_size_meters, map_resolution)
+
+        cones.append((left_x, left_y))
+    cones = np.array([cones], dtype=np.int32)
+    cv2.fillPoly(grid_image, cones, color=0)
+
+    return grid_image
+
+def plot_occupancy_grid(grid, map_size_meters, map_resolution):
+    plt.imshow(grid, cmap='gray', origin='lower')
+    plt.title('Occupancy Grid ({}m x {}m, {}m/pixel)'.format(map_size_meters, map_size_meters, map_resolution))
+    plt.xlabel('X Position (m)')
+    plt.ylabel('Y Position (m)')
+    plt.xticks(np.arange(0, map_size_meters/map_resolution, step=10), np.arange(0, map_size_meters, step=10*map_resolution))
+    plt.yticks(np.arange(0, map_size_meters/map_resolution, step=10), np.arange(0, map_size_meters, step=10*map_resolution))
+    plt.show()
+
+# Test Parameters
+map_size_meters = 200
+map_resolution = 0.1  # 1 meter per pixel for simplicity
+
+# Generate and plot test track
+left_cones, right_cones = generate_track()
+grid = create_occupancy_grid(left_cones, right_cones, map_size_meters, map_resolution)
+plot_occupancy_grid(grid, map_size_meters, map_resolution)
+
+
+
+
+
+
+

src/ft24_control/ft24_control/twist.py

@@ -0,0 +1,106 @@
+import rclpy
+from rclpy.node import Node
+
+from geometry_msgs.msg import Twist
+from std_msgs.msg import String
+from sensor_msgs.msg import Joy
+
+import numpy as np
+
+SCALE_LINEAR_VELOCITY = 0.2
+SCALE_ANGULAR_VELOCITY = 0.7
+PUBLISH_FREQUENCY_HZ = 30
+
+SMOOTH_WEIGHTS = np.array([1, 2, 4, 10, 20])
+
+
+class TwistPublisher(Node):
+    """
+    This node serves as a bridge between the Joy messages published from the PS3 controller
+    interface in joy.py and the Twist messages expected by ros_control.
+    """
+    def __init__(
+        self,
+        smooth_weights=np.ones(5),
+        linear_scale=1.0,
+        angular_scale=1.0,
+        publish_frequency_hz=30,
+    ):
+        super().__init__("twist_publisher")
+
+        self.mode = "manual"
+
+        self.linear_window = np.zeros(smooth_weights.shape[0])
+        self.angular_window = np.zeros(smooth_weights.shape[0])
+        self.linear_recent = 0.0
+        self.angular_recent = 0.0
+
+        self.linear_out = 0.0
+        self.angular_out = 0.0
+
+        self.weights = smooth_weights / smooth_weights.sum()
+        self.window_size = smooth_weights.shape[0]
+
+        self.linear_scale = linear_scale
+        self.angular_scale = angular_scale
+
+        self.publisher_2 = self.create_publisher(Twist, "/cmd_vel_joystick", 1)
+
+        self.create_subscription(Joy, "/joy", self.joy_callback, 1)
+        self.timer = self.create_timer(1 / publish_frequency_hz, self.timer_callback)
+
+    def joy_callback(self, joy_message):
+        x, y = joy_message.axes[:2]
+        self.linear_recent = -y * SCALE_LINEAR_VELOCITY
+        self.angular_recent = -x * SCALE_ANGULAR_VELOCITY
+
+        if joy_message.buttons[0]:
+            self.mode = "manual" if self.mode == "auto" else "auto"
+            self.get_logger().info(f"Switching to {self.mode!r} mode")
+
+  
+    def update_smooth_window(self):
+        self.linear_window = np.append(self.linear_window, self.linear_recent)[
+            -self.window_size :
+        ]
+        self.angular_window = np.append(self.angular_window, self.angular_recent)[
+            -self.window_size :
+        ]
+
+    def calc_smooth_speed(self):
+        self.linear_out = np.average(self.linear_window, weights=self.weights)
+        self.angular_out = np.average(self.angular_window, weights=self.weights)
+
+    def timer_callback(self):
+        
+        if self.mode != "manual":
+            return
+
+        msg = Twist()
+
+        self.update_smooth_window()
+        self.calc_smooth_speed()
+
+        msg.linear.x = self.linear_out
+        msg.angular.z = self.angular_out
+        self.publisher_2.publish(msg)
+
+
+def main(args=None):
+    rclpy.init(args=args)
+
+    minimal_publisher = TwistPublisher(
+        SMOOTH_WEIGHTS,
+        SCALE_LINEAR_VELOCITY,
+        SCALE_ANGULAR_VELOCITY,
+        PUBLISH_FREQUENCY_HZ,
+    )
+
+    rclpy.spin(minimal_publisher)
+
+    minimal_publisher.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/ft24_control/include/SetLinkStatePlugin.hpp

@@ -0,0 +1,34 @@
+#ifndef SET_LINK_STATE_SYSTEM_PLUGIN_H
+#define SET_LINK_STATE_SYSTEM_PLUGIN_H
+
+#include <gz/sim/System.hh>
+#include <gz/sim/EventManager.hh>
+
+
+using namespace gz::sim;
+
+namespace set_link_state_system_plugin
+{
+  class SetLinkStateSystemPlugin:
+  public gz::sim::System,
+  public gz::sim::ISystemConfigure,
+  public gz::sim::ISystemUpdate
+  {
+  public:
+    /// \brief Constructor
+    SetLinkStateSystemPlugin() = default;
+
+    /// \brief Destructor
+    virtual ~SetLinkStateSystemPlugin() = default;
+
+    // Ignition Gazebo System interface
+    public: void Configure(const Entity & _entity, const std::shared_ptr<const sdf::Element> & _sdf,
+               gz::sim::EntityComponentManager & _ecm, gz::sim::EventManager & _eventManager) override;
+
+    public: void Update(const gz::sim::UpdateInfo &_info,
+                gz::sim::EntityComponentManager &_ecm) override;
+                       
+  };
+}
+
+#endif // SET_LINK_STATE_SYSTEM_PLUGIN_H

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,9 +39,9 @@ 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'
+        'ft24_config.yml'
         )
     
     controller_manager = Node(
@@ -55,7 +55,7 @@ def generate_launch_description():
     diff_drive_spawner = Node(
         package="controller_manager",
         executable="spawner",
-        arguments=["diff_cont"],
+        arguments=["ackermann_steering_controller"],
     )
 
     joint_broad_spawner = Node(

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')
@@ -41,7 +41,7 @@ def generate_launch_description():
     gazebo = IncludeLaunchDescription(
                 PythonLaunchDescriptionSource([os.path.join(
                     get_package_share_directory('ros_gz_sim'), 'launch', 'gz_sim.launch.py')]),
-                    launch_arguments=[('gz_args', [f" -r -v 1 {world_path}/generated_worlds/AU2_skidpad.sdf"])],
+                    launch_arguments=[('gz_args', [f" -r -v 0 {world_path}/generated_worlds/AU2_skidpad.sdf"])],
              )
 
     # Run the spawner node from the gazebo_ros package. The entity name doesn't really matter if you only have a single robot.
@@ -62,7 +62,6 @@ def generate_launch_description():
         arguments=[
             '/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock',
             '/boxes@vision_msgs/msg/Detection2DArray@ignition.msgs.AnnotatedAxisAligned2DBox_V',
-            '/lidar@sensor_msgs/msg/LaserScan@ignition.msgs.LaserScan',
             '/lidar/points@sensor_msgs/msg/PointCloud2@ignition.msgs.PointCloudPacked',
             'lidar/imu@sensor_msgs/msg/Imu@gz.msgs.IMU'
         ],
@@ -94,6 +93,29 @@ def generate_launch_description():
         on_exit=[diff_drive_spawner],
       )
     )
+    
+    imu_covariance_adder = Node(
+        package='ft24_control',
+        executable='imu_covariance_adder.py',
+        name='imu_covariance_adder',
+        output='screen',
+        parameters=[
+            {'orientation_covariance': [0.01, 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, 0.01]},
+            {'linear_acceleration_covariance': [0.0002, 0.0, 0.0, 0.0, 0.0002, 0.0, 0.0, 0.0, 0.0002]},
+            {'angular_velocity_covariance': [0.0001, 0.0, 0.0, 0.0, 0.0001, 0.0, 0.0, 0.0, 0.0001]},
+            {'subscribe_topic': '/lidar/imu'},
+            {'publish_topic': '/lidar/imu_covariance'}
+        ]
+    )
+
+    ukf_node = Node(
+            package='robot_localization', 
+            executable='ukf_node', 
+            name='ukf_filter_node',
+	        output='screen',
+            parameters=[str(base_path / 'config' / 'ukf.yml')],
+            remappings=[('odometry/filtered', 'odometry/local')]           
+           )
 
 
     # Launch them all!
@@ -112,4 +134,6 @@ def generate_launch_description():
         spawn_entity,
         delayed_diff_drive_spawner,
         delayed_joint_broad_spawner,
+        #imu_covariance_adder,
+        #ukf_node
     ]) 

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'
         )
@@ -67,5 +67,5 @@ def generate_launch_description():
 
         node_robot_state_publisher,
         node_joint_state_publisher,
-        twist_mux,
+        #twist_mux,
     ])

src/ft24_control/package.xml

@@ -0,0 +1,29 @@
+<?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>ft24_control </name>
+  <version>0.0.0</version>
+  <description>This package houses all ros2_control related code</description>
+  <maintainer email="fake@email.com">Tim Korjakow</maintainer>
+  <license>Todo</license>
+
+  <depend>ros2_control</depend>
+  <depend>ros2_controllers</depend>
+  <depend>ros_gz</depend>
+  <depend>robot_localization</depend>
+  <depend>joint_state_publisher</depend>
+  <depend>twist_mux</depend>
+  <depend>gz_ros2_control</depend>
+  <depend>robot_state_publisher</depend>
+  <depend>teleop_twist_keyboard</depend>
+
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

src/ft24_control/src/SetLinkStatePlugin.cpp

@@ -0,0 +1,47 @@
+#include "../include/SetLinkStatePlugin.hpp"
+
+//! [registerSampleSystem]
+#include <gz/plugin/RegisterMore.hh>
+#include <gz/sim/components.hh>
+
+using namespace components;
+
+// Include a line in your source file for each interface implemented.
+IGNITION_ADD_PLUGIN(
+    set_link_state_system_plugin::SetLinkStateSystemPlugin,
+    gz::sim::System,
+    set_link_state_system_plugin::SetLinkStateSystemPlugin::ISystemConfigure,
+    set_link_state_system_plugin::SetLinkStateSystemPlugin::ISystemUpdate
+)
+//! [registerSampleSystem]
+//! [implementSampleSystem]
+using namespace set_link_state_system_plugin;
+
+using namespace ignition::gazebo;
+
+// Ignition Gazebo System interface
+void SetLinkStateSystemPlugin::Configure(const Entity & _entity, const std::shared_ptr<const sdf::Element> & _sdf,
+                                                                                 EntityComponentManager & _ecm, EventManager & _eventManager)
+{
+
+    // Get the world entity
+    auto worldEntity = _ecm.EntityByComponents(components::Name("world"), components::World());
+    if (worldEntity == kNullEntity)
+    {
+        ignerr << "Failed to find the world entity.\n";
+        return;
+    }
+
+
+
+    ignmsg << "SetLinkStateSystemPlugin configured.\n";
+}
+
+void SetLinkStateSystemPlugin::Update(const UpdateInfo & _info, EntityComponentManager & _ecm)
+{
+    // Get the current simulation time in Fortress
+
+    // TODO: Implement communication with Fortress co-simulation script to get link states, velocities, accelerations, etc.
+    //
+}
+

src/ft24_control/src/external_pose_set.cpp

@@ -0,0 +1,84 @@
+#include <ignition/msgs.hh>
+#include <ignition/transport/Node.hh>
+
+#include <iostream>
+#include <cmath>
+#include <chrono>
+#include <thread>
+
+// Settings
+const std::string world_name = "empty";
+const std::string model_name = "spawn_robot";
+
+const int timeout_ms = 10;
+const double update_rate_hz = 100.0;
+
+// Offsets
+double pose_offset_x = 0.0;
+double pose_offset_y = 0.0;
+double pose_offset_z = 0.17;
+
+int main()
+{
+    // Connect to the Ignition Transport node
+    ignition::transport::Node node;
+
+    // Set service details
+    const std::string service = "/world/" + world_name + "/set_pose";
+    const std::string reqtype = "ignition.msgs.Pose";
+    const std::string reptype = "ignition.msgs.Boolean";
+
+    // Configure the update loop
+    auto start_time = std::chrono::steady_clock::now();
+    double update_period_s = 1.0 / update_rate_hz;
+    auto last_update_time = start_time;
+
+    while (true)
+    {
+        auto now = std::chrono::steady_clock::now();
+        auto time_elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(now - start_time);
+        double time_s = time_elapsed.count() / 1000.0;
+
+        if (std::chrono::duration_cast<std::chrono::milliseconds>(now - last_update_time).count() >= update_period_s * 1000.0)
+        {
+            last_update_time = now;
+
+            // Create request message
+            ignition::msgs::Vector3d vector3d_msg;
+            pose_offset_x = 10 * sin(time_s);
+            pose_offset_y = 10 * cos(time_s);
+            pose_offset_z = 0.17;
+            vector3d_msg.set_x(pose_offset_x);
+            vector3d_msg.set_y(pose_offset_y);
+            vector3d_msg.set_z(pose_offset_z);
+
+            ignition::msgs::Quaternion quat_msg;
+            // follow circle
+            double yaw = atan2(pose_offset_y, pose_offset_x);
+            quat_msg.set_w(cos(yaw / 2));
+            quat_msg.set_x(0);
+            quat_msg.set_y(0);
+            quat_msg.set_z(sin(yaw / 2));
+
+            ignition::msgs::Pose pose_msg;
+            pose_msg.set_name(model_name);
+            pose_msg.mutable_position()->CopyFrom(vector3d_msg);
+            pose_msg.mutable_orientation()->CopyFrom(quat_msg);
+            bool result;
+            ignition::msgs::Boolean result_msg;
+
+            // Submit request (blocking)
+            result = node.Request(service, pose_msg, timeout_ms, result_msg, result);
+
+            if (!result)
+            {
+                std::cout << "[" << time_s << "] Update failed" << std::endl;
+            }
+        }
+
+        // Sleep for a short duration to avoid high CPU usage
+        std::this_thread::sleep_for(std::chrono::milliseconds(10));
+    }
+
+    return 0;
+}

src/ft24_control/worlds/empty.sdf.template

@@ -1,39 +1,24 @@
 <?xml version="1.0" ?>
 <sdf version="1.6">
-  <world name="empty">
-    <physics name="phys" type="ode">
+  <world name="empty" >
+    <physics name="phys" type="dart">
       <max_step_size>0.005</max_step_size>
-      <real_time_factor>1.0</real_time_factor>
-      <ode>
-        <solver>
-          <type>quick</type>
-          <island_threads>true</island_threads>
-          <thread_position_correction>true</thread_position_correction>
-          <friction_model>cone_model</friction_model>
-        </solver>
-      </ode>
     </physics>
     <plugin
       filename="ignition-gazebo-physics-system"
-      name="gz::sim::systems::Physics">
+      name="ignition::gazebo::systems::Physics">
     </plugin>
     <plugin
       filename="ignition-gazebo-user-commands-system"
-      name="gz::sim::systems::UserCommands">
+      name="ignition::gazebo::systems::UserCommands">
     </plugin>
     <plugin
       filename="ignition-gazebo-scene-broadcaster-system"
-      name="gz::sim::systems::SceneBroadcaster">
+      name="ignition::gazebo::systems::SceneBroadcaster">
     </plugin>
-    <plugin
-      filename="ignition-gazebo-contact-system"
-      name="gz::sim::systems::Contact">
-    </plugin>
-    <plugin filename="libignition-gazebo-imu-system.so"
-        name="ignition::gazebo::systems::Imu">
-    </plugin> 
-    <plugin filename="libignition-gazebo-sensors-system.so" name="ignition::gazebo::systems::Sensors">
-        <render_engine>ogre2</render_engine>
+    <plugin filename="ignition-gazebo-sensors-system"
+      name="ignition::gazebo::systems::Sensors">
+      <render_engine>ogre2</render_engine>
     </plugin>
     <scene>
       <shadows>false</shadows>

src/ft24_control/worlds/generate_track_world.py

@@ -185,19 +185,7 @@ if __name__ == "__main__":
                     255
                 </laser_retro>
             </visual>
-            <sensor name='sensor_contact' type='contact'>
-                <contact>
-                    <collision>collision</collision>
-                </contact>
-            </sensor>
         </link>
-        <plugin filename="libignition-gazebo-touchplugin-system.so"
-            name="ignition::gazebo::systems::TouchPlugin">
-            <target>vehicle_blue</target>
-            <namespace>wall</namespace>
-            <time>0.001</time>
-            <enabled>true</enabled>
-        </plugin>
     </model>
                 '''
             cone_list_sdf.append(cone_sdf)

src/ros2_control_can/CMakeLists.txt

@@ -0,0 +1,93 @@
+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)
+
+set( GENERATED_CAN_HEADER
+  ${CMAKE_CURRENT_SOURCE_DIR}/include/can1__main_ft24.h
+)
+
+set( 
+  GENERATED_CAN_SRC
+  ${CMAKE_CURRENT_SOURCE_DIR}/src/can1__main_ft24.cpp
+)
+
+add_custom_command(OUTPUT ${GENERATED_CAN_HEADER} ${GENERATED_CAN_SRC}
+  COMMAND python3 -m cantools generate_c_source --prune ${CMAKE_CURRENT_SOURCE_DIR}/dbc/CAN1-MainFT24.dbc
+  COMMAND mv ${CMAKE_CURRENT_SOURCE_DIR}/src/can1__main_ft24.h ${CMAKE_CURRENT_SOURCE_DIR}/include/can1__main_ft24.h
+  COMMAND mv ${CMAKE_CURRENT_SOURCE_DIR}/src/can1__main_ft24.c ${CMAKE_CURRENT_SOURCE_DIR}/src/can1__main_ft24.cpp
+  WORKING_DIRECTORY "${CMAKE_CURRENT_SOURCE_DIR}/src"
+  DEPENDS "${CMAKE_CURRENT_SOURCE_DIR}/dbc/CAN1-MainFT24.dbc"
+  PRE_BUILD
+  COMMENT "Generating CAN header files and code from DBC file"
+  VERBATIM
+)
+
+add_library(
+  ros2_control_can
+  SHARED 
+  src/ros2_control_can.cpp
+  src/wheel.cpp
+  ${GENERATED_CAN_HEADER}
+  ${GENERATED_CAN_SRC}
+)
+
+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
+)
+
+install(
+  TARGETS ros2_control_can
+  DESTINATION lib
+)
+
+ament_export_libraries(
+  ros2_control_can
+)
+
+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/include/ros2_control_can/config.h

@@ -0,0 +1,18 @@
+#ifndef ROS2_CONTROL_CAN_CONFIG_H
+#define ROS2_CONTROL_CAN_CONFIG_H
+
+#include <string>
+
+
+struct Config
+{
+  std::string front_left_wheel_name = "front_left_wheel";
+  std::string front_right_wheel_name = "front_right_wheel";
+  std::string rear_left_wheel_name = "rear_left_wheel";
+  std::string rear_right_wheel_name = "rear_right_wheel";
+  float loop_rate = 50;
+  std::string device = "vcan0";
+};
+
+
+#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,75 @@
+#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"
+#include "can1__main_ft24.h"
+
+
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <linux/can.h>
+#include <linux/can/raw.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 f_l_wheel_;
+  Wheel f_r_wheel_;
+  Wheel r_l_wheel_;
+  Wheel r_r_wheel_;
+
+  can1__main_ft24_jetson_tx_t tx_frame;
+  can1__main_ft24_jetson_rx_t rx_frame;
+  uint8_t* can_buffer;
+  struct can_frame frame;
+
+  rclcpp::Logger logger_;
+
+  std::chrono::time_point<std::chrono::system_clock> time_;
+
+  int socket_instance;
+  
+};
+} // namespace ros2_control_can
+
+#endif // ROS2_CONTROL_CAN_REAL_ROBOT_H

src/ros2_control_can/include/ros2_control_can/wheel.h

@@ -0,0 +1,34 @@
+#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;
+    double steer = 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,31 @@
+<?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="fake@email.com">Tim Korjakow</maintainer>
+  <author>Tim Korjakow</author>
+  <license>Todo</license>
+
+  <depend>ros2_control</depend>
+  <depend>ros2_controllers</depend>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>hardware_interface</depend>
+  <depend>controller_manager</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <!-- <depend>rclcpp_lifecyle</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/ros2_control_can.cpp

@@ -0,0 +1,198 @@
+#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>
+
+#include <net/if.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+
+#include <linux/can.h>
+#include <linux/can/raw.h>
+
+#define RADSTORPM(a)(a*60.0/(2.0*M_PI))
+#define RPMSTORADS(a)(a*2.0*M_PI/60.0)
+#define REVSTORAD(a)(a*2.0*M_PI)
+#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_.front_left_wheel_name = info_.hardware_parameters["front_left_wheel_name"];
+  cfg_.front_right_wheel_name = info_.hardware_parameters["front_right_wheel_name"];
+  cfg_.rear_left_wheel_name = info_.hardware_parameters["rear_left_wheel_name"];
+  cfg_.rear_right_wheel_name = info_.hardware_parameters["rear_right_wheel_name"];
+  cfg_.loop_rate = std::stof(info_.hardware_parameters["loop_rate"]);
+  cfg_.device = info_.hardware_parameters["device"];
+
+  // Set up the wheels
+  f_l_wheel_.setup(cfg_.front_left_wheel_name);
+  f_r_wheel_.setup(cfg_.front_right_wheel_name);
+  r_l_wheel_.setup(cfg_.rear_left_wheel_name);
+  r_r_wheel_.setup(cfg_.rear_right_wheel_name);
+
+  // init tx frame
+  can1__main_ft24_jetson_tx_init(&tx_frame);
+
+   if ((socket_instance = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
+    perror("Socket");
+    return CallbackReturn::ERROR;
+  }
+
+  struct ifreq ifr;
+  const char *cstr = cfg_.device.c_str();
+  strcpy(ifr.ifr_name, cstr );
+  ioctl(socket_instance, SIOCGIFINDEX, &ifr);
+
+  struct sockaddr_can addr;
+  memset(&addr, 0, sizeof(addr));
+  addr.can_family = AF_CAN;
+  addr.can_ifindex = ifr.ifr_ifindex;
+  if (bind(socket_instance, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+    perror("Bind");
+    return CallbackReturn::ERROR;
+  }
+
+  //struct can_filter rfilter[1];
+  //rfilter[0].can_id   = 0x550;
+  //rfilter[0].can_mask = 0xFF0;
+  //rfilter[1].can_id   = 0x200;
+  //rfilter[1].can_mask = 0x700;
+  //setsockopt(socket_instance, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter));
+
+  // allocate can_buffer to 64 bit
+  can_buffer = (uint8_t*) malloc(8);
+
+  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(f_l_wheel_.name, hardware_interface::HW_IF_POSITION, &f_l_wheel_.steer));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(f_r_wheel_.name, hardware_interface::HW_IF_POSITION, &f_r_wheel_.steer));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(r_l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_l_wheel_.vel));
+  state_interfaces.emplace_back(hardware_interface::StateInterface(r_r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_r_wheel_.vel));
+
+  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(f_l_wheel_.name, hardware_interface::HW_IF_POSITION, &f_l_wheel_.steer));
+  command_interfaces.emplace_back(hardware_interface::CommandInterface(f_r_wheel_.name, hardware_interface::HW_IF_POSITION, &f_r_wheel_.steer));
+  command_interfaces.emplace_back(hardware_interface::CommandInterface(r_l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_l_wheel_.vel));
+  command_interfaces.emplace_back(hardware_interface::CommandInterface(r_r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_r_wheel_.vel));
+
+  return command_interfaces;
+}
+
+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...");
+
+  if (close(socket_instance) < 0) {
+    perror("Close");
+    return CallbackReturn::ERROR;
+  }
+
+  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;
+
+  // read can frame
+  if (::read(socket_instance, &frame, sizeof(struct can_frame)) < 0) {
+    perror("Read from device: ");
+    return return_type::ERROR;
+  }
+
+  // decode can frame
+  int rx_size = can1__main_ft24_jetson_rx_unpack(&rx_frame, reinterpret_cast<uint8_t*>(&frame.data), sizeof(frame.data));
+
+  if (rx_size < 0) {
+    perror("Unpack");
+    return return_type::ERROR;
+  }
+
+
+  return return_type::OK;  
+}
+
+return_type Ros2ControlCAN::write(const rclcpp::Time&, const rclcpp::Duration&)
+{
+  tx_frame.jetson_steering_angle = can1__main_ft24_jetson_tx_jetson_steering_angle_encode((f_l_wheel_.steer + f_r_wheel_.steer) / 2.0);
+  tx_frame.jetson_speed_target = can1__main_ft24_jetson_tx_jetson_speed_target_encode((r_l_wheel_.vel + r_r_wheel_.vel) / 2.0);
+  
+  int tx_size = can1__main_ft24_jetson_tx_pack(reinterpret_cast<uint8_t*>(&frame.data), &tx_frame, sizeof(frame.data));
+
+  if (tx_size < 0) {
+    perror("Pack");
+    return return_type::ERROR;
+  }
+
+  frame.can_id = 0xe1;
+  frame.can_dlc = tx_size;
+  
+  if (::write(socket_instance, &frame, sizeof(struct can_frame)) < 0) {
+    perror("Write to device: ");
+    // print error with socket instance info
+    return return_type::ERROR;
+  }
+
+  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

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