feat(core): add code support for testbed

Adds parameter `is_testbed`. WIP: still needs support in astra_descriptions; new parameter will not be usable until then. When using an incorrect parameter value, odom will be incorrect and will drive at a scaled speed from requested over cmd_vel.

src/anchor_pkg/launch/rover.launch.py

@@ -51,6 +51,7 @@ def launch_setup(context, *args, **kwargs):
                 parameters=[
                     {"launch_mode": mode},
                     {"use_ros2_control": LaunchConfiguration("use_ros2_control", default=False)},
+                    {"is_testbed", LaunchConfiguration("is_testbed", default=False)},
                 ],
                 on_exit=Shutdown(),
             )
@@ -151,6 +152,12 @@ def generate_launch_description():
         description="Whether to use DiffDriveController for driving instead of direct Twist",
     )
 
+    testbed_arg = DeclareLaunchArgument(
+        "is_testbed",
+        default_value="false",
+        description="Whether using Testbed (true) or Clucky (false)",
+    )
+
     rsp = IncludeLaunchDescription(
         PythonLaunchDescriptionSource(
             PathJoinSubstitution(
@@ -183,6 +190,7 @@ def generate_launch_description():
         [
             declare_arg,
             ros2_control_arg,
+            testbed_arg,
             rsp,
             controllers,
             OpaqueFunction(function=launch_setup),

src/core_pkg/core_pkg/core_node.py

@@ -19,7 +19,12 @@ from astra_msgs.msg import VicCAN, NewCoreFeedback, Barometer, CoreCtrlState
 
 CORE_WHEELBASE = 0.836  # meters
 CORE_WHEEL_RADIUS = 0.171  # meters
-CORE_GEAR_RATIO = 100.0  # Clucky: 100:1, Testbed: 64:1
+CORE_GEAR_RATIO = 100.0  # Clucky: 100:1
+
+# TODO: update core_description or add testbed_description
+TESTBED_WHEELBASE = 0.368  # meters
+TESTBED_WHEEL_RADIUS = 0.108  # meters
+TESTBED_GEAR_RATIO = 64  # Testbed: 64:1
 
 control_qos = qos.QoSProfile(
     history=qos.QoSHistoryPolicy.KEEP_LAST,
@@ -63,6 +68,22 @@ class CoreNode(Node):
             self.get_parameter("use_ros2_control").get_parameter_value().bool_value
         )
 
+        self.declare_parameter("is_testbed", False)
+        self.is_testbed = (
+            self.get_parameter("is_testbed").get_parameter_value().bool_value
+        )
+
+        if self.is_testbed:
+            global TESTBED_WHEELBASE, TESTBED_WHEEL_RADIUS, TESTBED_GEAR_RATIO
+            self.wheelbase = TESTBED_WHEELBASE
+            self.wheel_radius = TESTBED_WHEEL_RADIUS
+            self.gear_ratio = TESTBED_GEAR_RATIO
+        else:
+            global CORE_WHEELBASE, CORE_WHEEL_RADIUS, CORE_GEAR_RATIO
+            self.wheelbase = CORE_WHEELBASE
+            self.wheel_radius = CORE_WHEEL_RADIUS
+            self.gear_ratio = CORE_GEAR_RATIO
+
         ##################################################
         # Old Topics
 
@@ -80,9 +101,7 @@ class CoreNode(Node):
         self.feedback_pub = self.create_publisher(CoreFeedback, "/core/feedback", 10)
         self.core_feedback = CoreFeedback()
 
-        self.telemetry_pub_timer = self.create_timer(
+        self.telemetry_pub_timer = self.create_timer(1.0, self.publish_feedback)
-            1.0, self.publish_feedback
-        )
 
         ##################################################
         # New Topics
@@ -104,10 +123,6 @@ class CoreNode(Node):
                 JointState, "/core/joint_commands", self.joint_command_callback, 2
             )
         else:
-            # autonomy twist -- m/s and rad/s -- for autonomy, in particular Nav2
-            self.cmd_vel_sub_ = self.create_subscription(
-                TwistStamped, "/cmd_vel", self.cmd_vel_callback, 1
-            )
             # manual twist -- [-1, 1] rather than real units
             # TODO: change topic to '/core/control/twist'
             self.twist_man_sub_ = self.create_subscription(
@@ -226,13 +241,16 @@ class CoreNode(Node):
         #  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:
+        if (
+            len(msg.name) != (4 if self.is_testbed else 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 != [
+        if msg.name[-4:] != [  # type: ignore
-            "left_suspension_joint",
             "bl_wheel_joint",
             "br_wheel_joint",
             "fl_wheel_joint",
@@ -245,27 +263,15 @@ class CoreNode(Node):
 
         (bl_vel, br_vel, fl_vel, fr_vel) = msg.velocity
 
-        bl_rpm = radps_to_rpm(bl_vel) * CORE_GEAR_RATIO
+        bl_rpm = radps_to_rpm(bl_vel) * self.gear_ratio
-        br_rpm = radps_to_rpm(br_vel) * CORE_GEAR_RATIO
+        br_rpm = radps_to_rpm(br_vel) * self.gear_ratio
-        fl_rpm = radps_to_rpm(fl_vel) * CORE_GEAR_RATIO
+        fl_rpm = radps_to_rpm(fl_vel) * self.gear_ratio
-        fr_rpm = radps_to_rpm(fr_vel) * CORE_GEAR_RATIO
+        fr_rpm = radps_to_rpm(fr_vel) * self.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
-
-        vel_left_rads = (linear - (angular * CORE_WHEELBASE / 2)) / CORE_WHEEL_RADIUS
-        vel_right_rads = (linear + (angular * CORE_WHEELBASE / 2)) / CORE_WHEEL_RADIUS
-
-        vel_left_rpm = round((vel_left_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
-        vel_right_rpm = round((vel_right_rads * 60) / (2 * 3.14159)) * CORE_GEAR_RATIO
-
-        self.send_viccan(20, [vel_left_rpm, vel_right_rpm])
-
     def twist_man_callback(self, msg: Twist):
         linear = msg.linear.x  # [-1 1] for forward/back from left stick y
         angular = msg.angular.z  # [-1 1] for left/right from right stick x
@@ -483,14 +489,12 @@ class CoreNode(Node):
                 motorId = round(float(msg.data[0]))
                 position = float(msg.data[1])
                 velocity = float(msg.data[2])
-                joint_state_msg = (
+                joint_state_msg = JointState()
-                    JointState()
-                )
                 joint_state_msg.position = [
-                    position * (2 * pi) / CORE_GEAR_RATIO
+                    position * (2 * pi) / self.gear_ratio
                 ]  # revolutions to radians
                 joint_state_msg.velocity = [
-                    velocity * (2 * pi / 60.0) / CORE_GEAR_RATIO
+                    velocity * (2 * pi / 60.0) / self.gear_ratio
                 ]  # RPM to rad/s
 
                 motor: RevMotorState | None = None
@@ -519,9 +523,10 @@ class CoreNode(Node):
                     motor.velocity = velocity
 
                 # make the fucking shit work
-                joint_state_msg.name.append("left_suspension_joint")
+                if not self.is_testbed:
-                joint_state_msg.position.append(0.0)
+                    joint_state_msg.name.append("left_suspension_joint")
-                joint_state_msg.velocity.append(0.0)
+                    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)