From 985b21fb4f98cc6d1551b9bf025368de6320a536 Mon Sep 17 00:00:00 2001 From: shenchenyang <664376944@qq.com> Date: Wed, 8 Apr 2026 16:54:24 +0800 Subject: [PATCH] =?UTF-8?q?=E4=BF=AE=E6=94=B9=E5=8F=B3=E8=87=82=E5=8F=98?= =?UTF-8?q?=E9=87=8F=E5=90=8D?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/joint_sensor_zero_offsets.yaml | 9 +++---- .../openarm_joint_sensor/joint_sensor_can.py | 26 +++++++++---------- 2 files changed, 17 insertions(+), 18 deletions(-) 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 6b03d40..64bb58c 100644 --- a/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml +++ b/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml @@ -1,12 +1,11 @@ joint_sensor_node: ros__parameters: - 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_zero_offsets: - 2.7995149378912956 - 2.5233983960718795 - 0.0 @@ -15,7 +14,7 @@ joint_sensor_node: - 0.0 - 0.0 - 0.0 - sensor_directions: + right_sensor_directions: - -1.0 - 1.0 - -1.0 @@ -24,7 +23,7 @@ joint_sensor_node: - 1.0 - 1.0 - -1.0 - auto_zero_on_first_sample: true + right_auto_zero_on_first_sample: true left_zero_offsets: - -1.1550875332778876 - 0.7838641826095625 @@ -44,7 +43,7 @@ joint_sensor_node: - -1.0 - 1.0 left_auto_zero_on_first_sample: true - calibrate_now: 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 save_calibration_now: false 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 c17f025..c132903 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 @@ -35,13 +35,13 @@ class JointSensorNode(Node): self.declare_parameter('left_joint_states_topic', 'left_joint_sensor_states') # Calibration and direction parameters - self.declare_parameter('zero_offsets', [0.0] * self.num_joints) - self.declare_parameter('sensor_directions', [1.0] * self.num_joints) + self.declare_parameter('right_zero_offsets', [0.0] * self.num_joints) + self.declare_parameter('right_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', True) + self.declare_parameter('right_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('right_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') @@ -105,9 +105,9 @@ class JointSensorNode(Node): self.arms = { 'right': self._new_arm_state( - zero_offsets=self._load_offsets_param('zero_offsets'), - sensor_directions=self._load_directions_param('sensor_directions'), - auto_zero=bool(self.get_parameter('auto_zero_on_first_sample').value), + zero_offsets=self._load_offsets_param('right_zero_offsets'), + sensor_directions=self._load_directions_param('right_sensor_directions'), + auto_zero=bool(self.get_parameter('right_auto_zero_on_first_sample').value), publisher=self.create_publisher(JointState, right_topic, 10), bus=can.interface.Bus(channel=right_can_interface, bustype='socketcan'), ) @@ -297,8 +297,8 @@ class JointSensorNode(Node): 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']] + ros_params['right_zero_offsets'] = [float(v) for v in right['zero_offsets']] + ros_params['right_sensor_directions'] = [float(v) for v in right['sensor_directions']] left = self.arms.get('left') if left is not None: @@ -328,7 +328,7 @@ class JointSensorNode(Node): if abs(float(v)) < 1e-9: return SetParametersResult(successful=False, reason=f'{param.name}[{i}] 不能为0') - if param.name == 'auto_zero_on_first_sample': + if param.name == 'right_auto_zero_on_first_sample': self.arms['right']['auto_zero_on_first_sample'] = bool(param.value) if param.name == 'left_auto_zero_on_first_sample' and 'left' in self.arms: @@ -373,7 +373,7 @@ class JointSensorNode(Node): if param.name == 'raw_jump_reject_threshold_rad': self.raw_jump_reject_threshold_rad = float(param.value) - if param.name == 'calibrate_now' and bool(param.value): + if param.name == 'right_calibrate_now' and bool(param.value): self.calibrate_zero_from_current('right') if param.name == 'left_calibrate_now' and bool(param.value): @@ -383,11 +383,11 @@ class JointSensorNode(Node): self.save_calibration_to_yaml() for param in params: - if param.name == 'zero_offsets': + if param.name == 'right_zero_offsets': self.arms['right']['zero_offsets'] = [float(v) for v in param.value] self.arms['right']['has_position'] = [False] * self.num_joints - if param.name == 'sensor_directions': + if param.name == 'right_sensor_directions': self.arms['right']['sensor_directions'] = [float(v) for v in param.value] self.arms['right']['has_position'] = [False] * self.num_joints