fix: 修改 RealManAdapter 中对机械臂的引用,确保正确获取关节状态

This commit is contained in:
2026-08-05 15:59:06 +08:00
parent 43699a81f1
commit bb672e3f39
2 changed files with 9 additions and 8 deletions
+1 -1
View File
@@ -288,7 +288,7 @@ test: 添加 xxx 测试
## 项目专属规则 ## 项目专属规则
* 本项目面向 Ubuntu 22.04 和 ROS2 Humble构建、测试和运行命令应在工作空间根目录 `/home/robot/WS_xr` 执行,并先 `source /opt/ros/humble/setup.bash` * 本项目面向 Ubuntu 22.04 和 ROS2 Humble编译前必须先切换到工作空间根目录 `/home/robot/WS_xr`,并执行 `source /opt/ros/humble/setup.bash`禁止在 `/home/robot/WS_xr/src` 中运行 `colcon build`,否则会在源码目录生成多余的 `build/``install/``log/`;测试和运行命令也应在工作空间根目录执行。
* 工作空间包含 `xr_rm_input``xr_rm_teleop` 两个 `ament_python` 包,以及 `xr_rm_interfaces``xr_rm_bringup` 两个 `ament_cmake` 包;优先使用现有 ROS2 包、节点和消息,不要另建重复入口。 * 工作空间包含 `xr_rm_input``xr_rm_teleop` 两个 `ament_python` 包,以及 `xr_rm_interfaces``xr_rm_bringup` 两个 `ament_cmake` 包;优先使用现有 ROS2 包、节点和消息,不要另建重复入口。
* 修改 ROS 节点、launch、消息定义或安装配置后,至少运行 `colcon build --symlink-install`;涉及遥操作姿态控制时,再运行 `pytest src/xr_rm_teleop/test/test_orientation_control.py` * 修改 ROS 节点、launch、消息定义或安装配置后,至少运行 `colcon build --symlink-install`;涉及遥操作姿态控制时,再运行 `pytest src/xr_rm_teleop/test/test_orientation_control.py`
* 遥操作统一使用 `xr_rm_bringup/launch/arm_debug.launch.py`;调试和验证默认使用 `use_mock:=true`,未经用户明确要求不得连接真机、移动机械臂或操作夹爪。 * 遥操作统一使用 `xr_rm_bringup/launch/arm_debug.launch.py`;调试和验证默认使用 `use_mock:=true`,未经用户明确要求不得连接真机、移动机械臂或操作夹爪。
+8 -7
View File
@@ -237,9 +237,9 @@ class RealManAdapter:
) )
def read_joint_state(self) -> JointStateSnapshot: def read_joint_state(self) -> JointStateSnapshot:
self._require_arm() arm = self._require_arm()
started_at = time.monotonic() started_at = time.monotonic()
result = self._arm.rm_get_joint_degree() result = arm.rm_get_joint_degree()
finished_at = time.monotonic() finished_at = time.monotonic()
if not isinstance(result, tuple) or len(result) != 2: if not isinstance(result, tuple) or len(result) != 2:
raise RuntimeError( raise RuntimeError(
@@ -256,10 +256,10 @@ class RealManAdapter:
) )
def send_joint_target(self, joints: list[float], follow: bool) -> None: def send_joint_target(self, joints: list[float], follow: bool) -> None:
self._require_arm() arm = self._require_arm()
if len(joints) != 7 or not all(math.isfinite(value) for value in joints): if len(joints) != 7 or not all(math.isfinite(value) for value in joints):
raise ValueError("joint target must contain 7 finite values") raise ValueError("joint target must contain 7 finite values")
ret = self._arm.rm_movej_canfd( ret = arm.rm_movej_canfd(
[math.degrees(value) for value in joints], [math.degrees(value) for value in joints],
follow, follow,
0, 0,
@@ -315,9 +315,10 @@ class RealManAdapter:
self._arm = None self._arm = None
self._realtime_callback = None self._realtime_callback = None
def _require_arm(self) -> None: def _require_arm(self) -> Any:
if self._arm is None: if self._arm is None:
raise RuntimeError("睿尔曼机械臂尚未连接") raise RuntimeError("睿尔曼机械臂尚未连接")
return self._arm
def _on_realtime_arm_state(self, data: Any) -> None: def _on_realtime_arm_state(self, data: Any) -> None:
if not self._accept_realtime_feedback: if not self._accept_realtime_feedback:
@@ -457,11 +458,11 @@ class RealManAdapter:
self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc) self._try_call("rm_set_joint_max_acc", joint_index, self._joint_max_acc)
def move_to_initial_pose(self) -> None: def move_to_initial_pose(self) -> None:
self._require_arm() arm = self._require_arm()
if self._initial_joint_pose is None: if self._initial_joint_pose is None:
raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose") raise RuntimeError("启用初始位姿移动时必须配置 initial_joint_pose")
ret = self._arm.rm_movej( ret = arm.rm_movej(
self._initial_joint_pose, self._initial_joint_pose,
self._init_move_speed, self._init_move_speed,
0, 0,