Added working CAN and first draft of sim with chrono

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

src/ft24_control/CMakeLists.txt

@@ -17,9 +17,43 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> 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)
@@ -33,7 +67,7 @@ if(BUILD_TESTING)
 endif()
 
 install(
-  DIRECTORY config description launch worlds src
+  DIRECTORY config description launch worlds ft24_control
   DESTINATION share/${PROJECT_NAME}
 )
 
@@ -41,7 +75,9 @@ ament_python_install_package(${PROJECT_NAME})
  
 # Install Python executables
 install(PROGRAMS
-  src/imu_covariance_adder.py
+  ft24_control/imu_covariance_adder.py
+  ft24_control/joy.py
+  ft24_control/twist.py
   DESTINATION lib/${PROJECT_NAME}
 )
 

src/ft24_control/config/ft24_config.yml

@@ -21,7 +21,7 @@ ackermann_steering_controller:
     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/description/real_robot_control.xacro

@@ -13,23 +13,19 @@
         <!-- Note everything below here is the same as the Gazebo one -->
         <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>

src/ft24_control/description/ros2_control.xacro

@@ -6,23 +6,19 @@
         </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>

src/ft24_control/description/wheel.xacro

@@ -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/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/package.xml

@@ -15,6 +15,7 @@
   <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>

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/src/testf1.py

@@ -1,74 +0,0 @@
-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/ft24_control/worlds/empty.sdf.template

@@ -1,44 +1,24 @@
 <?xml version="1.0" ?>
 <sdf version="1.6">
-  <world name="empty">
+  <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>
-      <dart>
-        <collision_detector>
-          bullet
-        </collision_detector>
-      </dart>
     </physics>
     <plugin
-      filename="gz-sim-physics-system"
-      name="gz::sim::systems::Physics">
+      filename="ignition-gazebo-physics-system"
+      name="ignition::gazebo::systems::Physics">
     </plugin>
     <plugin
-      filename="gz-sim-user-commands-system"
-      name="gz::sim::systems::UserCommands">
+      filename="ignition-gazebo-user-commands-system"
+      name="ignition::gazebo::systems::UserCommands">
     </plugin>
     <plugin
-      filename="gz-sim-scene-broadcaster-system"
-      name="gz::sim::systems::SceneBroadcaster">
+      filename="ignition-gazebo-scene-broadcaster-system"
+      name="ignition::gazebo::systems::SceneBroadcaster">
     </plugin>
-    <plugin
-      filename="gz-sim-contact-system"
-      name="gz::sim::systems::Contact">
-    </plugin>
-    <plugin filename="gz-sim-imu-system.so"
-        name="gz::sim::systems::Imu">
-    </plugin> 
-    <plugin filename="gz-sim-sensors-system.so" name="gz::sim::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/include/ros2_control_can/ros2_control_can.h

@@ -14,6 +14,14 @@
 #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;
 
@@ -52,7 +60,8 @@ private:
 
   can1__main_ft24_jetson_tx_t tx_frame;
   can1__main_ft24_jetson_rx_t rx_frame;
-  uint8_t can_buffer[64];
+  uint8_t* can_buffer;
+  struct can_frame frame;
 
   rclcpp::Logger logger_;
 

src/ros2_control_can/src/ros2_control_can.cpp

@@ -48,12 +48,41 @@ CallbackReturn Ros2ControlCAN::on_init(const hardware_interface::HardwareInfo &
   // 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_.front_left_wheel_name);
-  r_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;
@@ -69,8 +98,6 @@ std::vector<hardware_interface::StateInterface> Ros2ControlCAN::export_state_int
   state_interfaces.emplace_back(hardware_interface::StateInterface(f_r_wheel_.name, hardware_interface::HW_IF_POSITION, &f_r_wheel_.steer));
   state_interfaces.emplace_back(hardware_interface::StateInterface(r_l_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_l_wheel_.vel));
   state_interfaces.emplace_back(hardware_interface::StateInterface(r_r_wheel_.name, hardware_interface::HW_IF_VELOCITY, &r_r_wheel_.vel));
-  state_interfaces.emplace_back(hardware_interface::StateInterface(r_l_wheel_.name, hardware_interface::HW_IF_POSITION, &r_l_wheel_.pos));
-  state_interfaces.emplace_back(hardware_interface::StateInterface(r_r_wheel_.name, hardware_interface::HW_IF_POSITION, &r_r_wheel_.pos));
 
   return state_interfaces;
 }
@@ -93,34 +120,8 @@ CallbackReturn Ros2ControlCAN::on_activate(const rclcpp_lifecycle::State & previ
 {
   RCLCPP_INFO(logger_, "Starting Controller...");
 
-  if ((socket_instance = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
-    perror("Socket");
-    return CallbackReturn::ERROR;
-  }
-
-  struct ifreq ifr;
-  const char *cstr = cfg_.device.c_str();
-  strcpy(ifr.ifr_name, cstr );
-  ioctl(socket_instance, SIOCGIFINDEX, &ifr);
-
-  struct sockaddr_can addr;
-  memset(&addr, 0, sizeof(addr));
-  addr.can_family = AF_CAN;
-  addr.can_ifindex = ifr.ifr_ifindex;
-  if (bind(socket_instance, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
-    perror("Bind");
-    return CallbackReturn::ERROR;
-  }
-
-  struct can_filter rfilter[1];
-  rfilter[0].can_id   = 0x550;
-  rfilter[0].can_mask = 0xFF0;
-  //rfilter[1].can_id   = 0x200;
-  //rfilter[1].can_mask = 0x700;
-  setsockopt(socket_instance, SOL_CAN_RAW, CAN_RAW_FILTER, &rfilter, sizeof(rfilter));
-
-    return CallbackReturn::SUCCESS;
-  }
+  return CallbackReturn::SUCCESS;
+}
 
 CallbackReturn Ros2ControlCAN::on_deactivate(const rclcpp_lifecycle::State & previous_state)
 {
@@ -145,45 +146,42 @@ return_type Ros2ControlCAN::read(const rclcpp::Time & /* time */, const rclcpp::
   double deltaSeconds = diff.count();
   time_ = new_time;
 
-
-  if (false)
-  {
+  // read can frame
+  if (::read(socket_instance, &frame, sizeof(struct can_frame)) < 0) {
+    perror("Read from device: ");
     return return_type::ERROR;
   }
 
-  // convert rpm to rad/s
-  //l_wheel_.vel = RPMSTORADS(l_wheel_.vel);
-  //r_wheel_.vel = RPMSTORADS(r_wheel_.vel);
+  // 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 & /* time */, const rclcpp::Duration & /* period */)
+return_type Ros2ControlCAN::write(const rclcpp::Time&, const rclcpp::Duration&)
 {
-  if (false)
-  {
-    return return_type::ERROR;
-  }
+  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);
   
-  // send can frame
-  tx_frame.jetson_steering_angle = can1__main_ft24_jetson_tx_jetson_steering_angle_encode((f_l_wheel_.steer + f_r_wheel_.steer)/2.0);
-  
-  // pack tx_frame
-  int tx_size = can1__main_ft24_jetson_tx_pack(can_buffer, &tx_frame, 64);
+  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;
   }
 
-  // send tx_frame
-  struct can_frame frame;
-  frame.can_id = 0x550;
+  frame.can_id = 0xe1;
   frame.can_dlc = tx_size;
-  memcpy(frame.data, can_buffer, tx_size);
+  
   if (::write(socket_instance, &frame, sizeof(struct can_frame)) < 0) {
-    perror("Write");
+    perror("Write to device: ");
+    // print error with socket instance info
     return return_type::ERROR;
   }