修改校准配置文件

src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml

@@ -16,14 +16,14 @@ joint_sensor_node:
     - 1.3744467859455343
     right_sensor_directions:
     - -1.0
-    - 1.0
+    - -1.0
     - -1.0
     - 1.0
     - -1.0
     - 1.0
     - -1.0
     - 1.0
-    right_auto_zero_on_first_sample: false
+    right_auto_zero_on_first_sample: true
     left_zero_offsets:
     - 2.492718780314167
     - -0.7577865092155065
@@ -35,14 +35,14 @@ joint_sensor_node:
     - 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: false
+    - -1.0
+    left_auto_zero_on_first_sample: true
     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

@@ -537,10 +537,6 @@ class JointSensorNode(Node):
         if len(data) < 2:
             return None
 
-        # 通用格式：前2字节角度，默认 centi-rad
-        # raw = int.from_bytes(data[0:2], byteorder='big', signed=True)
-        # return self.raw_to_rad(raw)
-
     def wrap_to_pi(self, angle_rad: float) -> float:
         # 归一化到 [-pi, pi)，解决 0~2pi 到 ±pi 的过零映射问题
         return (angle_rad + math.pi) % (2.0 * math.pi) - math.pi
@@ -562,12 +558,6 @@ class JointSensorNode(Node):
 
         return prev + delta
 
-    def raw_to_rad(self, raw_value: int) -> float:
-        if self.raw_angle_unit == 'centi-deg':
-            return math.radians(raw_value / 100.0)
-        # 默认按 centi-rad 处理，适配你当前 0~6.28 弧度输出
-        return raw_value / 100.0
-
     def parse_sensor_from_can(self, data: bytes):
         if len(data) == 0:
             return 0.0, 0.0
@@ -587,14 +577,6 @@ class JointSensorNode(Node):
         if len(data) < 8:
             return 0.0, 0.0
 
-        # # 通用格式：前2字节为角度，后2字节为速度，默认按 centi-rad 解析
-        # raw_deg = int.from_bytes(data[0:2], byteorder='big', signed=True)
-        # rad = self.wrap_to_pi(self.raw_to_rad(raw_deg))
-
-        # raw_vel = int.from_bytes(data[2:4], byteorder='big', signed=True)
-        # vel_rad = self.raw_to_rad(raw_vel)
-
-        # return rad, vel_rad
         
 
 def main():