openarm/openarm_test
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

增加零点写入yaml文件

Browse Source
This commit is contained in:
shenchenyang 2026-04-08 16:32:01 +08:00
parent 96b00bd0a1
commit b8d20e21a3
3 changed files with 118 additions and 22 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml
View File

@ -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

4
src/openarm_joint_sensor/openarm_joint_sensor/joint_control.py
View File

@ -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,

82
src/openarm_joint_sensor/openarm_joint_sensor/joint_sensor_can.py
View File

@ -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