diff --git a/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml b/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml index 5033465..6b03d40 100644 --- a/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml +++ b/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 - - # Right arm calibration - zero_offsets: [2.7995149378912956, 2.5233983960718795, 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] + zero_offsets: + - 2.7995149378912956 + - 2.5233983960718795 + - 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 arm calibration - left_zero_offsets: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] - left_sensor_directions: [-1.0, 1.0, -1.0, 1.0, -1.0, 1.0, -1.0, 1.0] + left_zero_offsets: + - -1.1550875332778876 + - 0.7838641826095625 + - -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 - - # Diagnostics and anomaly filtering + calibration_yaml_path: /home/shen/openarm_test/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml + 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 diff --git a/src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py b/src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py index 25e20f9..cc793d7 100644 --- a/src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py +++ b/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, diff --git a/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py b/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py index aff4230..c17f025 100644 --- a/src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py +++ b/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('left_auto_zero_on_first_sample', False) + self.declare_parameter('auto_zero_on_first_sample', True) + 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 or legacy_can) + right_can_interface = str(self.get_parameter('right_can_interface').value) 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