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

修改右臂变量名

Browse Source
This commit is contained in:
shenchenyang 2026-04-08 16:54:24 +08:00
parent b8d20e21a3
commit 985b21fb4f
2 changed files with 17 additions and 18 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -1,12 +1,11 @@
joint_sensor_node: joint_sensor_node:
ros__parameters: ros__parameters:
can_interface: can2
right_can_interface: can2 right_can_interface: can2
left_can_interface: can3 left_can_interface: can3
enable_left_arm: true enable_left_arm: true
right_joint_states_topic: /right_joint_sensor_states right_joint_states_topic: /right_joint_sensor_states
left_joint_states_topic: /left_joint_sensor_states left_joint_states_topic: /left_joint_sensor_states
zero_offsets: right_zero_offsets:
- 2.7995149378912956 - 2.7995149378912956
- 2.5233983960718795 - 2.5233983960718795
- 0.0 - 0.0
@ -15,7 +14,7 @@ joint_sensor_node:
- 0.0 - 0.0
- 0.0 - 0.0
- 0.0 - 0.0
sensor_directions: right_sensor_directions:
- -1.0 - -1.0
- 1.0 - 1.0
- -1.0 - -1.0
@ -24,7 +23,7 @@ joint_sensor_node:
- 1.0 - 1.0
- 1.0 - 1.0
- -1.0 - -1.0
auto_zero_on_first_sample: true right_auto_zero_on_first_sample: true
left_zero_offsets: left_zero_offsets:
- -1.1550875332778876 - -1.1550875332778876
- 0.7838641826095625 - 0.7838641826095625
@ -44,7 +43,7 @@ joint_sensor_node:
- -1.0 - -1.0
- 1.0 - 1.0
left_auto_zero_on_first_sample: true left_auto_zero_on_first_sample: true
calibrate_now: false right_calibrate_now: false
left_calibrate_now: false left_calibrate_now: false
calibration_yaml_path: /home/shen/openarm_test/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml calibration_yaml_path: /home/shen/openarm_test/src/openarm_joint_sensor/config/joint_sensor_zero_offsets.yaml
save_calibration_now: false save_calibration_now: false

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

@ -35,13 +35,13 @@ class JointSensorNode(Node):
self.declare_parameter('left_joint_states_topic', 'left_joint_sensor_states') self.declare_parameter('left_joint_states_topic', 'left_joint_sensor_states')
# Calibration and direction parameters # Calibration and direction parameters
self.declare_parameter('zero_offsets', [0.0] * self.num_joints) self.declare_parameter('right_zero_offsets', [0.0] * self.num_joints)
self.declare_parameter('sensor_directions', [1.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_zero_offsets', [0.0] * self.num_joints)
self.declare_parameter('left_sensor_directions', [1.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('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) self.declare_parameter('left_calibrate_now', False)
default_yaml_path = os.path.normpath( default_yaml_path = os.path.normpath(
os.path.join(os.path.dirname(__file__), '..', 'config', 'joint_sensor_zero_offsets.yaml') os.path.join(os.path.dirname(__file__), '..', 'config', 'joint_sensor_zero_offsets.yaml')
@ -105,9 +105,9 @@ class JointSensorNode(Node):
self.arms = { self.arms = {
'right': self._new_arm_state( 'right': self._new_arm_state(
zero_offsets=self._load_offsets_param('zero_offsets'), zero_offsets=self._load_offsets_param('right_zero_offsets'),
sensor_directions=self._load_directions_param('sensor_directions'), sensor_directions=self._load_directions_param('right_sensor_directions'),
auto_zero=bool(self.get_parameter('auto_zero_on_first_sample').value), auto_zero=bool(self.get_parameter('right_auto_zero_on_first_sample').value),
publisher=self.create_publisher(JointState, right_topic, 10), publisher=self.create_publisher(JointState, right_topic, 10),
bus=can.interface.Bus(channel=right_can_interface, bustype='socketcan'), bus=can.interface.Bus(channel=right_can_interface, bustype='socketcan'),
) )
@ -297,8 +297,8 @@ class JointSensorNode(Node):
right = self.arms.get('right') right = self.arms.get('right')
if right is not None: if right is not None:
ros_params['zero_offsets'] = [float(v) for v in right['zero_offsets']] ros_params['right_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_sensor_directions'] = [float(v) for v in right['sensor_directions']]
left = self.arms.get('left') left = self.arms.get('left')
if left is not None: if left is not None:
@ -328,7 +328,7 @@ class JointSensorNode(Node):
if abs(float(v)) < 1e-9: if abs(float(v)) < 1e-9:
return SetParametersResult(successful=False, reason=f'{param.name}[{i}] 不能为0') 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) 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: 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': if param.name == 'raw_jump_reject_threshold_rad':
self.raw_jump_reject_threshold_rad = float(param.value) 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') self.calibrate_zero_from_current('right')
if param.name == 'left_calibrate_now' and bool(param.value): if param.name == 'left_calibrate_now' and bool(param.value):
@ -383,11 +383,11 @@ class JointSensorNode(Node):
self.save_calibration_to_yaml() self.save_calibration_to_yaml()
for param in params: 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']['zero_offsets'] = [float(v) for v in param.value]
self.arms['right']['has_position'] = [False] * self.num_joints 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']['sensor_directions'] = [float(v) for v in param.value]
self.arms['right']['has_position'] = [False] * self.num_joints self.arms['right']['has_position'] = [False] * self.num_joints