feat: add head control and joint data decoder

tg3_local_teleop/test_session_gate.py

@@ -28,7 +28,7 @@ for package, names in {
     "brainco_hand_msgs.msg": ("MotorStatus", "SetMotorMulti"),
     "diagnostic_msgs.msg": ("DiagnosticStatus",),
     "geometry_msgs.msg": ("TwistStamped",),
-    "ros2_bridge_msgs.msg": ("ArmStatus",),
+    "ros2_bridge_msgs.msg": ("ArmStatus", "HeadCtrl", "MotorCtrl", "RobotState"),
     "sensor_msgs.msg": ("JointState",),
     "std_msgs.msg": ("String",),
     "std_srvs.srv": ("Trigger",),
@@ -83,6 +83,7 @@ class RobotSessionGateTest(unittest.TestCase):
         bridge.hands_enabled = False
         bridge.right_point_gesture_enabled = False
         bridge.right_a_pose_enabled = False
+        bridge.head_enabled = False
         bridge.robot_arm_positions = [0.0] * 14
         bridge.last_command = None
         bridge.last_publish_at = 0.0
@@ -307,6 +308,22 @@ class RobotSessionGateTest(unittest.TestCase):
         bridge._tick_locomotion(1.3, turning)
         self.assertEqual(published[-1], (0.0, -0.8))
 
+        head_mode_with_sideways_error = ArmSnapshot(
+            {
+                "button": {
+                    "left": [False, False, True],
+                    "right": [False, False, False],
+                },
+                "joystick": {
+                    "left": [0.0, 0.0],
+                    "right": [0.8, 1.0],
+                },
+            },
+            received_at=1.35,
+        )
+        bridge._tick_locomotion(1.35, head_mode_with_sideways_error)
+        self.assertEqual(published[-1], (0.0, 0.0))
+
         old_wrong_binding = ArmSnapshot(
             {
                 "button": {
@@ -351,11 +368,17 @@ class RobotSessionGateTest(unittest.TestCase):
 
         bridge._publish_walk = MethodType(publish, bridge)
         deflected = ArmSnapshot(
-            {"joystick": {"left": [1.0, 0.0], "right": [0.0, 0.0]}},
+            {
+                "button": {"left": [0, 0, 0]},
+                "joystick": {"left": [1.0, 0.0], "right": [0.0, 0.0]},
+            },
             received_at=2.0,
         )
         neutral = ArmSnapshot(
-            {"joystick": {"left": [0.0, 0.0], "right": [0.0, 0.0]}},
+            {
+                "button": {"left": [0, 0, 0]},
+                "joystick": {"left": [0.0, 0.0], "right": [0.0, 0.0]},
+            },
             received_at=2.1,
         )
         bridge._tick_locomotion(2.0, deflected)