机器人 / 系统课时
读取规划结果并处理失败码
读取规划结果并处理失败码
本课用 ROS 2 Jazzy 和 MoveIt 2 Panda 演示如何读取 plan() 的错误码与轨迹数据:先检查是否成功,再核对关节轨迹是否完整。程序遇到超时、无效目标或碰撞相关失败会立即停止,不会把失败结果交给执行器。
学习目标
- 读取
MoveItErrorCode::val,区分成功、规划超时、目标无效和碰撞相关失败。 - 检查
Plan中的轨迹关节名称、轨迹点和规划耗时。 - 在失败或结果不完整时立即拒绝执行请求,并能据错误码定位下一步检查项。
先看返回码与轨迹字段
move_group.plan(plan) 返回 moveit::core::MoveItErrorCode;其中 val == 1(SUCCESS)代表本次规划报告成功,不代表机械臂已经运动。错误码为非成功值时,不应再依赖 plan 中残留或未填写的轨迹数据。每次重试应创建新的 Plan。
| 常量 | 数值 | 排查方向 |
|---|---|---|
SUCCESS |
1 | 继续检查轨迹完整性 |
PLANNING_FAILED |
-1 | 通用规划失败;查看规划器日志与场景 |
TIMED_OUT |
-6 | 搜索超过允许时间,检查约束或复杂障碍 |
INVALID_GOAL_CONSTRAINTS |
-16 | 目标约束缺失、格式或配置有误 |
START_STATE_IN_COLLISION |
-10 | 起点与自身或环境障碍发生碰撞 |
GOAL_IN_COLLISION |
-12 | 目标状态碰撞 |
NO_IK_SOLUTION |
-31 | 末端位姿目标没有可用逆解 |
这些常量来自 moveit_msgs::msg::MoveItErrorCodes,实际使用时可以直接比较 status.val,也可以调用 moveit::core::errorCodeToString(status) 获取名称。碰撞不保证一定返回 GOAL_IN_COLLISION:有些失败只会报告 PLANNING_FAILED,还需结合 move_group 的日志、Planning Scene 和起终点状态判断。
MoveGroupInterface::Plan 在 Jazzy 中提供 start_state、trajectory、planning_time。对 Panda 的 panda_arm 关节规划,重点读取 plan.trajectory.joint_trajectory.joint_names 与 points;每个轨迹点的 positions 数量应与关节名数量一致。planning_time 单位是秒,只在规划成功后读取用于诊断。
把失败挡在执行入口之前
检查顺序固定为:先确认目标设置成功,再判断规划码;只有 SUCCESS 才读取轨迹,若关节列表、轨迹点为空或位置向量长度不匹配,同样视为不可提交。此处通过检查也只是待进一步确认的规划结果,并不触发运动。

以下是完整的 src/plan_guard.cpp。它使用 Panda 配置里存在的 panda_arm 规划组与 ready 命名状态,并在后台处理 ROS 回调;没有调用 execute() 或 move()。
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <moveit/utils/moveit_error_code.hpp>
#include <rclcpp/rclcpp.hpp>
#include <exception>
#include <iostream>
#include <memory>
#include <string>
#include <thread>
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
rclcpp::NodeOptions options;
options.automatically_declare_parameters_from_overrides(true);
auto node = std::make_shared<rclcpp::Node>("plan_guard", options);
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
// 回调线程负责接收机器人状态和动作结果。
std::thread spinner([&executor]() { executor.spin(); });
int result = 1;
try {
moveit::planning_interface::MoveGroupInterface group(node, "panda_arm");
group.setPlanningTime(5.0);
const std::string target_name = "ready";
if (!group.setNamedTarget(target_name)) {
std::cerr << "TARGET_INVALID name=" << target_name << '\n';
} else {
group.setStartStateToCurrentState();
moveit::planning_interface::MoveGroupInterface::Plan plan;
const auto status = group.plan(plan);
// 错误码不成功时,绝不读取旧轨迹来执行。
if (status.val != moveit::core::MoveItErrorCode::SUCCESS) {
std::cerr << "PLAN_FAILED code=" << status.val
<< " kind=" << moveit::core::errorCodeToString(status) << '\n';
} else {
const auto& traj = plan.trajectory.joint_trajectory;
bool valid = !traj.joint_names.empty() && !traj.points.empty();
for (const auto& point : traj.points) {
if (point.positions.size() != traj.joint_names.size()) {
valid = false;
break;
}
}
if (!valid) {
std::cerr << "PLAN_REJECTED incomplete_joint_trajectory\n";
} else {
// 这里只打印可用候选结果,不下发控制器。
std::cout << "PLAN_READY joints=" << traj.joint_names.size()
<< " points=" << traj.points.size()
<< " planning_seconds=" << plan.planning_time << '\n';
result = 0;
}
}
}
} catch (const std::exception& e) {
std::cerr << "CONFIG_ERROR " << e.what() << '\n';
}
executor.cancel();
spinner.join();
rclcpp::shutdown();
return result;
}预期输出(Panda 演示场景正常且规划成功时,保留本程序的关键输出):
PLAN_READY joints=7 points=<N> planning_seconds=<TIME><N> 是实际轨迹点数,<TIME> 是实际规划耗时,两者会随状态、场景和规划器变化。失败时会打印 TARGET_INVALID、PLAN_FAILED code=... kind=...、PLAN_REJECTED 或 CONFIG_ERROR,并返回非零退出码;不会打印 PLAN_READY。
编译并在 Panda 演示中验证
使用 Ubuntu 24.04、ROS 2 Jazzy 和已安装的 MoveIt 2、Panda 演示配置。先在自己的工作空间创建包;已有可用的 ament_cmake 包也可直接放入上述源文件。
source /opt/ros/jazzy/setup.bash
mkdir -p ~/moveit_ws/src
cd ~/moveit_ws/src
ros2 pkg create moveit_plan_guard --build-type ament_cmake --node-name plan_guard --dependencies rclcpp moveit_ros_planning_interface moveit_core
mkdir -p moveit_plan_guard/launch预期结果:生成 moveit_plan_guard 包及 src/plan_guard.cpp、CMakeLists.txt 和 package.xml。将生成的 src/plan_guard.cpp 替换为上面的完整代码,再将 CMakeLists.txt 替换为:
cmake_minimum_required(VERSION 3.8)
project(moveit_plan_guard)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_core REQUIRED)
add_executable(plan_guard src/plan_guard.cpp)
ament_target_dependencies(plan_guard rclcpp moveit_ros_planning_interface moveit_core)
install(TARGETS plan_guard DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
ament_package()此配置为程序链接 MoveIt 规划接口,并把下一步创建的启动文件安装到包共享目录。创建 launch/inspect_plan.launch.py,向客户端节点传入 Panda 机器人描述和运动学参数:
from launch import LaunchDescription
from launch_ros.actions import Node
from moveit_configs_utils import MoveItConfigsBuilder
def generate_launch_description():
config = MoveItConfigsBuilder("moveit_resources_panda").to_moveit_configs()
return LaunchDescription([
Node(
package="moveit_plan_guard",
executable="plan_guard",
output="screen",
parameters=[
config.robot_description,
config.robot_description_semantic,
config.robot_description_kinematics,
],
)
])预期结果:启动该 launch 时,客户端能读取 panda_arm 和 ready;实际执行前仍需单独启动 move_group 与状态发布组件。先在终端 A 启动已安装的 Panda 演示,再在终端 B 编译并启动检查程序:
source /opt/ros/jazzy/setup.bash
ros2 launch moveit_resources_panda_moveit_config demo.launch.py预期结果:RViz 出现 Panda 模型,move_group 启动并提供规划服务。终端 B:
source /opt/ros/jazzy/setup.bash
cd ~/moveit_ws
colcon build --packages-select moveit_plan_guard
source install/setup.bash
ros2 launch moveit_plan_guard inspect_plan.launch.py预期结果:成功编译后打印上一节的 PLAN_READY ...,RViz 中机械臂不会因该程序而运动。若环境未安装 Panda 演示包,应先安装相应配置或改用自己的 MoveIt 启动文件和 SRDF 命名目标,而不是猜测规划组名称。
按失败类型定位原因
故意把 target_name 改为 not_exist,应看到 TARGET_INVALID name=not_exist,且程序会在调用 plan() 前停止。这验证了目标设置错误也不会提交执行。恢复 ready 后,可结合下表继续排查。
| 现象 | 优先检查与修复 |
|---|---|
TIMED_OUT 或 PLANNING_FAILED |
RViz 障碍物、路径约束、规划组;确认目标合理后再评估是否将 setPlanningTime(5.0) 调为 10.0 |
INVALID_GOAL_CONSTRAINTS |
目标是否真的设置、约束是否为空、SRDF 分组是否匹配 |
START_STATE_IN_COLLISION |
/joint_states、碰撞几何、Planning Scene 中的工作台位置 |
GOAL_IN_COLLISION 或 NO_IK_SOLUTION |
目标姿态、参考坐标系、关节范围与障碍物 |
COMMUNICATION_FAILURE、获取状态失败 |
move_group 是否运行、参数是否齐全、ROS Domain 与仿真时间是否一致 |
实际项目应把“失败即停止”作为硬门槛:返回码不成功时保留错误码与现场日志,修改目标或场景后重新规划,绝不复用上一轮的轨迹。