机器人 / 系统课时
执行轨迹并处理取消与失败
执行轨迹并处理取消与失败
本课使用 ROS 2 Jazzy、MoveIt 2 和 Panda 模拟控制器,把已经规划出的关节轨迹交给执行动作,观察接收、反馈与最终结果,并在运动中主动停止。完成后能区分“规划成功”“请求被接收”和“控制器确实执行成功”。
学习目标
- 使用
ExecuteTrajectoryAction 提交MoveGroupInterface产生的轨迹。 - 区分动作拒绝、执行失败、正常完成与主动停止。
- 利用控制器状态和
/joint_states验证停止后的真实关节位置。
先确认执行通道
在安装了 MoveIt 2 与 Panda 资源包的 Ubuntu 24.04 / ROS 2 Jazzy 环境中,先启动带模拟硬件的 Panda。真实机械臂不要直接运行本课的自动执行示例。
source /opt/ros/jazzy/setup.bash
ros2 launch moveit_resources_panda_moveit_config demo.launch.py预期结果:RViz 显示 Panda,move_group 和 ros2_control_node 启动,机械臂控制器处于活动状态。保持此终端运行,另开终端检查:
source /opt/ros/jazzy/setup.bash
ros2 control list_controllers
ros2 action list -t预期输出(节选,其他控制器与顺序可能不同):
panda_arm_controller[joint_trajectory_controller/JointTrajectoryController] active
joint_state_broadcaster[joint_state_broadcaster/JointStateBroadcaster] active
/execute_trajectory [moveit_msgs/action/ExecuteTrajectory]
/panda_arm_controller/follow_joint_trajectory [control_msgs/action/FollowJointTrajectory]/execute_trajectory 是 MoveIt 的动作入口,MoveIt 再通过 /panda_arm_controller/follow_joint_trajectory 交给关节轨迹控制器。若后者不存在或控制器非 active,即使 plan() 成功也无法完成运动。
提交轨迹并判断终态
plan() 只计算路径;动作 Goal 被接收,也不等于控制器已接受轨迹。下例使用 ExecuteTrajectory 的 Goal、Feedback 和 Result 分别记录三个阶段。取消时先调用 MoveGroupInterface::stop() 请求停止轨迹,再尝试取消当前动作 Goal;最终仍以 Result 和实测状态为准。

将下面源码保存为 src/execute_guard.cpp,完整示例依赖下一节的 CMake 与启动文件。
#include <chrono>
#include <future>
#include <memory>
#include <thread>
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <moveit_msgs/action/execute_trajectory.hpp>
#include <moveit_msgs/msg/move_it_error_codes.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>
using Group = moveit::planning_interface::MoveGroupInterface;
using Action = moveit_msgs::action::ExecuteTrajectory;
using GoalHandle = rclcpp_action::ClientGoalHandle<Action>;
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared(
"panda_execute_guard",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
std::thread spinner([&executor] { executor.spin(); });
int exit_code = 1;
{
Group group(node, "panda_arm");
group.setPlanningTime(5.0);
group.setMaxVelocityScalingFactor(0.05);
group.setMaxAccelerationScalingFactor(0.05);
group.setStartStateToCurrentState();
Group::Plan plan;
// 规划到 Panda SRDF 中的命名关节状态,未成功或空轨迹不允许执行。
if (!group.setNamedTarget("transport") ||
group.plan(plan) != moveit::core::MoveItErrorCode::SUCCESS ||
plan.trajectory.joint_trajectory.points.empty()) {
RCLCPP_ERROR(node->get_logger(), "planning failed or trajectory empty");
} else {
RCLCPP_INFO(node->get_logger(), "planned points=%zu",
plan.trajectory.joint_trajectory.points.size());
auto client = rclcpp_action::create_client<Action>(node, "/execute_trajectory");
if (!client->wait_for_action_server(std::chrono::seconds(5))) {
RCLCPP_ERROR(node->get_logger(), "execution action server unavailable");
} else {
Action::Goal goal;
goal.trajectory = plan.trajectory;
rclcpp_action::Client<Action>::SendGoalOptions options;
options.feedback_callback =
[node](GoalHandle::SharedPtr,
const std::shared_ptr<const Action::Feedback> feedback) {
RCLCPP_INFO(node->get_logger(), "feedback state=%s", feedback->state.c_str());
};
// 被接收只是开始;还要等待终态以及 MoveIt 错误码。
auto handle = client->async_send_goal(goal, options).get();
if (!handle) {
RCLCPP_ERROR(node->get_logger(), "execution goal rejected");
} else {
RCLCPP_INFO(node->get_logger(), "execution goal accepted");
auto future = client->async_get_result(handle);
const double seconds = node->get_parameter_or("stop_after_sec", -1.0);
if (seconds > 0.0 &&
future.wait_for(std::chrono::duration<double>(seconds)) ==
std::future_status::timeout) {
group.stop(); // 请求 MoveIt 停止当前轨迹
client->async_cancel_goal(handle);
RCLCPP_WARN(node->get_logger(), "stop and cancel requested");
}
const auto result = future.get();
const int code = result.result ? result.result->error_code.val : 0;
if (result.code == rclcpp_action::ResultCode::SUCCEEDED &&
code == moveit_msgs::msg::MoveItErrorCodes::SUCCESS) {
RCLCPP_INFO(node->get_logger(), "execution succeeded");
exit_code = 0;
} else if (result.code == rclcpp_action::ResultCode::CANCELED ||
code == moveit_msgs::msg::MoveItErrorCodes::PREEMPTED) {
RCLCPP_WARN(node->get_logger(), "execution interrupted");
exit_code = 2;
} else {
RCLCPP_ERROR(node->get_logger(), "execution failed: action=%d moveit=%d",
static_cast<int>(result.code), code);
}
}
}
}
}
executor.cancel();
spinner.join();
rclcpp::shutdown();
return exit_code;
}预期结果(正常运行时关键日志,时间和轨迹点数会变化):
[INFO] [<TIME>] [panda_execute_guard]: planned points=<N>
[INFO] [<TIME>] [panda_execute_guard]: execution goal accepted
[INFO] [<TIME>] [panda_execute_guard]: execution succeeded反馈回调中的 state 是 MoveIt 的执行阶段字符串,并非各关节误差。代码的 stop_after_sec 默认是 -1.0(不主动停止),传入正数后只在该时间内仍未执行完毕时请求停止。
构建并运行示例
新建一个独立 ROS 2 工作空间和 C++ 包;若已有工作空间,直接在对应 src 中创建包即可。
mkdir -p ~/ws_moveit/src
cd ~/ws_moveit/src
ros2 pkg create panda_execute_guard --build-type ament_cmake --dependencies rclcpp rclcpp_action moveit_ros_planning_interface moveit_msgs moveit_configs_utils moveit_resources_panda_moveit_config
mkdir -p panda_execute_guard/launch预期结果:生成 panda_execute_guard 包及 package.xml、CMakeLists.txt,在包内保存上述 src/execute_guard.cpp。用下列内容替换 CMakeLists.txt:
cmake_minimum_required(VERSION 3.8)
project(panda_execute_guard)
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_msgs REQUIRED)
add_executable(execute_guard src/execute_guard.cpp)
target_compile_features(execute_guard PUBLIC cxx_std_17)
ament_target_dependencies(execute_guard
rclcpp rclcpp_action moveit_ros_planning_interface moveit_msgs)
install(TARGETS execute_guard DESTINATION lib/${PROJECT_NAME})
install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
ament_package()配置生效后会构建并安装 execute_guard 可执行程序以及启动文件。将下面代码保存为 launch/execute_guard.launch.py,显式为客户端传入机器人 URDF、SRDF 和运动学参数;单独 ros2 run 而不传入这些参数可能无法构造 Panda 机器人模型。
from launch import LaunchDescription
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch_ros.actions import Node
from launch_ros.parameter_descriptions import ParameterValue
from moveit_configs_utils import MoveItConfigsBuilder
def generate_launch_description():
config = (MoveItConfigsBuilder("moveit_resources_panda")
.robot_description(
file_path="config/panda.urdf.xacro",
mappings={"ros2_control_hardware_type": "mock_components"})
.robot_description_semantic(file_path="config/panda.srdf")
.to_moveit_configs())
return LaunchDescription([
DeclareLaunchArgument("stop_after_sec", default_value="-1.0"),
Node(
package="panda_execute_guard",
executable="execute_guard",
output="screen",
parameters=[
config.robot_description,
config.robot_description_semantic,
config.robot_description_kinematics,
{"stop_after_sec": ParameterValue(
LaunchConfiguration("stop_after_sec"), value_type=float)},
],
),
])启动文件只启动客户端;Panda 的 demo.launch.py 必须继续运行以提供规划服务和控制器。在另一个已加载 Jazzy 环境的终端执行:
cd ~/ws_moveit
colcon build --packages-select panda_execute_guard
source install/setup.bash
ros2 launch panda_execute_guard execute_guard.launch.py预期结果:构建成功,终端依次报告规划完成、动作接收和执行成功;RViz 内 Panda 从初始 ready 姿态缓慢移动到 transport。若首遍已到目标,重新启动 Panda 演示或切换一个安全起点后再测试停止。
取消与失败应如何处理
只有在模拟环境执行以下停止测试。先重启 Panda 的 demo.launch.py,使机械臂恢复初始 ready 姿态;不要将软件取消当作硬件急停。
source ~/ws_moveit/install/setup.bash
ros2 launch panda_execute_guard execute_guard.launch.py stop_after_sec:=2.0预期结果:未在 2 秒内结束的轨迹会产生 stop and cancel requested,随后通常记录 execution interrupted;实际 Action 终态不一定是 CANCELED,MoveIt 可能用 PREEMPTED(-7)并返回 ABORTED。动作取消请求被接受,也不能代替检查机械臂是否已经停下。
| 观察到的信号 | 含义与处理 |
|---|---|
plan() 成功、Action Goal 被拒绝 |
规划可用但请求未进入执行;检查 /execute_trajectory 和服务器日志 |
Goal 接收,MoveIt SUCCESS(1) |
执行管理器报告成功;再核对末端关节实际值 |
MoveIt CONTROL_FAILED(-4) |
控制器缺失、拒绝轨迹或执行故障;先检查活动控制器和 FollowJointTrajectory 日志 |
MoveIt TIMED_OUT(-6) |
执行超时;核对执行时间预算、控制器速度及反馈 |
PREEMPTED(-7)或 Action CANCELED |
中断执行;重新获取当前状态,禁止直接重放旧轨迹 |
测试控制器拒绝时,可在 Panda 模拟轨迹尚未执行前手动将 panda_arm_controller 切成 inactive,观察规划仍可能成功但执行失败;完成后恢复 active。不可在真实运动中用切换控制器状态模拟急停。
ros2 control set_controller_state panda_arm_controller inactive
ros2 control list_controllers预期结果:查询结果中 panda_arm_controller 显示 inactive。这时在另一终端重跑客户端:
ros2 launch panda_execute_guard execute_guard.launch.py预期结果:通常能得到规划结果,但执行返回控制器相关失败,常见 MoveIt 错误码为 CONTROL_FAILED(-4)。最后恢复模拟控制器:
ros2 control set_controller_state panda_arm_controller active预期结果:重新查询 ros2 control list_controllers 时,该控制器恢复为 active。
用实际关节反馈验收
ExecuteTrajectory 的反馈只说明 MoveIt 正处于哪个执行阶段。要确认 Panda 是否真的运动或停止,应同时核对控制器的期望值与实测值,并查看广播出来的关节状态。
ros2 topic echo /panda_arm_controller/controller_state --once
ros2 topic echo /joint_states --once预期结果:第一条消息含 desired、actual、error 等轨迹跟踪字段,第二条含各关节的 name、position。取消后间隔约 1 秒重复读取,确认位置不再持续变化;不要把两次瞬时误差为零当作急停安全认证。