Added working CAN and first draft of sim with chrono

src/chabo_chrono

@@ -0,0 +1 @@
+Subproject commit 3064103811a228c1ad5e50f1b75006c04a36d6b9

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
+find_package(ignition-cmake2 REQUIRED)
-# further dependencies manually.
+find_package(ignition-msgs5 REQUIRED)
-# find_package(<dependency> 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,9 +75,9 @@ ament_python_install_package(${PROJECT_NAME})
  
 # Install Python executables
 install(PROGRAMS
-  src/imu_covariance_adder.py
+  ft24_control/imu_covariance_adder.py
-  src/joy.py
+  ft24_control/joy.py
-  src/twist.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/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/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/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,43 +1,23 @@
 <?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"
+      filename="ignition-gazebo-physics-system"
-      name="gz::sim::systems::Physics">
+      name="ignition::gazebo::systems::Physics">
     </plugin>
     <plugin
-      filename="gz-sim-user-commands-system"
+      filename="ignition-gazebo-user-commands-system"
-      name="gz::sim::systems::UserCommands">
+      name="ignition::gazebo::systems::UserCommands">
     </plugin>
     <plugin
-      filename="gz-sim-scene-broadcaster-system"
+      filename="ignition-gazebo-scene-broadcaster-system"
-      name="gz::sim::systems::SceneBroadcaster">
+      name="ignition::gazebo::systems::SceneBroadcaster">
     </plugin>
-    <plugin
+    <plugin filename="ignition-gazebo-sensors-system"
-      filename="gz-sim-contact-system"
+      name="ignition::gazebo::systems::Sensors">
-      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>
     <scene>

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_l_wheel_.setup(cfg_.rear_left_wheel_name);
-  r_r_wheel_.setup(cfg_.front_right_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;
-  }
+}
 
 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;
 
-
+  // read can frame
-  if (false)
+  if (::read(socket_instance, &frame, sizeof(struct can_frame)) < 0) {
-  {
+    perror("Read from device: ");
     return return_type::ERROR;
   }
 
-  // convert rpm to rad/s
+  // decode can frame
-  //l_wheel_.vel = RPMSTORADS(l_wheel_.vel);
+  int rx_size = can1__main_ft24_jetson_rx_unpack(&rx_frame, reinterpret_cast<uint8_t*>(&frame.data), sizeof(frame.data));
-  //r_wheel_.vel = RPMSTORADS(r_wheel_.vel);
+
+  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)
+  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);
-    return return_type::ERROR;
-  }
   
-  // send can frame
+  int tx_size = can1__main_ft24_jetson_tx_pack(reinterpret_cast<uint8_t*>(&frame.data), &tx_frame, sizeof(frame.data));
-  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);
 
   if (tx_size < 0) {
     perror("Pack");
     return return_type::ERROR;
   }
 
-  // send tx_frame
+  frame.can_id = 0xe1;
-  struct can_frame frame;
-  frame.can_id = 0x550;
   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;
   }