From 00ba38ce1146d943b9f261f023e34cc02ddadf05 Mon Sep 17 00:00:00 2001
From: dev01 <dev01@example.com>
Date: Sun, 7 Jun 2026 13:34:06 +0800
Subject: [PATCH] feat: add /chassis/feedforward subscription to gimbal_node

Receives angle_velocity from chassis_node and sends 0xA9 feedforward
frame to gimbal MCU for yaw motor feedforward control.
---
 .../gimbal_control_pkg/gimbal_node.py         | 19 +++++++++++++++++--
 1 file changed, 17 insertions(+), 2 deletions(-)

diff --git a/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py b/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py
index 3fbc149..87d764a 100644
--- a/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py
+++ b/workspace/src/gimbal_control_pkg/gimbal_control_pkg/gimbal_node.py
@@ -6,7 +6,7 @@ import serial
 import rclpy
 from rclpy.node import Node
 from geometry_msgs.msg import Twist
-from std_msgs.msg import Float32MultiArray, String
+from std_msgs.msg import Float32, Float32MultiArray, String
 
 CMD_AUTO = 0x01
 CMD_MANUAL = 0x02
@@ -53,12 +53,16 @@ class GimbalNode(Node):
         self._laser_sub = self.create_subscription(
             String, '/laser/command', self._laser_callback, 10
         )
+        self._feedforward_sub = self.create_subscription(
+            Float32, '/chassis/feedforward', self._feedforward_callback, 10
+        )
 
         self._current_mode = 'debug'
 
         self.get_logger().info(
             'GimbalNode ready - subs: /cmd_gimbal, /vision/target, '
-            '/web/gimbal_pid_config, /gimbal/mode, /laser/command'
+            '/web/gimbal_pid_config, /gimbal/mode, /laser/command, '
+            '/chassis/feedforward'
         )
 
     def _write_frame(self, data: bytearray):
@@ -169,6 +173,17 @@ class GimbalNode(Node):
         data.append(_checksum(data))
         self._write_frame(data)
 
+    def _feedforward_callback(self, msg: Float32):
+        angle_vel = float(msg.data)
+        self.get_logger().debug(
+            f'Feedforward: angle_vel={angle_vel:.4f} deg/s'
+        )
+        data = bytearray()
+        data.append(0xA9)
+        data.extend(struct.pack('<f', angle_vel))
+        data.append(_checksum(data))
+        self._write_frame(data)
+
     def destroy_node(self):
         if self._ser and self._ser.is_open:
             self._ser.close()