增加零点写入yaml文件

src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml

@@ -1,34 +1,60 @@
 joint_sensor_node:
   ros__parameters:
-    # CAN and topic settings (dual-arm)
     can_interface: can2
     right_can_interface: can2
     left_can_interface: can3
     enable_left_arm: true
     right_joint_states_topic: /right_joint_sensor_states
     left_joint_states_topic: /left_joint_sensor_states
-
+    zero_offsets:
-    # Right arm calibration
+    - 2.7995149378912956
-    zero_offsets: [2.7995149378912956, 2.5233983960718795, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+    - 2.5233983960718795
-    sensor_directions: [-1.0, 1.0, -1.0, -1.0, -1.0, 1.0, 1.0, -1.0]
+    - 0.0
+    - 0.0
+    - 0.0
+    - 0.0
+    - 0.0
+    - 0.0
+    sensor_directions:
+    - -1.0
+    - 1.0
+    - -1.0
+    - -1.0
+    - -1.0
+    - 1.0
+    - 1.0
+    - -1.0
     auto_zero_on_first_sample: true
-
+    left_zero_offsets:
-    # Left arm calibration
+    - -1.1550875332778876
-    left_zero_offsets: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
+    - 0.7838641826095625
-    left_sensor_directions: [-1.0, 1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0]
+    - -2.710544052193928
+    - 0.12118448224296552
+    - 3.0265440944983695
+    - 2.2196702000705226
+    - 1.0921943209745777
+    - 1.3729128051576485
+    left_sensor_directions:
+    - -1.0
+    - 1.0
+    - -1.0
+    - 1.0
+    - -1.0
+    - 1.0
+    - -1.0
+    - 1.0
     left_auto_zero_on_first_sample: true
-
-    # Runtime calibration triggers
     calibrate_now: false
     left_calibrate_now: false
-
+    calibration_yaml_path: /home/shen/openarm_test/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml
-    # Diagnostics and anomaly filtering
+    save_calibration_now: false
+    auto_save_yaml_on_calibrate: true
     enable_stage_diag_log: false
     stage_log_min_interval_sec: 0.5
     enable_speed_jump_log: false
     speed_jump_threshold_rad_s: 3.0
     jump_log_min_interval_sec: 0.2
-    joint_update_epsilon_rad: 1.0e-4
+    joint_update_epsilon_rad: 0.0001
     enable_raw_jump_log: false
     raw_jump_threshold_rad: 0.2
     raw_jump_log_min_interval_sec: 0.2

src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py

@@ -130,7 +130,7 @@ class ProgressActionClient(Node):
                                              r=self.kalman_r)
                               for _ in range(self.num_joints)]
 
-        self.right_command_queue = deque(maxlen=20)
+        self.right_command_queue = deque(maxlen=10)
 
         # Left arm runtime states
         self.left_filters = [PositionFilter(filter_type=self.filter_type,
@@ -139,7 +139,7 @@ class ProgressActionClient(Node):
                                             r=self.kalman_r)
                              for _ in range(self.num_joints)]
 
-        self.left_command_queue = deque(maxlen=20)
+        self.left_command_queue = deque(maxlen=10)
 
         self.right_subscriber_ = self.create_subscription(
             JointState, 

src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py

@@ -2,6 +2,8 @@ import rclpy
 from rclpy.node import Node
 import can
 import math
+import os
+import yaml
 from sensor_msgs.msg import JointState
 from rcl_interfaces.msg import SetParametersResult
 
@@ -26,7 +28,6 @@ class JointSensorNode(Node):
         self.arm_labels = {'right': 'RIGHT', 'left': 'LEFT'}
 
         # CAN interface and topic parameters
-        self.declare_parameter('can_interface', 'can2')  # backward-compatible alias for right arm
         self.declare_parameter('right_can_interface', 'can2')
         self.declare_parameter('left_can_interface', 'can3')
         self.declare_parameter('enable_left_arm', True)
@@ -38,10 +39,18 @@ class JointSensorNode(Node):
         self.declare_parameter('sensor_directions', [1.0] * self.num_joints)
         self.declare_parameter('left_zero_offsets', [0.0] * self.num_joints)
         self.declare_parameter('left_sensor_directions', [1.0] * self.num_joints)
-        self.declare_parameter('auto_zero_on_first_sample', False)
+        self.declare_parameter('auto_zero_on_first_sample', True)
-        self.declare_parameter('left_auto_zero_on_first_sample', False)
+        self.declare_parameter('left_auto_zero_on_first_sample', True)
         self.declare_parameter('calibrate_now', False)
         self.declare_parameter('left_calibrate_now', False)
+        default_yaml_path = os.path.normpath(
+            os.path.join(os.path.dirname(__file__), '..', 'config', 'joint_sensor_zero_offsets.yaml')
+        )
+        if not os.path.exists(default_yaml_path):
+            default_yaml_path = ''
+        self.declare_parameter('calibration_yaml_path', default_yaml_path)
+        self.declare_parameter('save_calibration_now', False)
+        self.declare_parameter('auto_save_yaml_on_calibrate', True)
 
         # Diagnostics and anomaly filters
         self.declare_parameter('enable_speed_jump_log', False)
@@ -54,7 +63,7 @@ class JointSensorNode(Node):
         self.declare_parameter('raw_jump_threshold_rad', 0.2)
         self.declare_parameter('raw_jump_log_min_interval_sec', 0.2)
         self.declare_parameter('enable_raw_jump_reject', True)
-        self.declare_parameter('raw_jump_reject_threshold_rad', 0.35)
+        self.declare_parameter('raw_jump_reject_threshold_rad', 0.40)
 
         # Runtime flags
         self.enable_left_arm = bool(self.get_parameter('enable_left_arm').value)
@@ -69,14 +78,15 @@ class JointSensorNode(Node):
         self.raw_jump_log_min_interval_sec = float(self.get_parameter('raw_jump_log_min_interval_sec').value)
         self.enable_raw_jump_reject = bool(self.get_parameter('enable_raw_jump_reject').value)
         self.raw_jump_reject_threshold_rad = float(self.get_parameter('raw_jump_reject_threshold_rad').value)
+        self.calibration_yaml_path = str(self.get_parameter('calibration_yaml_path').value)
+        self.auto_save_yaml_on_calibrate = bool(self.get_parameter('auto_save_yaml_on_calibrate').value)
 
         # Sensor unit setting
         self.raw_angle_unit = 'centi-rad'
 
         self.get_logger().info('正在初始化CAN接口...')
 
-        legacy_can = str(self.get_parameter('can_interface').value)
+        right_can_interface = str(self.get_parameter('right_can_interface').value)
-        right_can_interface = str(self.get_parameter('right_can_interface').value or legacy_can)
         left_can_interface = str(self.get_parameter('left_can_interface').value)
         right_topic = str(self.get_parameter('right_joint_states_topic').value)
         left_topic = str(self.get_parameter('left_joint_states_topic').value)
@@ -256,9 +266,55 @@ class JointSensorNode(Node):
                 )
             ])
             self.get_logger().info(f'[{self.arm_labels[arm_name]}] 零点校准完成，已更新 {updated}/{self.num_joints} 个关节零偏。')
+            if self.auto_save_yaml_on_calibrate:
+                self.save_calibration_to_yaml()
         else:
             self.get_logger().warn('零点校准失败：当前还没有可用的关节采样数据。')
 
+    def save_calibration_to_yaml(self):
+        yaml_path = self.calibration_yaml_path
+        if not yaml_path:
+            self.get_logger().warn('未配置 calibration_yaml_path，无法写回 YAML。')
+            return False
+
+        try:
+            data = {}
+            if os.path.exists(yaml_path):
+                with open(yaml_path, 'r', encoding='utf-8') as f:
+                    loaded = yaml.safe_load(f)
+                    if isinstance(loaded, dict):
+                        data = loaded
+
+            node_cfg = data.get('joint_sensor_node')
+            if not isinstance(node_cfg, dict):
+                node_cfg = {}
+                data['joint_sensor_node'] = node_cfg
+
+            ros_params = node_cfg.get('ros__parameters')
+            if not isinstance(ros_params, dict):
+                ros_params = {}
+                node_cfg['ros__parameters'] = ros_params
+
+            right = self.arms.get('right')
+            if right is not None:
+                ros_params['zero_offsets'] = [float(v) for v in right['zero_offsets']]
+                ros_params['sensor_directions'] = [float(v) for v in right['sensor_directions']]
+
+            left = self.arms.get('left')
+            if left is not None:
+                ros_params['left_zero_offsets'] = [float(v) for v in left['zero_offsets']]
+                ros_params['left_sensor_directions'] = [float(v) for v in left['sensor_directions']]
+
+            os.makedirs(os.path.dirname(yaml_path), exist_ok=True)
+            with open(yaml_path, 'w', encoding='utf-8') as f:
+                yaml.safe_dump(data, f, allow_unicode=False, sort_keys=False)
+
+            self.get_logger().info(f'已将当前校准值写回 YAML: {yaml_path}')
+            return True
+        except Exception as e:
+            self.get_logger().error(f'写回 YAML 失败: {e}')
+            return False
+
     def on_set_parameters(self, params):
         for param in params:
             if param.name in ('zero_offsets', 'left_zero_offsets'):
@@ -278,6 +334,12 @@ class JointSensorNode(Node):
             if param.name == 'left_auto_zero_on_first_sample' and 'left' in self.arms:
                 self.arms['left']['auto_zero_on_first_sample'] = bool(param.value)
 
+            if param.name == 'calibration_yaml_path':
+                self.calibration_yaml_path = str(param.value)
+
+            if param.name == 'auto_save_yaml_on_calibrate':
+                self.auto_save_yaml_on_calibrate = bool(param.value)
+
             if param.name == 'enable_speed_jump_log':
                 self.enable_speed_jump_log = bool(param.value)
 
@@ -317,6 +379,9 @@ class JointSensorNode(Node):
             if param.name == 'left_calibrate_now' and bool(param.value):
                 self.calibrate_zero_from_current('left')
 
+            if param.name == 'save_calibration_now' and bool(param.value):
+                self.save_calibration_to_yaml()
+
         for param in params:
             if param.name == 'zero_offsets':
                 self.arms['right']['zero_offsets'] = [float(v) for v in param.value]
@@ -422,6 +487,11 @@ class JointSensorNode(Node):
         if raw_sensor_angle is None:
             return False
 
+        # joint8 不做跳变丢弃过滤，仅更新基线用于后续比较
+        if joint_index == 7:
+            state['prev_raw_sensor_angle'][joint_index] = raw_sensor_angle
+            return False
+
         prev = state['prev_raw_sensor_angle'][joint_index]
         if prev is None:
             state['prev_raw_sensor_angle'][joint_index] = raw_sensor_angle