Post SAR updates

src/bringup/CMakeLists.txt

@@ -17,9 +17,10 @@ if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/config)
 endif()
 
 # Install controller_switcher script from bringup package if present
-if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/scripts/controller_switcher.py)
+if(EXISTS ${CMAKE_CURRENT_SOURCE_DIR}/scripts/controller_switcher.py OR ${CMAKE_CURRENT_SOURCE_DIR}/scripts/cmd_vel_to_rmd.py)
   install(PROGRAMS
     scripts/controller_switcher.py
+    scripts/cmd_vel_to_rmd.py
     DESTINATION lib/${PROJECT_NAME}
   )
 endif()

src/bringup/scripts/cmd_vel_to_rmd.py

@@ -0,0 +1,83 @@
+#!/usr/bin/env python3
+import rclpy
+from rclpy.node import Node
+from geometry_msgs.msg import TwistStamped
+import socket
+import struct
+import time
+
+CAN_FMT  = "=IH2x8s"
+CAN_SIZE = struct.calcsize(CAN_FMT)
+
+
+CAN_IDS       = [0x145, 0x141, 0x142, 0x144]
+CAN_INTERFACE = "can1"
+MAX_SPEED_DPS = 500.0
+SCALE         = 100.0
+TOPIC         = "/rear_ackermann_controller/reference"
+
+
+SPEED_CONTROL_CMD = 0xA2
+
+
+
+
+class CmdVelToRMD(Node):
+   def __init__(self):
+       super().__init__("cmd_vel_to_rmd")
+
+
+       self._sock = socket.socket(socket.AF_CAN, socket.SOCK_RAW, socket.CAN_RAW)
+       self._sock.bind((CAN_INTERFACE,))
+       self.get_logger().info(f"Listening on {TOPIC}, sending to {[hex(i) for i in CAN_IDS]}")
+
+
+       self.create_subscription(TwistStamped, TOPIC, self._cb, 10)
+
+
+   def _cb(self, msg: TwistStamped):
+       speed_dps  = msg.twist.linear.x * SCALE
+       speed_dps  = max(-MAX_SPEED_DPS, min(MAX_SPEED_DPS, speed_dps))
+       speed_ctrl = int(speed_dps / 0.01) & 0xFFFFFFFF
+
+
+       data = bytes([
+           SPEED_CONTROL_CMD, 0x00, 0x00, 0x00,
+           (speed_ctrl)       & 0xFF,
+           (speed_ctrl >> 8)  & 0xFF,
+           (speed_ctrl >> 16) & 0xFF,
+           (speed_ctrl >> 24) & 0xFF,
+       ])
+
+
+       for can_id in CAN_IDS:
+           self._sock.send(struct.pack(CAN_FMT, can_id, 8, data))
+           time.sleep(.001)
+
+
+   def destroy_node(self):
+       self._sock.close()
+       super().destroy_node()
+
+
+
+
+def main(args=None):
+   rclpy.init(args=args)
+   node = CmdVelToRMD()
+   try:
+       rclpy.spin(node)
+   except KeyboardInterrupt:
+       pass
+   finally:
+       node.destroy_node()
+       rclpy.shutdown()
+
+
+
+
+if __name__ == "__main__":
+   main()
+
+
+

src/config/cyclonedds/config_jetson.xml

@@ -10,8 +10,8 @@ https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclo
         </General>
         <Discovery>
             <Peers>
-                <Peer address="192.168.1.1"/>
-                <Peer address="192.168.1.12"/>
+                <Peer address="192.168.88.90"/>
+                <Peer address="192.168.88.10"/>
             </Peers>
             <ParticipantIndex>auto</ParticipantIndex>
             <MaxAutoParticipantIndex>1000</MaxAutoParticipantIndex>

src/config/cyclonedds/config_right_base.xml

@@ -10,8 +10,8 @@ https://raw.githubusercontent.com/eclipse-cyclonedds/cyclonedds/master/etc/cyclo
         </General>
         <Discovery>
             <Peers>
-                <Peer address="192.168.1.12"/>
-                <Peer address="192.168.1.1"/>
+                <Peer address="192.168.88.90"/>
+                <Peer address="192.168.88.10"/>
             </Peers>
             <ParticipantIndex>auto</ParticipantIndex>
             <MaxAutoParticipantIndex>1000</MaxAutoParticipantIndex>

src/description/ros2_control/arm/arm.rmd.ros2_control.xacro

@@ -4,14 +4,14 @@
     <ros2_control name="${name}" type="system">
       <hardware>
         <plugin>rmd_ros2_control/RMDHardwareInterface</plugin>
-        <param name="update_rate">2</param> <!-- Hz -->
+        <param name="update_rate">10</param> <!-- Hz -->
         <param name="logger_rate">5</param> <!-- Hz -->
         <param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
         <param name="can_interface">can0</param>
       </hardware>
 
       <joint name="shoulder_pitch">
-        <param name="node_id">0x14A</param>
+        <param name="node_id">0x149</param>
         <param name="gear_ratio">100</param>
         <param name="joint_orientation">1.0</param>
         <param name="operating_velocity">150</param> <!-- motor dps -->

src/description/ros2_control/arm/arm.smc.ros2_control.xacro

@@ -4,7 +4,7 @@
     <ros2_control name="${name}" type="system">
       <hardware>
         <plugin>smc_ros2_control/SMCHardwareInterface</plugin>
-        <param name="update_rate">2</param> <!-- Hz -->
+        <param name="update_rate">10</param> <!-- Hz -->
         <param name="logger_rate">5</param> <!-- Hz -->
         <param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
         <param name="can_interface">can0</param>

src/description/ros2_control/drive/drive.odrive.ros2_control.xacro

@@ -10,7 +10,7 @@
       </hardware>
 
       <joint name="steer_bl_joint">
-        <param name="node_id">5</param>
+        <param name="node_id">7</param>
         <param name="gear_ratio">26</param>
         <command_interface name="position"/>
         <command_interface name="velocity"/>
@@ -21,7 +21,7 @@
       </joint>
 
       <joint name="steer_br_joint">
-        <param name="node_id">6</param>
+        <param name="node_id">5</param>
         <param name="gear_ratio">26</param>
         <command_interface name="position"/>
         <command_interface name="velocity"/>

src/description/ros2_control/drive/drive.rmd.ros2_control.xacro

@@ -4,7 +4,7 @@
     <ros2_control name="${name}" type="system">
       <hardware>
         <plugin>rmd_ros2_control/RMDHardwareInterface</plugin>
-        <param name="update_rate">30</param> <!-- Hz -->
+        <param name="update_rate">10</param> <!-- Hz -->
         <param name="logger_rate">5</param> <!-- Hz -->
         <param name="logger_state">0</param> <!-- 0 for off, 1 for on -->
         <param name="can_interface">can0</param>
@@ -24,7 +24,7 @@
       </joint>
 
       <joint name="propulsion_fr_joint">
-        <param name="node_id">0x149</param>
+        <param name="node_id">0x145</param>
         <param name="gear_ratio">1</param>
         <param name="joint_orientation">1.0</param>
         <param name="operating_velocity">150</param> <!-- motor dps -->
@@ -37,7 +37,7 @@
       </joint>
 
       <joint name="propulsion_bl_joint">
-        <param name="node_id">0x141</param>
+        <param name="node_id">0x144</param>
         <param name="gear_ratio">1</param>
         <param name="joint_orientation">-1.0</param>
         <param name="operating_velocity">150</param> <!-- motor dps -->
@@ -50,7 +50,7 @@
       </joint>
 
       <joint name="propulsion_br_joint">
-        <param name="node_id">0x143</param>
+        <param name="node_id">0x141</param>
         <param name="gear_ratio">1</param>
         <param name="joint_orientation">1.0</param>
         <param name="operating_velocity">150</param> <!-- motor dps -->
@@ -63,4 +63,4 @@
       </joint>
     </ros2_control>
   </xacro:macro>
-</robot>
+</robot>

src/hardware_interfaces/ros_odrive/odrive_ros2_control/src/odrive_hardware_interface.cpp

@@ -283,14 +283,14 @@ return_type ODriveHardwareInterface::write(const rclcpp::Time&, const rclcpp::Du
             msg.Input_Pos = (axis.pos_setpoint_ * axis.gear_ratio_) / (2 * M_PI);
             msg.Vel_FF = axis.vel_input_enabled_ ? ((axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI)) : 0.0f;
             msg.Torque_FF = axis.torque_input_enabled_ ? (axis.torque_setpoint_ / axis.gear_ratio_) : 0.0f;
-            // RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Writing positions for ODrive %d Setpoint: %f, Joint angle of motor (rev): %f", axis.node_id_, axis.pos_setpoint_, (axis.pos_setpoint_ * axis.gear_ratio_) / (2 * M_PI));
+            //RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Writing positions for ODrive %d Setpoint: %f, Joint angle of motor (rev): %f", axis.node_id_, axis.pos_setpoint_, (axis.pos_setpoint_ * axis.gear_ratio_) / (2 * M_PI));
 
             axis.send(msg);
         } else if (axis.vel_input_enabled_) {
             Set_Input_Vel_msg_t msg;
             msg.Input_Vel = (axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI);
             msg.Input_Torque_FF = axis.torque_input_enabled_ ? (axis.torque_setpoint_ / axis.gear_ratio_): 0.0f;
-            // RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Writing velocities for ODrive %d Joint velocity of motor (rev/s): %f", axis.node_id_, (axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI));
+            RCLCPP_INFO(rclcpp::get_logger("ODriveHardwareInterface"), "Writing velocities for ODrive %d Joint velocity of motor (rev/s): %f", axis.node_id_, (axis.vel_setpoint_ * axis.gear_ratio_) / (2 * M_PI));
 
             axis.send(msg);
         } else if (axis.torque_input_enabled_) {

src/infrastructure/umdloop_can_library/include/umdloop_can_library/SocketCanBus.hpp

@@ -1,9 +1,11 @@
 #pragma once
 
 #include "ICanBus.hpp"
-#include <memory>
 #include <thread>
 #include <atomic>
+#include <queue>
+#include <mutex>
+#include <condition_variable>
 
 namespace CANLib {
 
@@ -17,13 +19,21 @@ class SocketCanBus : public ICanBus {
     bool send(const CanFrame& frame) override;
 
 private:
+    static constexpr size_t kMaxTxQueueDepth = 256;
+
     int socketFd_;
     ReceiveCallback receiveCallback_;
     std::thread receiveThread_;
+    std::thread txThread_;
     std::atomic<bool> running_;
     std::string interfaceName_;
 
+    std::queue<CanFrame> txQueue_;
+    std::mutex txMutex_;
+    std::condition_variable txCv_;
+
     void receiveLoop_();
+    void txLoop_();
 };
 
 } // namespace CANLib