add estop function
This commit is contained in:
NuoDaJia
492
docs/MC-S-002_点到点运动（PTP）设计方案4.md
Normal file
492
docs/MC-S-002_点到点运动（PTP）设计方案4.md
Normal file
@@ -0,0 +1,492 @@
|
||||
# MC-S-002\_点到点运动（PTP）设计方案
|
||||
|
||||
# 双臂运动控制设计文档（MoveIt 规划 + Action 下发）
|
||||
|
||||
| 版本 | 日期 | 作者 | 描述 |
|
||||
| --- | --- | --- | --- |
|
||||
| v1.0.0 | 2026/02/25 | Ray | 文档新建 |
|
||||
| v1.0.1 | 2026/02/28 | Ray | 完善对外接口部分 |
|
||||
|
||||
## 1. 概述
|
||||
|
||||
机械臂需要快速、平滑地从一个位置运动到另一个位置。点到点运动（PTP）在关节空间进行规划：基于 **MoveIt 2** 生成轨迹，并通过左右臂各自的 `**FollowJointTrajectory**` action 下发到 **ros2\_control** 的 `JointTrajectoryController` 执行。目前支持单臂运动和双臂协同PTP运动类型。
|
||||
|
||||
## 2. 功能需求
|
||||
|
||||
* 在PTP运动规划过程中，系统应生成平滑的运动轨迹，速度曲线，加速度曲线在所有时间点连续，无阶跃。
|
||||
|
||||
* 在PTP运动执行过程中，系统应在轨迹规划失败时返回详细的错误原因，帮助上层应用诊断问题。
|
||||
|
||||
* 在PTP执行过程中，系统应在运动过程中检测到急停或错误时，立即停止运动，返回中断状态。
|
||||
|
||||
* 在PTP执行过程中，系统应在执行运动前检测碰撞风险，发现碰撞风险时拒绝执行，返回碰撞信息。
|
||||
|
||||
* 在PTP运动执行过程中，系统应在执行运动前验证目标位置是否在工作空间内，超出工作空间时返回错误，不执行运动。
|
||||
|
||||
* 在PTP运动规划过程中，系统应在规定时间内完成轨迹规划，确保实时响应，规划时间 ≤ 50ms。
|
||||
|
||||
* 在PTP运动指令规划执行过程中，系统应严格控制实际运动速度，确保不超过设定的速度限制，支持速度比例参数（1%-100%），可动态调整
|
||||
|
||||
* 在PTP运动执行过程中，系统应在运动完成后通过Action Result通知上层应用，包含最终位置和状态信息，运动执行结果：SUCCESS（成功）、FAILED（失败）、CANCELLED（取消）。
|
||||
|
||||
* 在PTP运动规划中，系统应确保机器人到达目标位置时无振荡，到达目标后，位置波动范围 ≤ ±0.05mm。
|
||||
|
||||
|
||||
## 3. 依赖
|
||||
|
||||
* **ROS 2 / rclcpp**
|
||||
|
||||
* **MoveIt 2**：`move_group`、`MoveGroupInterface`
|
||||
|
||||
* **ros2\_control**：`controller_manager`、`JointTrajectoryController`
|
||||
|
||||
* **控制接口**：`FollowJointTrajectory` action
|
||||
|
||||
|
||||
---
|
||||
|
||||
## 4. 内部结构设计（按当前代码）
|
||||
|
||||
### 4.1 总体架构
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
subgraph App[上层应用]
|
||||
A1[DualArm Action Client]
|
||||
A2["Service Client<br/>IK/Enable/ClearError"]
|
||||
A3[Status Subscriber]
|
||||
end
|
||||
|
||||
subgraph RC[robot_control]
|
||||
N[robot_control_node]
|
||||
AM[ActionManager]
|
||||
DA[DualArmAction]
|
||||
RCM[RobotControlManager]
|
||||
AC[ArmControl]
|
||||
KS[KinematicsService]
|
||||
AHS[ArmHardwareService]
|
||||
ASS[ArmStatusService]
|
||||
end
|
||||
|
||||
subgraph MoveIt[MoveIt 2]
|
||||
MGI["MoveGroupInterface<br/>arm_left/arm_right/dual_arm"]
|
||||
RS[RobotState IK/FK]
|
||||
end
|
||||
|
||||
subgraph Ctrl[ros2_control]
|
||||
L[left_arm_controller]
|
||||
R[right_arm_controller]
|
||||
G1[left_arm_gpio_controller]
|
||||
G2[right_arm_gpio_controller]
|
||||
end
|
||||
|
||||
A1 --> DA
|
||||
A2 --> KS
|
||||
A2 --> AHS
|
||||
A3 --> ASS
|
||||
|
||||
N --> AM --> DA
|
||||
DA --> RCM --> AC --> MGI
|
||||
KS --> RCM --> AC --> RS
|
||||
AHS --> G1
|
||||
AHS --> G2
|
||||
ASS --> RCM
|
||||
DA --> L
|
||||
DA --> R
|
||||
```
|
||||
|
||||
### 4.2 分层职责
|
||||
|
||||
#### `robot_control_node`（编排层）
|
||||
|
||||
- 负责节点生命周期、订阅/发布器/service/action server 的创建与绑定。
|
||||
- 保持“薄节点”原则，不承载具体业务算法。
|
||||
- 启动时激活左右臂控制器并初始化 MoveIt。
|
||||
|
||||
#### `ActionManager` + `DualArmAction`（PTP主链路）
|
||||
|
||||
- 对外提供 `DualArm` action。
|
||||
- 负责单臂/双臂 PTP 目标校验、规划调用、轨迹导出、拆分与并发下发。
|
||||
- 负责执行期反馈、取消处理和结果回传。
|
||||
|
||||
#### `RobotControlManager`（领域能力聚合）
|
||||
|
||||
- 聚合运动学、状态映射、控制器状态等核心能力。
|
||||
- 提供 `PlanArmMotion` / `PlanDualArmJointMotion`、轨迹导出、关节状态查询等统一接口。
|
||||
- 提供 IK 与 FK 路径：
|
||||
- IK：调用 `ArmControl::SolveInverseKinematics`（纯 IK，`setFromIK`）
|
||||
- FK：使用当前 `jointPositions_` 计算末端位姿。
|
||||
|
||||
#### `ArmControl`（MoveIt 适配层）
|
||||
|
||||
- 管理 `arm_left` / `arm_right` / `dual_arm` 的 `MoveGroupInterface`。
|
||||
- 轨迹规划、时间参数化、轨迹缓存导出。
|
||||
- 纯 IK 与基于给定关节值的 FK 计算。
|
||||
|
||||
#### 新增服务层（`services/`）
|
||||
|
||||
- `KinematicsService`：对外逆解服务入口，负责请求/响应封装与错误语义。
|
||||
- `ArmHardwareService`：对外使能与清错服务，负责 GPIO 命令下发和缓存更新。
|
||||
- `ArmStatusService`：解析 `DynamicJointState`，定时发布 `RobotArmStatus`。
|
||||
|
||||
> 结论：第4版设计从“节点内聚”调整为“节点编排 + 业务服务分层”，便于维护与扩展。
|
||||
|
||||
---
|
||||
|
||||
## 5. 配置项设计
|
||||
|
||||
### 5.1 控制器与MoveIt配置
|
||||
|
||||
- `left_arm_controller`、`right_arm_controller`：6轴 `JointTrajectoryController`。
|
||||
- `controller_manager/switch_controller`：启动时由节点激活。
|
||||
- MoveIt 使用三个 group：`arm_left`、`arm_right`、`dual_arm`。
|
||||
|
||||
### 5.2 关键运行参数
|
||||
|
||||
- `robot_arm_status_period_ms`：状态发布周期，范围 `[10, 20]` ms，默认 `20` ms。
|
||||
- `velocity_scaling`：Action 目标速度比例（兼容 1~100 输入语义）。
|
||||
|
||||
### 5.3 主题与服务约定
|
||||
|
||||
- GPIO命令：
|
||||
- `/left_arm_gpio_controller/commands`
|
||||
- `/right_arm_gpio_controller/commands`
|
||||
- 状态输入：
|
||||
- `/joint_states`
|
||||
- `/dynamic_joint_states`
|
||||
- 状态输出：
|
||||
- `/robot_control/arm_status`
|
||||
- 对外服务：
|
||||
- `/robot_control/inverse_kinematics`
|
||||
- `/robot_control/set_arm_enable`
|
||||
- `/robot_control/clear_arm_error`
|
||||
- `/robot_control/reset_estop`
|
||||
|
||||
---
|
||||
|
||||
## 6. 接口定义（当前实现）
|
||||
|
||||
### 6.1 PTP执行接口（Action）
|
||||
|
||||
- `interfaces/action/DualArm`（当前代码）
|
||||
|
||||
```plaintext
|
||||
# DualArm.action
|
||||
|
||||
# 状态常量定义
|
||||
uint8 STATUS_PLANNING = 0 # 状态: 规划中
|
||||
uint8 STATUS_EXECUTING = 1 # 状态: 执行中
|
||||
uint8 STATUS_DONE = 2 # 状态: 完成
|
||||
|
||||
# Action Goal: 双臂控制目标
|
||||
interfaces/ArmMotionParams[] arm_motion_params # 每条手臂的运动参数 (包含目标位姿、轨迹等)
|
||||
int32 velocity_scaling # 速度百分比系数 [1, 100] (100表示全速)
|
||||
|
||||
---
|
||||
|
||||
# Action Result: 执行结果
|
||||
bool success # 执行是否成功 (true=成功, false=失败)
|
||||
string message # 结果描述信息
|
||||
float64 final_progress # 最终进度 [0.0, 1.0]
|
||||
|
||||
---
|
||||
|
||||
# Action Feedback: 实时反馈
|
||||
float64 progress # 实时总体进度 [0.0, 1.0]
|
||||
uint8 status # 实时状态 (0:规划中, 1:执行中, 2:完成)
|
||||
float64[] joints_left # 左臂当前关节角 [单位: 弧度]
|
||||
float64[] joints_right # 右臂当前关节角 [单位: 弧度]
|
||||
```
|
||||
|
||||
### 6.2 运动学与硬件服务
|
||||
|
||||
- `interfaces/srv/InverseKinematics`（当前代码）
|
||||
|
||||
```plaintext
|
||||
int32 arm_id #0-左臂，1-右臂
|
||||
geometry_msgs/Pose pose #目标位姿
|
||||
---
|
||||
int32 result #结果：0-成功，非0-失败
|
||||
float32[] joint_angles #解算出来的关节角
|
||||
```
|
||||
|
||||
- `interfaces/srv/SetArmEnable`（当前代码）
|
||||
|
||||
```plaintext
|
||||
# SetArmEnable.srv
|
||||
# 机械臂关节上下使能服务
|
||||
#
|
||||
# 用途：
|
||||
# - 对单关节进行使能/失能
|
||||
# - 对单臂全部关节进行使能/失能
|
||||
# - 对双臂全部关节（或同编号关节）进行使能/失能
|
||||
#
|
||||
# 参数组合说明：
|
||||
# - arm_id=0, joint_num=0: 左臂全部关节
|
||||
# - arm_id=1, joint_num=0: 右臂全部关节
|
||||
# - arm_id=2, joint_num=0: 双臂全部关节
|
||||
# - arm_id=0/1, joint_num=1~6: 指定单臂的指定关节
|
||||
# - arm_id=2, joint_num=1~6: 双臂的同编号关节（如左右第3关节）
|
||||
#
|
||||
# 注意：
|
||||
# - joint_num=0 表示“该作用范围内的全部关节”
|
||||
# - 关节编号按 1 开始计数
|
||||
|
||||
int8 arm_id # 机械臂ID: 0=左臂, 1=右臂, 2=双臂
|
||||
int8 joint_num # 关节编号: 1~6=指定关节, 0=全部关节
|
||||
bool enable # 目标状态: true=使能, false=失能
|
||||
---
|
||||
bool success # true=操作成功, false=操作失败
|
||||
string message # 执行结果描述（失败原因/调试信息）
|
||||
```
|
||||
|
||||
- `interfaces/srv/ClearArmError`（当前代码）
|
||||
|
||||
```plaintext
|
||||
# ClearArmError.srv
|
||||
# 清除机械臂错误服务
|
||||
|
||||
int8 arm_id # 机械臂ID (例如: 0=左臂, 1=右臂, 2=双臂)
|
||||
int8 joint_num # 关节编号 (1~7表示第1个到第7个关节, 0表示所有关节)
|
||||
---
|
||||
bool success # 操作是否成功
|
||||
string message # 结果信息
|
||||
```
|
||||
|
||||
- `interfaces/srv/ResetEStop`（当前代码）
|
||||
|
||||
```plaintext
|
||||
# ResetEStop.srv
|
||||
# 急停复位服务（用于清除上层软件锁存态）
|
||||
#
|
||||
# 注意：
|
||||
# - 该服务仅表达“复位请求”，是否允许复位由安全链路策略决定
|
||||
# - 硬件安全回路未恢复时，应返回 success=false
|
||||
|
||||
bool reset # true=请求复位，false=无效请求
|
||||
---
|
||||
bool success # true=复位成功，false=复位失败
|
||||
string message # 结果描述（失败原因/诊断信息）
|
||||
```
|
||||
|
||||
### 6.3 状态发布接口
|
||||
|
||||
- `interfaces/msg/RobotArmStatus`（topic: `/robot_control/arm_status`，当前代码）
|
||||
|
||||
```plaintext
|
||||
builtin_interfaces/Time stamp
|
||||
string[] joint_names
|
||||
float64[] joint_positions
|
||||
bool[] joint_enabled
|
||||
int32[] joint_error_codes
|
||||
bool estop_latched
|
||||
uint32 estop_generation
|
||||
geometry_msgs/Pose left_arm_pose
|
||||
geometry_msgs/Pose right_arm_pose
|
||||
```
|
||||
|
||||
> 末端位姿采用 `jointPositions_ + FK` 计算，确保与同帧关节值一致。
|
||||
|
||||
---
|
||||
|
||||
## 7. 行为与流程设计
|
||||
|
||||
### 7.1 单臂 PTP 主流程
|
||||
|
||||
```mermaid
|
||||
sequenceDiagram
|
||||
participant C as Client
|
||||
participant DA as DualArmAction
|
||||
participant RCM as RobotControlManager
|
||||
participant AC as ArmControl
|
||||
participant FJT as JointTrajectoryController
|
||||
|
||||
C->>DA: Goal(单臂MOVEJ)
|
||||
DA->>DA: 参数/状态校验
|
||||
DA->>RCM: PlanArmMotion()
|
||||
RCM->>AC: PlanJointMotion()
|
||||
AC-->>RCM: 规划轨迹缓存
|
||||
DA->>RCM: ExportArmPlannedTrajectory()
|
||||
DA->>FJT: async_send_goal()
|
||||
FJT-->>DA: result/feedback
|
||||
DA-->>C: result
|
||||
```
|
||||
|
||||
### 7.2 双臂 PTP 主流程
|
||||
|
||||
```mermaid
|
||||
sequenceDiagram
|
||||
participant C as Client
|
||||
participant DA as DualArmAction
|
||||
participant RCM as RobotControlManager
|
||||
participant AC as ArmControl(dual_arm)
|
||||
participant L as left FJT
|
||||
participant R as right FJT
|
||||
|
||||
C->>DA: Goal(双臂)
|
||||
DA->>RCM: PlanDualArmJointMotion()
|
||||
RCM->>AC: PlanJointMotion(arm_id=2)
|
||||
AC-->>RCM: 12轴轨迹
|
||||
DA->>RCM: ExportDualArmPlannedTrajectory()
|
||||
DA->>DA: 拆分左右轨迹
|
||||
par left
|
||||
DA->>L: send goal
|
||||
and right
|
||||
DA->>R: send goal
|
||||
end
|
||||
L-->>DA: result
|
||||
R-->>DA: result
|
||||
DA-->>C: result
|
||||
```
|
||||
|
||||
### 7.3 服务链路流程
|
||||
|
||||
- `InverseKinematics`：`KinematicsService -> RobotControlManager -> ArmControl(setFromIK)`
|
||||
- `SetArmEnable/ClearArmError`：`ArmHardwareService -> GPIO controllers`
|
||||
- `RobotArmStatus`：`ArmStatusService` 定时汇总发布（状态输入来自 `joint_states` + `dynamic_joint_states`）
|
||||
|
||||
---
|
||||
|
||||
## 8. 时序与性能设计
|
||||
|
||||
### 8.1 PTP链路性能目标
|
||||
|
||||
- 规划阶段：以“稳定优先”，不再文档强约束固定 50ms。
|
||||
- 执行反馈：依赖 action feedback 周期与控制器状态更新频率。
|
||||
- 状态发布：10~20ms 周期（参数可调，默认20ms）。
|
||||
|
||||
### 8.2 时序一致性策略
|
||||
|
||||
- 关节位置：来自 `joint_states` 映射后的 `jointPositions_`。
|
||||
- 末端位姿：基于同一时刻 `jointPositions_` 进行 FK。
|
||||
- 使能/错误：优先由 `dynamic_joint_states` 更新，服务操作会同步更新缓存。
|
||||
|
||||
### 8.3 取消与停止
|
||||
|
||||
- 取消沿用 `DualArmAction` 既有策略（含软停逻辑，按当前实现执行）。
|
||||
- 硬急停状态机作为后续增强项，不在本次服务化重构内改变执行协议。
|
||||
|
||||
### 8.4 急停分层链路与状态机
|
||||
|
||||
#### 分层链路（与当前代码对齐）
|
||||
|
||||
```mermaid
|
||||
flowchart LR
|
||||
IO[外部急停IO] --> HAL[HAL安全输入]
|
||||
HAL --> Driver[驱动层安全状态]
|
||||
Driver --> Topic["/safety/estop_state"]
|
||||
Topic --> SS[SafetyService]
|
||||
SS --> DA[DualArmAction]
|
||||
SS --> ASS[ArmStatusService]
|
||||
DA --> Stop1["Stop1减速停车<br/>MakeStopTrajectory + cancel"]
|
||||
ASS --> Status["/robot_control/arm_status<br/>estop_latched/generation"]
|
||||
```
|
||||
|
||||
- `SafetyService` 订阅 `/safety/estop_state`，维护 `latched/generation` 原子状态。
|
||||
- `SafetyService` 提供 `/robot_control/reset_estop`：
|
||||
- 必须已收到至少一次 `/safety/estop_state`（完成驱动侧状态同步）；
|
||||
- 且当前 `latched=false`（硬件链路已释放）；
|
||||
- 否则返回 `success=false`，拒绝复位请求。
|
||||
- `DualArmAction` 在 **接收目标前** 与 **执行循环中** 检查急停锁存：
|
||||
- 锁存态下拒绝新目标；
|
||||
- 执行中触发时，执行 Stop1 风格减速停车并返回中断结果（`E_STOP_STOP1`）。
|
||||
- `ArmStatusService` 将 `estop_latched/estop_generation` 合并进 `RobotArmStatus`，便于上层统一观测执行态与安全态。
|
||||
|
||||
#### 急停状态机（软件视角）
|
||||
|
||||
```mermaid
|
||||
stateDiagram-v2
|
||||
[*] --> RUNNING
|
||||
RUNNING --> E_STOP_LATCHED: estop_latched=true
|
||||
E_STOP_LATCHED --> STOPPING: 执行中检测到急停
|
||||
STOPPING --> ABORTED: Stop1完成/Action中止
|
||||
E_STOP_LATCHED --> REJECT_NEW_GOAL: 新目标到达
|
||||
REJECT_NEW_GOAL --> E_STOP_LATCHED
|
||||
ABORTED --> WAIT_RESET: 等待安全链路复位
|
||||
WAIT_RESET --> RUNNING: latched=false 且允许复位
|
||||
```
|
||||
|
||||
- `generation` 用于区分“同一锁存态”与“新触发事件”，上层可据此做边沿判定与告警去重。
|
||||
- `ResetEStop` 服务仅作为复位请求入口，复位成功前提是“驱动状态已同步 + 锁存已释放”，不允许绕过硬件安全链路。
|
||||
|
||||
---
|
||||
|
||||
## 9. 错误处理与恢复策略
|
||||
|
||||
### 9.1 错误分类
|
||||
|
||||
- 请求参数错误：`arm_id`/`joint_num` 越界、目标维度不匹配。
|
||||
- 状态错误：未初始化完成、控制器不可用、资源冲突。
|
||||
- 规划错误：MoveIt 无解/规划失败。
|
||||
- 执行错误：FJT 执行失败、超时、被取消。
|
||||
|
||||
### 9.2 上报语义
|
||||
|
||||
- Action：`result.success=false + message`。
|
||||
- Service：`success/result + message` 明确失败原因。
|
||||
- 日志：在 service/action 层保留关键错误日志，便于定位。
|
||||
|
||||
### 9.3 恢复策略
|
||||
|
||||
- 参数错误：立即失败返回，不进入执行链路。
|
||||
- 规划失败：保持控制器状态不变，允许重试。
|
||||
- 执行失败：由上层决定重试、回零或清错。
|
||||
- 清错与使能：通过 service 明确触发，避免隐式副作用。
|
||||
|
||||
---
|
||||
|
||||
## 10. 可观测性与调试支持
|
||||
|
||||
- 统一状态topic：`/robot_control/arm_status` 作为运维与调试主观测口。
|
||||
- 建议录包：`joint_states`、`dynamic_joint_states`、`robot_control/arm_status`、Action feedback/result。
|
||||
- 调试逆解：调用 `/robot_control/inverse_kinematics` 并与 FK结果做闭环比对。
|
||||
|
||||
---
|
||||
|
||||
## 11. 安全性与可靠性
|
||||
|
||||
### 11.1 安全性
|
||||
|
||||
- 关节限位在规划入口前校验。
|
||||
- 使能/清错接口显式化，避免业务层绕过硬件控制语义。
|
||||
- 双臂/单臂关节范围通过 `arm_id + joint_num` 严格限定。
|
||||
|
||||
### 11.2 可靠性
|
||||
|
||||
- 节点瘦身 + 服务分层后，修改影响面更小。
|
||||
- 状态发布与执行链路解耦，降低互相阻塞风险。
|
||||
- IK 与 FK 语义清晰，避免“规划末点冒充逆解”的歧义。
|
||||
|
||||
---
|
||||
|
||||
## 12. 测试设计
|
||||
|
||||
### 12.1 功能测试
|
||||
|
||||
- 单臂/双臂 PTP 成功路径（MOVEJ）。
|
||||
- `SetArmEnable`：
|
||||
- `arm_id=0/1/2`
|
||||
- `joint_num=0`（全部）与 `1~6`（单关节）
|
||||
- `ClearArmError`：单关节、单臂全部、双臂全部。
|
||||
- `InverseKinematics`：左/右臂典型位姿、不可达位姿。
|
||||
|
||||
### 12.2 一致性测试
|
||||
|
||||
- 校验 `RobotArmStatus.joint_positions` 与实际 `joint_states` 对齐。
|
||||
- 校验 `RobotArmStatus` 中 pose 是否与同帧关节值 FK 一致。
|
||||
- 校验 service 调用后使能/错误缓存刷新行为。
|
||||
|
||||
### 12.3 稳定性测试
|
||||
|
||||
- 连续PTP请求压力（含取消）。
|
||||
- 状态topic长时间发布（10ms 周期）稳定性与CPU占用。
|
||||
- 控制器重启后的恢复行为（switch_controller + service/action 重新可用）。
|
||||
|
||||
---
|
||||
|
||||
## 13. 变更影响分析
|
||||
|
||||
- **架构层面**：从“node内聚实现”升级为“service分层实现”，可维护性提升。
|
||||
- **兼容性**：对外 action/service/topic 名称保持不变，调用方无感。
|
||||
- **性能影响**：新增服务对象几乎无额外开销；状态发布逻辑迁移不改变频率与数据结构。
|
||||
- **风险点**：需关注缓存一致性（dynamic state 与 service写入并存场景）；建议通过录包回归验证。
|
||||
@@ -55,6 +55,7 @@ set(SOURCES
|
||||
src/core/controller_factory.cpp
|
||||
src/core/remote_controller.cpp
|
||||
src/services/motor_service.cpp
|
||||
src/services/safety_service.cpp
|
||||
src/services/arm_hardware_service.cpp
|
||||
src/services/kinematics_service.cpp
|
||||
src/services/arm_status_service.cpp
|
||||
|
||||
@@ -7,11 +7,13 @@
|
||||
namespace robot_control
|
||||
{
|
||||
class RobotControlManager;
|
||||
class SafetyService;
|
||||
|
||||
struct ActionContext
|
||||
{
|
||||
rclcpp::Node* node{nullptr};
|
||||
RobotControlManager* robot_control_manager{nullptr};
|
||||
SafetyService* safety_service{nullptr};
|
||||
};
|
||||
} // namespace robot_control
|
||||
|
||||
|
||||
@@ -19,13 +19,15 @@ class MoveWaistAction;
|
||||
class MoveWheelAction;
|
||||
class DualArmAction;
|
||||
class MotorService;
|
||||
class SafetyService;
|
||||
|
||||
class ActionManager
|
||||
{
|
||||
public:
|
||||
ActionManager(
|
||||
rclcpp::Node* node,
|
||||
RobotControlManager& robot_control_manager);
|
||||
RobotControlManager& robot_control_manager,
|
||||
const std::shared_ptr<SafetyService>& safety_service);
|
||||
|
||||
~ActionManager();
|
||||
|
||||
@@ -35,6 +37,7 @@ private:
|
||||
rclcpp::Node* node_{nullptr};
|
||||
RobotControlManager& robot_control_manager_;
|
||||
std::shared_ptr<MotorService> motor_service_;
|
||||
std::shared_ptr<SafetyService> safety_service_;
|
||||
|
||||
std::unique_ptr<MoveHomeAction> move_home_;
|
||||
std::unique_ptr<MoveLegAction> move_leg_;
|
||||
|
||||
@@ -135,6 +135,8 @@ private:
|
||||
const std::vector<std::string>& right_joint_names,
|
||||
bool* canceled) const;
|
||||
|
||||
bool is_estop_latched_() const;
|
||||
|
||||
void save_trajectory_to_file_(
|
||||
const trajectory_msgs::msg::JointTrajectory& traj,
|
||||
const std::string& tag) const;
|
||||
|
||||
@@ -25,6 +25,7 @@
|
||||
#include "services/arm_hardware_service.hpp"
|
||||
#include "services/kinematics_service.hpp"
|
||||
#include "services/arm_status_service.hpp"
|
||||
#include "services/safety_service.hpp"
|
||||
|
||||
#include "control_msgs/msg/dynamic_interface_group_values.hpp"
|
||||
#include "control_msgs/msg/dynamic_joint_state.hpp"
|
||||
@@ -87,6 +88,7 @@ namespace robot_control
|
||||
std::unique_ptr<ArmHardwareService> arm_hardware_service_; ///< 机械臂GPIO服务实现
|
||||
std::unique_ptr<KinematicsService> kinematics_service_; ///< 运动学服务实现
|
||||
std::unique_ptr<ArmStatusService> arm_status_service_; ///< 机械臂状态服务实现
|
||||
std::shared_ptr<SafetyService> safety_service_; ///< 急停状态服务实现
|
||||
|
||||
// ==================== 控制状态 ====================
|
||||
bool is_robot_enabled_; ///< 机器人是否启用
|
||||
|
||||
@@ -8,6 +8,7 @@
|
||||
#include "control_msgs/msg/dynamic_joint_state.hpp"
|
||||
#include "interfaces/msg/robot_arm_status.hpp"
|
||||
#include "core/robot_control_manager.hpp"
|
||||
#include "services/safety_service.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
@@ -18,6 +19,7 @@ public:
|
||||
ArmStatusService(
|
||||
rclcpp::Node* node,
|
||||
RobotControlManager* manager,
|
||||
SafetyService* safety_service,
|
||||
const rclcpp::Publisher<interfaces::msg::RobotArmStatus>::SharedPtr& status_pub,
|
||||
std::vector<bool>* joint_enabled_cache,
|
||||
std::vector<int32_t>* joint_error_code_cache);
|
||||
@@ -28,6 +30,7 @@ public:
|
||||
private:
|
||||
rclcpp::Node* node_{nullptr};
|
||||
RobotControlManager* manager_{nullptr};
|
||||
SafetyService* safety_service_{nullptr};
|
||||
rclcpp::Publisher<interfaces::msg::RobotArmStatus>::SharedPtr status_pub_;
|
||||
std::vector<bool>* joint_enabled_cache_{nullptr};
|
||||
std::vector<int32_t>* joint_error_code_cache_{nullptr};
|
||||
|
||||
38
src/robot_control/include/services/safety_service.hpp
Normal file
38
src/robot_control/include/services/safety_service.hpp
Normal file
@@ -0,0 +1,38 @@
|
||||
#pragma once
|
||||
|
||||
#include <atomic>
|
||||
#include <memory>
|
||||
|
||||
#include "rclcpp/rclcpp.hpp"
|
||||
#include "interfaces/msg/e_stop_state.hpp"
|
||||
#include "interfaces/srv/reset_e_stop.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
class SafetyService
|
||||
{
|
||||
public:
|
||||
using EStopState = interfaces::msg::EStopState;
|
||||
using ResetEStop = interfaces::srv::ResetEStop;
|
||||
|
||||
explicit SafetyService(rclcpp::Node* node);
|
||||
|
||||
void OnEStopState(const EStopState::SharedPtr msg);
|
||||
void HandleResetEStop(
|
||||
const std::shared_ptr<ResetEStop::Request> request,
|
||||
std::shared_ptr<ResetEStop::Response> response);
|
||||
|
||||
bool IsEstopLatched() const { return estop_latched_.load(); }
|
||||
uint32_t GetEstopGeneration() const { return estop_generation_.load(); }
|
||||
|
||||
private:
|
||||
rclcpp::Node* node_{nullptr};
|
||||
rclcpp::Subscription<EStopState>::SharedPtr estop_sub_;
|
||||
rclcpp::Service<ResetEStop>::SharedPtr reset_estop_srv_;
|
||||
std::atomic<bool> estop_state_received_{false};
|
||||
std::atomic<bool> estop_latched_{false};
|
||||
std::atomic<uint32_t> estop_generation_{0};
|
||||
};
|
||||
|
||||
} // namespace robot_control
|
||||
@@ -14,14 +14,17 @@
|
||||
|
||||
#include "core/robot_control_manager.hpp"
|
||||
#include "services/motor_service.hpp"
|
||||
#include "services/safety_service.hpp"
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
ActionManager::ActionManager(
|
||||
rclcpp::Node* node,
|
||||
RobotControlManager& robot_control_manager)
|
||||
RobotControlManager& robot_control_manager,
|
||||
const std::shared_ptr<SafetyService>& safety_service)
|
||||
: node_(node)
|
||||
, robot_control_manager_(robot_control_manager)
|
||||
, safety_service_(safety_service)
|
||||
{
|
||||
motor_service_ = std::make_shared<MotorService>(node_);
|
||||
}
|
||||
@@ -33,6 +36,7 @@ void ActionManager::initialize()
|
||||
ActionContext ctx;
|
||||
ctx.node = node_;
|
||||
ctx.robot_control_manager = &robot_control_manager_;
|
||||
ctx.safety_service = safety_service_.get();
|
||||
|
||||
move_home_ = std::make_unique<MoveHomeAction>(ctx);
|
||||
move_leg_ = std::make_unique<MoveLegAction>(ctx);
|
||||
|
||||
@@ -1,6 +1,7 @@
|
||||
#include "actions/dual_arm_action.hpp"
|
||||
#include "core/robot_control_manager.hpp"
|
||||
#include "core/common_enum.hpp"
|
||||
#include "services/safety_service.hpp"
|
||||
#include "utils/future_utils.hpp"
|
||||
#include "utils/trajectory_utils.hpp"
|
||||
#include "utils/velocity_scale_utils.hpp"
|
||||
@@ -59,6 +60,12 @@ rclcpp_action::GoalResponse DualArmAction::handle_goal_(
|
||||
"Received DualArm goal: velocity_scaling=%d, arm_motion_params_count=%zu",
|
||||
goal->velocity_scaling, goal->arm_motion_params.size());
|
||||
|
||||
if (is_estop_latched_()) {
|
||||
RCLCPP_ERROR(ctx_.node->get_logger(),
|
||||
"DualArm goal rejected: E-Stop latched");
|
||||
return rclcpp_action::GoalResponse::REJECT;
|
||||
}
|
||||
|
||||
// 关节状态未稳定前进行规划/执行，容易出现起点跳变（看起来像启停抖动）
|
||||
if (!ctx_.robot_control_manager->RobotInitFinished()) {
|
||||
RCLCPP_ERROR(ctx_.node->get_logger(),
|
||||
@@ -439,6 +446,12 @@ void DualArmAction::execute_single_arm_(
|
||||
{
|
||||
const auto& arm_param = goal->arm_motion_params[0];
|
||||
const uint8_t arm_id = arm_param.arm_id;
|
||||
if (is_estop_latched_()) {
|
||||
result->success = false;
|
||||
result->message = "E_STOP_LATCHED";
|
||||
goal_handle->abort(result);
|
||||
return;
|
||||
}
|
||||
if (arm_id != ArmMotionParams::ARM_LEFT && arm_id != ArmMotionParams::ARM_RIGHT) {
|
||||
result->success = false;
|
||||
result->message = "Invalid arm_id for single-arm goal";
|
||||
@@ -579,6 +592,12 @@ void DualArmAction::execute_dual_arm_(
|
||||
double acceleration_scaling)
|
||||
{
|
||||
const size_t ARM_DOF = motion_params.arm_dof_;
|
||||
if (is_estop_latched_()) {
|
||||
result->success = false;
|
||||
result->message = "E_STOP_LATCHED";
|
||||
goal_handle->abort(result);
|
||||
return;
|
||||
}
|
||||
|
||||
if (!left_fjt_client_ || !right_fjt_client_) {
|
||||
result->success = false;
|
||||
@@ -862,6 +881,16 @@ bool DualArmAction::wait_for_execution_(
|
||||
auto start_time = ctx_.node->now();
|
||||
|
||||
while (rclcpp::ok()) {
|
||||
if (is_estop_latched_()) {
|
||||
if (cancel_actions) {
|
||||
cancel_actions();
|
||||
}
|
||||
result->success = false;
|
||||
result->message = "E_STOP_STOP1";
|
||||
goal_handle->abort(result);
|
||||
return false;
|
||||
}
|
||||
|
||||
if (goal_handle->is_canceling()) {
|
||||
if (cancel_actions) {
|
||||
cancel_actions();
|
||||
@@ -896,6 +925,11 @@ bool DualArmAction::wait_for_execution_(
|
||||
return rclcpp::ok();
|
||||
}
|
||||
|
||||
bool DualArmAction::is_estop_latched_() const
|
||||
{
|
||||
return ctx_.safety_service && ctx_.safety_service->IsEstopLatched();
|
||||
}
|
||||
|
||||
void DualArmAction::execute_(const std::shared_ptr<GoalHandleDualArm> goal_handle)
|
||||
{
|
||||
if (!ctx_.robot_control_manager) {
|
||||
|
||||
@@ -38,9 +38,11 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
|
||||
kinematics_service_ = std::make_unique<KinematicsService>(
|
||||
this,
|
||||
&robotControlManager_);
|
||||
safety_service_ = std::make_shared<SafetyService>(this);
|
||||
arm_status_service_ = std::make_unique<ArmStatusService>(
|
||||
this,
|
||||
&robotControlManager_,
|
||||
safety_service_.get(),
|
||||
robot_arm_status_pub_,
|
||||
&joint_enabled_cache_,
|
||||
&joint_error_code_cache_);
|
||||
@@ -83,7 +85,8 @@ RobotControlNode::RobotControlNode() : Node("robot_control_node")
|
||||
// 初始化 ActionManager（内部会创建所需的 MotorService）
|
||||
action_manager_ = std::make_unique<ActionManager>(
|
||||
this,
|
||||
robotControlManager_);
|
||||
robotControlManager_,
|
||||
safety_service_);
|
||||
action_manager_->initialize();
|
||||
|
||||
const bool ret1 = activateController("left_arm_controller");
|
||||
|
||||
@@ -8,11 +8,13 @@ namespace robot_control
|
||||
ArmStatusService::ArmStatusService(
|
||||
rclcpp::Node* node,
|
||||
RobotControlManager* manager,
|
||||
SafetyService* safety_service,
|
||||
const rclcpp::Publisher<interfaces::msg::RobotArmStatus>::SharedPtr& status_pub,
|
||||
std::vector<bool>* joint_enabled_cache,
|
||||
std::vector<int32_t>* joint_error_code_cache)
|
||||
: node_(node),
|
||||
manager_(manager),
|
||||
safety_service_(safety_service),
|
||||
status_pub_(status_pub),
|
||||
joint_enabled_cache_(joint_enabled_cache),
|
||||
joint_error_code_cache_(joint_error_code_cache)
|
||||
@@ -96,6 +98,14 @@ void ArmStatusService::PublishRobotArmStatus()
|
||||
msg.joint_error_codes.assign(msg.joint_names.size(), 0);
|
||||
}
|
||||
|
||||
if (safety_service_) {
|
||||
msg.estop_latched = safety_service_->IsEstopLatched();
|
||||
msg.estop_generation = safety_service_->GetEstopGeneration();
|
||||
} else {
|
||||
msg.estop_latched = false;
|
||||
msg.estop_generation = 0;
|
||||
}
|
||||
|
||||
(void)manager_->GetArmCurrentPose(0, &msg.left_arm_pose, nullptr);
|
||||
(void)manager_->GetArmCurrentPose(1, &msg.right_arm_pose, nullptr);
|
||||
|
||||
|
||||
77
src/robot_control/src/services/safety_service.cpp
Normal file
77
src/robot_control/src/services/safety_service.cpp
Normal file
@@ -0,0 +1,77 @@
|
||||
#include "services/safety_service.hpp"
|
||||
#include <functional>
|
||||
|
||||
namespace robot_control
|
||||
{
|
||||
|
||||
SafetyService::SafetyService(rclcpp::Node* node)
|
||||
: node_(node)
|
||||
{
|
||||
if (node_) {
|
||||
estop_sub_ = node_->create_subscription<EStopState>(
|
||||
"/safety/estop_state",
|
||||
10,
|
||||
std::bind(&SafetyService::OnEStopState, this, std::placeholders::_1));
|
||||
reset_estop_srv_ = node_->create_service<ResetEStop>(
|
||||
"/robot_control/reset_estop",
|
||||
std::bind(
|
||||
&SafetyService::HandleResetEStop,
|
||||
this,
|
||||
std::placeholders::_1,
|
||||
std::placeholders::_2));
|
||||
}
|
||||
}
|
||||
|
||||
void SafetyService::OnEStopState(const EStopState::SharedPtr msg)
|
||||
{
|
||||
if (!msg) {
|
||||
return;
|
||||
}
|
||||
|
||||
const bool prev = estop_latched_.load();
|
||||
estop_state_received_.store(true);
|
||||
estop_latched_.store(msg->latched);
|
||||
estop_generation_.store(msg->generation);
|
||||
|
||||
if (node_ && msg->latched && !prev) {
|
||||
RCLCPP_ERROR(
|
||||
node_->get_logger(),
|
||||
"E-Stop latched: source=%u generation=%u reason='%s'",
|
||||
static_cast<unsigned>(msg->source),
|
||||
static_cast<unsigned>(msg->generation),
|
||||
msg->reason.c_str());
|
||||
}
|
||||
}
|
||||
|
||||
void SafetyService::HandleResetEStop(
|
||||
const std::shared_ptr<ResetEStop::Request> request,
|
||||
std::shared_ptr<ResetEStop::Response> response)
|
||||
{
|
||||
if (!response) {
|
||||
return;
|
||||
}
|
||||
if (!request || !request->reset) {
|
||||
response->success = false;
|
||||
response->message = "Invalid reset request";
|
||||
return;
|
||||
}
|
||||
|
||||
if (!estop_state_received_.load()) {
|
||||
response->success = false;
|
||||
response->message =
|
||||
"No /safety/estop_state received yet, reset is not allowed before safety state sync";
|
||||
return;
|
||||
}
|
||||
|
||||
if (estop_latched_.load()) {
|
||||
response->success = false;
|
||||
response->message =
|
||||
"E-Stop is still latched by safety input, wait for /safety/estop_state release";
|
||||
return;
|
||||
}
|
||||
|
||||
response->success = true;
|
||||
response->message = "E-Stop reset request accepted";
|
||||
}
|
||||
|
||||
} // namespace robot_control
|
||||
Reference in New Issue
Block a user