修复丢弃数据解析的bug

src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml

@@ -6,43 +6,43 @@ joint_sensor_node:
     right_joint_states_topic: /right_joint_sensor_states
     left_joint_states_topic: /left_joint_sensor_states
     right_zero_offsets:
-    - 2.7995149378912956
+    - 0.023009711818284373
-    - 2.5233983960718795
+    - -2.304039143404233
-    - 0.0
+    - 1.9236119080085938
-    - 0.0
+    - 0.09357282806102418
-    - 0.0
+    - 1.6321555583103224
-    - 0.0
+    - -0.9848156658225804
-    - 0.0
+    - -2.06934008285773
-    - 0.0
+    - 1.3744467859455343
     right_sensor_directions:
     - -1.0
     - 1.0
     - -1.0
-    - -1.0
-    - -1.0
-    - 1.0
     - 1.0
     - -1.0
-    right_auto_zero_on_first_sample: true
+    - 1.0
+    - -1.0
+    - 1.0
+    right_auto_zero_on_first_sample: false
     left_zero_offsets:
-    - -1.1550875332778876
+    - 2.492718780314167
-    - 0.7838641826095625
+    - -0.7577865092155065
-    - -2.710544052193928
+    - -0.09817477042468115
-    - 0.12118448224296552
+    - -1.7257283863713466
-    - 3.0265440944983695
+    - 1.7119225592803753
-    - 2.2196702000705226
+    - -1.5201749607946704
-    - 1.0921943209745777
+    - -3.100175172316881
-    - 1.3729128051576485
+    - 1.2977477465512521
     left_sensor_directions:
     - -1.0
     - 1.0
     - -1.0
-    - 1.0
+    - -1.0
     - -1.0
     - 1.0
-    - -1.0
     - 1.0
-    left_auto_zero_on_first_sample: true
+    - -1.0
+    left_auto_zero_on_first_sample: false
     right_calibrate_now: false
     left_calibrate_now: false
     calibration_yaml_path: /home/shen/openarm_test/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml

src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py

@@ -505,14 +505,19 @@ class JointSensorNode(Node):
             return False
 
         since_last = now_sec - state['last_raw_reject_log_sec'][joint_index]
+        raw_data = (msg.data[1] << 8) | msg.data[2]
+        deg = (raw_data / 4096.0) * 360.0
+        rad = math.radians(deg)
+        rad = self.wrap_to_pi(rad)
         if since_last >= self.raw_jump_log_min_interval_sec:
             self.get_logger().warning(
                 f"[{self.arm_labels[arm_name]}] Joint{joint_index + 1} RAW frame dropped: prev_raw={prev:.6f} curr_raw={raw_sensor_angle:.6f} "
                 f"delta={delta:.6f} rad threshold={self.raw_jump_reject_threshold_rad:.6f} "
                 f"can_id=0x{msg.arbitration_id:X} data={msg.data.hex()} "
+                f"angle={rad:.6f} rad"
             )
             state['last_raw_reject_log_sec'][joint_index] = now_sec
-
+        state['prev_raw_sensor_angle'][joint_index] = raw_sensor_angle
         return True
 
     def extract_raw_sensor_angle_rad(self, data: bytes):
@@ -524,14 +529,17 @@ class JointSensorNode(Node):
             if len(data) < 3:
                 return None
             raw = (data[1] << 8) | data[2]
-            return (raw / 4096.0) * (2.0 * math.pi)
+            deg = (raw / 4096.0) * 360.0
+            rad = math.radians(deg)
+            rad = self.wrap_to_pi(rad)
+            return rad
 
         if len(data) < 2:
             return None
 
         # 通用格式：前2字节角度，默认 centi-rad
-        raw = int.from_bytes(data[0:2], byteorder='big', signed=True)
+        # raw = int.from_bytes(data[0:2], byteorder='big', signed=True)
-        return self.raw_to_rad(raw)
+        # return self.raw_to_rad(raw)
 
     def wrap_to_pi(self, angle_rad: float) -> float:
         # 归一化到 [-pi, pi)，解决 0~2pi 到 ±pi 的过零映射问题
@@ -566,7 +574,7 @@ class JointSensorNode(Node):
 
         # 特殊格式：0xAA + 16bit角度 + 16bit速度
         if data[0] == 0xAA:
-            if len(data) < 5:
+            if len(data) < 8:
                 return 0.0, 0.0
             raw_deg = (data[1] << 8) | data[2]
             raw_vel = (data[3] << 8) | data[4]
@@ -576,17 +584,17 @@ class JointSensorNode(Node):
             vel_rad = math.radians(vel_deg)
             return rad, vel_rad
 
-        if len(data) < 4:
+        if len(data) < 8:
             return 0.0, 0.0
 
-        # 通用格式：前2字节为角度，后2字节为速度，默认按 centi-rad 解析
+        # # 通用格式：前2字节为角度，后2字节为速度，默认按 centi-rad 解析
-        raw_deg = int.from_bytes(data[0:2], byteorder='big', signed=True)
+        # raw_deg = int.from_bytes(data[0:2], byteorder='big', signed=True)
-        rad = self.wrap_to_pi(self.raw_to_rad(raw_deg))
+        # rad = self.wrap_to_pi(self.raw_to_rad(raw_deg))
 
-        raw_vel = int.from_bytes(data[2:4], byteorder='big', signed=True)
+        # raw_vel = int.from_bytes(data[2:4], byteorder='big', signed=True)
-        vel_rad = self.raw_to_rad(raw_vel)
+        # vel_rad = self.raw_to_rad(raw_vel)
 
-        return rad, vel_rad
+        # return rad, vel_rad
         
 
 def main():