feat: (core) make compatible with ros2_control

Qos disable

src/anchor_pkg/launch/rover.launch.py

@@ -1,13 +1,21 @@
 #!/usr/bin/env python3
 
 from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, Shutdown
+from launch.actions import (
+    DeclareLaunchArgument,
+    OpaqueFunction,
+    Shutdown,
+    IncludeLaunchDescription,
+)
 from launch.substitutions import (
     LaunchConfiguration,
     ThisLaunchFileDir,
     PathJoinSubstitution,
 )
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.conditions import IfCondition
 from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
 
 
 # Prevent making __pycache__ directories
@@ -40,7 +48,10 @@ def launch_setup(context, *args, **kwargs):
                 executable="core",  # change as needed
                 name="core",
                 output="both",
-                parameters=[{"launch_mode": mode}],
+                parameters=[
+                    {"launch_mode": mode},
+                    {"use_ros2_control": LaunchConfiguration("use_ros2_control", default=False)},
+                ],
                 on_exit=Shutdown(),
             )
         )
@@ -133,4 +144,46 @@ def generate_launch_description():
         description="Launch mode: arm, core, bio, anchor, or ptz",
     )
 
-    return LaunchDescription([declare_arg, OpaqueFunction(function=launch_setup)])
+    ros2_control_arg = DeclareLaunchArgument(
+        "use_ros2_control",
+        default_value="false",
+        description="Whether to use DiffDriveController for driving instead of direct Twist",
+    )
+
+    rsp = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("core_description"),
+                    "launch",
+                    "robot_state_publisher.launch.py",
+                ]
+            )
+        ),
+        condition=IfCondition(LaunchConfiguration("use_ros2_control")),
+        launch_arguments={("hardware_mode", "physical")},
+    )
+
+    controllers = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("core_description"),
+                    "launch",
+                    "spawn_controllers.launch.py",
+                ]
+            )
+        ),
+        condition=IfCondition(LaunchConfiguration("use_ros2_control")),
+        launch_arguments={("hardware_mode", "physical")},
+    )
+
+    return LaunchDescription(
+        [
+            declare_arg,
+            ros2_control_arg,
+            rsp,
+            controllers,
+            OpaqueFunction(function=launch_setup),
+        ]
+    )

src/arm_pkg/arm_pkg/arm_node.py

@@ -49,7 +49,7 @@ class SerialRelay(Node):
 
         # Create subscribers
         self.man_sub = self.create_subscription(
-            ArmManual, "/arm/control/manual", self.send_manual, 10
+            ArmManual, "/arm/control/manual", self.send_manual, 2
         )
 
         # New messages
@@ -190,7 +190,7 @@ class SerialRelay(Node):
 
         command += f"can_relay_tovic,digit,39,{msg.effector_yaw},{msg.effector_roll}\n"
 
-        # command += f"can_relay_tovic,digit,26,{msg.gripper}\n"  # no hardware rn
+        command += f"can_relay_tovic,digit,26,{msg.gripper}\n"  # no hardware rn
 
         command += f"can_relay_tovic,digit,28,{msg.laser}\n"
 

src/core_pkg/core_pkg/core_node.py

@@ -31,14 +31,14 @@ CORE_WHEEL_RADIUS = 0.171  # meters
 CORE_GEAR_RATIO = 100.0  # Clucky: 100:1, Testbed: 64:1
 
 control_qos = qos.QoSProfile(
-    history=qos.QoSHistoryPolicy.KEEP_LAST,
+    # history=qos.QoSHistoryPolicy.KEEP_LAST,
     depth=2,
-    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    durability=qos.QoSDurabilityPolicy.VOLATILE,
+    # durability=qos.QoSDurabilityPolicy.VOLATILE,
-    deadline=Duration(seconds=1),
+    # deadline=Duration(seconds=1),
-    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
+    # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    liveliness_lease_duration=Duration(seconds=5),
+    # liveliness_lease_duration=Duration(seconds=5),
 )
 
 # Used to verify the length of an incoming VicCAN feedback message
@@ -66,6 +66,10 @@ class SerialRelay(Node):
         self.launch_mode = self.get_parameter("launch_mode").value
         self.get_logger().info(f"Core launch_mode is: {self.launch_mode}")
 
+        self.declare_parameter("use_ros2_control", False)
+        self.use_ros2_control = self.get_parameter("use_ros2_control").value
+        self.get_logger().info(f"Use ros2_control: {self.use_ros2_control}")
+
         ##################################################
         # Topics
 
@@ -85,24 +89,28 @@ class SerialRelay(Node):
 
         # Control
 
-        # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
+        if self.use_ros2_control:
-        self.cmd_vel_sub_ = self.create_subscription(
+            # Joint state control for topic-based controller
-            TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
+            self.joint_command_sub_ = self.create_subscription(
-        )
+                JointState, "/core/joint_commands", self.joint_command_callback, 2
-        # manual twist -- [-1, 1] rather than real units
+            )
-        self.twist_man_sub_ = self.create_subscription(
+        else:
-            Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
+            # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
-        )
+            self.cmd_vel_sub_ = self.create_subscription(
-        # manual flags -- brake mode and max duty cycle
+                TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
-        self.control_state_sub_ = self.create_subscription(
+            )
-            CoreCtrlState,
+            # manual twist -- [-1, 1] rather than real units
-            "/core/control/state",
+            self.twist_man_sub_ = self.create_subscription(
-            self.control_state_callback,
+                Twist, "/core/twist", self.twist_man_callback, qos_profile=control_qos
-            qos_profile=control_qos,
+            )
-        )
+            # manual flags -- brake mode and max duty cycle
-        self.twist_max_duty = (
+            self.control_state_sub_ = self.create_subscription(
-            0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
+                CoreCtrlState,
-        )
+                "/core/control/state",
+                self.control_state_callback,
+                qos_profile=control_qos,
+            )
+            self.twist_max_duty = 0.5  # max duty cycle for twist commands (0.0 - 1.0); walking speed is 0.5
 
         # Feedback
 
@@ -122,7 +130,7 @@ class SerialRelay(Node):
         )  # TODO: not sure about this
         # Joint states for topic-based controller
         self.joint_state_pub_ = self.create_publisher(
-            JointState, "/core/joint_states", qos_profile=qos.qos_profile_sensor_data
+            JointState, "/joint_states", qos_profile=qos.qos_profile_sensor_data
         )
         # IMU (embedded BNO-055)
         self.imu_pub_ = self.create_publisher(
@@ -148,10 +156,11 @@ class SerialRelay(Node):
 
         # Old
 
-        # /core/control
+        if not self.use_ros2_control:
-        self.control_sub = self.create_subscription(
+            # /core/control
-            CoreControl, "/core/control", self.send_controls, 10
+            self.control_sub = self.create_subscription(
-        )  # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
+                CoreControl, "/core/control", self.send_controls, 10
+            )  # old control method -- left_stick, right_stick, max_speed, brake, and some other random autonomy stuff
         # /core/feedback
         self.feedback_pub = self.create_publisher(CoreFeedback, "/core/feedback", 10)
         self.core_feedback = CoreFeedback()
@@ -283,6 +292,40 @@ class SerialRelay(Node):
 
         # print(f"[Sys] Relaying: {command}")
 
+    def joint_command_callback(self, msg: JointState):
+        # So... topic based control node publishes JointState messages over /joint_commands
+        #  with len(msg.name) == 5 and len(msg.velocity) == 4... all 5 non-fixed joints
+        #  are included in msg.name, but ig it is implied that msg.velocity only
+        #  includes velocities for the commanded joints (ros__parameters.joints).
+        # So, this will be much more hacky and less adaptable than I would like it to be.
+        if len(msg.name) != 5 or len(msg.velocity) != 4 or len(msg.position) != 0:
+            self.get_logger().warning(
+                f"Received joint control message with unexpected number of joints. Ignoring."
+            )
+            return
+        if msg.name != [
+            "left_suspension_joint",
+            "bl_wheel_joint",
+            "br_wheel_joint",
+            "fl_wheel_joint",
+            "fr_wheel_joint",
+        ]:
+            self.get_logger().warning(
+                f"Received joint control message with unexpected name[]. Ignoring."
+            )
+            return
+
+        (bl_vel, br_vel, fl_vel, fr_vel) = msg.velocity
+
+        bl_rpm = radps_to_rpm(bl_vel) * CORE_GEAR_RATIO
+        br_rpm = radps_to_rpm(br_vel) * CORE_GEAR_RATIO
+        fl_rpm = radps_to_rpm(fl_vel) * CORE_GEAR_RATIO
+        fr_rpm = radps_to_rpm(fr_vel) * CORE_GEAR_RATIO
+
+        self.send_viccan(
+            20, [fl_rpm, bl_rpm, fr_rpm, br_rpm]
+        )  # order expected by embedded
+
     def cmd_vel_callback(self, msg: TwistStamped):
         linear = msg.twist.linear.x
         angular = -msg.twist.angular.z
@@ -528,22 +571,27 @@ class SerialRelay(Node):
                 match motorId:
                     case 1:
                         motor = self.feedback_new_state.fl_motor
-                        joint_state_msg.name = ["fl_motor_joint"]
+                        joint_state_msg.name = ["fl_wheel_joint"]
                     case 2:
                         motor = self.feedback_new_state.bl_motor
-                        joint_state_msg.name = ["bl_motor_joint"]
+                        joint_state_msg.name = ["bl_wheel_joint"]
                     case 3:
                         motor = self.feedback_new_state.fr_motor
-                        joint_state_msg.name = ["fr_motor_joint"]
+                        joint_state_msg.name = ["fr_wheel_joint"]
                     case 4:
                         motor = self.feedback_new_state.br_motor
-                        joint_state_msg.name = ["br_motor_joint"]
+                        joint_state_msg.name = ["br_wheel_joint"]
                     case _:
                         self.get_logger().warning(
                             f"Ignoring REV motor feedback 58 with invalid motorId {motorId}"
                         )
                         return
 
+                # make the fucking shit work
+                joint_state_msg.name.append("left_suspension_joint")
+                joint_state_msg.position.append(0.0)
+                joint_state_msg.velocity.append(0.0)
+
                 joint_state_msg.header.stamp = msg.header.stamp
                 self.joint_state_pub_.publish(joint_state_msg)
             case _:
@@ -581,6 +629,10 @@ def map_range(
     return (value - in_min) * (out_max - out_min) / (in_max - in_min) + out_min
 
 
+def radps_to_rpm(radps: float):
+    return radps * 60 / (2 * pi)
+
+
 def main(args=None):
     rclpy.init(args=args)
     sys.excepthook = myexcepthook

src/headless_pkg/src/headless_node.py

@@ -35,14 +35,14 @@ ARM_STOP_MSG = ArmManual()  # "
 BIO_STOP_MSG = BioControl()  # "
 
 control_qos = qos.QoSProfile(
-    history=qos.QoSHistoryPolicy.KEEP_LAST,
+    # history=qos.QoSHistoryPolicy.KEEP_LAST,
     depth=2,
-    reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
+    # reliability=qos.QoSReliabilityPolicy.BEST_EFFORT,
-    durability=qos.QoSDurabilityPolicy.VOLATILE,
+    # durability=qos.QoSDurabilityPolicy.VOLATILE,
-    deadline=Duration(seconds=1),
+    # deadline=Duration(seconds=1),
-    lifespan=Duration(nanoseconds=500_000_000),  # 500ms
+    # lifespan=Duration(nanoseconds=500_000_000),  # 500ms
-    liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
+    # liveliness=qos.QoSLivelinessPolicy.SYSTEM_DEFAULT,
-    liveliness_lease_duration=Duration(seconds=5),
+    # liveliness_lease_duration=Duration(seconds=5),
 )
 
 CORE_MODE = "twist"  # "twist" or "duty"