机器人 / 系统课时

读取规划结果并处理失败码

读取规划结果并处理失败码

本课用 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()。

cpp
#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 演示场景正常且规划成功时,保留本程序的关键输出):

text
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 包也可直接放入上述源文件。

bash
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
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 机器人描述和运动学参数:

python
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 编译并启动检查程序:

bash
source /opt/ros/jazzy/setup.bash
ros2 launch moveit_resources_panda_moveit_config demo.launch.py

预期结果:RViz 出现 Panda 模型,move_group 启动并提供规划服务。终端 B:

bash
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 与仿真时间是否一致

实际项目应把“失败即停止”作为硬门槛:返回码不成功时保留错误码与现场日志,修改目标或场景后重新规划,绝不复用上一轮的轨迹。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

读取规划结果并处理失败码

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通