机器人 / 系统课时
用 C++ 设置目标并调用 plan
用 C++ 设置目标并调用 plan
在 Panda 机械臂的 MoveIt 2 场景中,分别提交关节角度目标和末端位姿目标,读取规划是否成功以及返回的轨迹点数。整个示例只调用 plan(),不会驱动机械臂,便于先验证起点、目标和规划场景是否一致。
学习目标
- 使用
MoveGroupInterface从当前状态构造关节目标。 - 用
setPoseTarget()指定末端位姿,并区分关节目标与位姿目标。 - 检查
plan()的返回码和Plan中的轨迹,不把规划成功当成执行完成。
准备 Panda 规划环境
示例适用于 Ubuntu 24.04、ROS 2 Jazzy 和已配置 Panda 的 MoveIt 2。需要 ROS 2 C++ 开发工具、moveit_ros_planning_interface,以及能启动 move_group、机器人状态发布与关节状态的 Panda 演示配置。先检查依赖:
source /opt/ros/jazzy/setup.bash
ros2 pkg prefix moveit_ros_planning_interface
ros2 pkg prefix moveit2_tutorials预期输出:
<PATH>
<PATH><PATH> 是安装前缀,可能位于 /opt/ros/jazzy 或工作空间的 install 目录;第二条包查询失败时,须先安装教程包,或改用已有的 panda_moveit_config 演示启动文件。
创建独立的 CMake 软件包,命令会自动生成依赖声明与可执行目标:
mkdir -p ~/ws_moveit/src
cd ~/ws_moveit/src
ros2 pkg create moveit_plan_targets --build-type ament_cmake --dependencies rclcpp geometry_msgs moveit_ros_planning_interface --node-name plan_targets预期结果:生成 moveit_plan_targets/src/plan_targets.cpp、CMakeLists.txt 和 package.xml;下文直接替换生成的 .cpp 文件内容。
理解起点与规划请求的关系
MoveGroupInterface 是 C++ 请求入口,move_group 负责调用规划流水线。getCurrentState() 从状态监视器取得关节状态;setStartStateToCurrentState() 指定本次规划起点;目标设置接口决定终点条件;plan() 返回 MoveGroupInterface::Plan,其中 trajectory 是候选轨迹。

两次规划都从实际当前状态出发:第一次得到的关节轨迹没有执行,不能直接把它的终点当成第二次的真实起点。位姿目标的坐标取自规划参考系,姿态四元数必须有效。
编写两类目标的完整程序
将以下代码保存为 ~/ws_moveit/src/moveit_plan_targets/src/plan_targets.cpp。panda_arm 是 Panda 示例的规划组;其他机器人需要替换为自己 SRDF 中的组名,并调整关节数量和目标位姿。
#include <geometry_msgs/msg/pose.hpp>
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <moveit/robot_state/robot_state.hpp>
#include <rclcpp/rclcpp.hpp>
#include <thread>
#include <vector>
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = rclcpp::Node::make_shared(
"plan_targets",
rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));
auto logger = node->get_logger();
// 状态监视器需要持续处理 ROS 2 回调。
rclcpp::executors::SingleThreadedExecutor executor;
executor.add_node(node);
std::thread spinner([&executor]() { executor.spin(); });
int status = 0;
{
using moveit::planning_interface::MoveGroupInterface;
MoveGroupInterface arm(node, "panda_arm");
arm.setPlanningTime(5.0);
arm.setMaxVelocityScalingFactor(0.1);
arm.setMaxAccelerationScalingFactor(0.1);
auto current = arm.getCurrentState(10.0);
if (!current) {
RCLCPP_ERROR(logger, "No current robot state");
status = 1;
} else {
const auto* group = current->getJointModelGroup("panda_arm");
if (!group) {
RCLCPP_ERROR(logger, "Planning group not found");
status = 1;
} else {
std::vector<double> joints;
current->copyJointGroupPositions(group, joints);
joints.at(0) = -0.4; // Panda 第一个关节,单位为弧度。
arm.setStartStateToCurrentState();
if (arm.setJointValueTarget(joints)) {
MoveGroupInterface::Plan joint_plan;
if (arm.plan(joint_plan) == moveit::core::MoveItErrorCode::SUCCESS) {
RCLCPP_INFO(logger, "Joint plan OK, points=%zu",
joint_plan.trajectory.joint_trajectory.points.size());
} else {
RCLCPP_ERROR(logger, "Joint plan failed");
status = 1;
}
} else {
RCLCPP_ERROR(logger, "Joint target violates limits");
status = 1;
}
// 上一次仅规划未执行,所以再次取实际当前状态作为起点。
arm.setStartStateToCurrentState();
arm.setPoseReferenceFrame(arm.getPlanningFrame());
geometry_msgs::msg::Pose goal;
goal.orientation.w = 1.0; // 单位四元数。
goal.position.x = 0.28;
goal.position.y = -0.20;
goal.position.z = 0.50;
if (arm.setPoseTarget(goal)) {
MoveGroupInterface::Plan pose_plan;
if (arm.plan(pose_plan) == moveit::core::MoveItErrorCode::SUCCESS) {
RCLCPP_INFO(logger, "Pose plan OK, points=%zu",
pose_plan.trajectory.joint_trajectory.points.size());
} else {
RCLCPP_ERROR(logger, "Pose plan failed");
status = 1;
}
} else {
RCLCPP_ERROR(logger, "Pose target rejected");
status = 1;
}
}
}
}
executor.cancel();
spinner.join();
rclcpp::shutdown();
return status;
}预期结果:程序对关节目标与末端位姿目标分别发送一次规划请求;可达且无碰撞时,两次各得到一条含关节轨迹点的 Plan。程序没有调用 execute(),机械臂不会因为本程序而运动。
编译、运行与验收
在终端 A 编译软件包,确保新终端已经加载 ROS 2 环境:
source /opt/ros/jazzy/setup.bash
cd ~/ws_moveit
colcon build --packages-select moveit_plan_targets
source install/setup.bash预期输出包含:
Summary: 1 package finished [<TIME>]<TIME> 为实际耗时。然后在终端 B 启动 Panda 演示,保持窗口运行:
source /opt/ros/jazzy/setup.bash
source ~/ws_moveit/install/setup.bash
ros2 launch moveit2_tutorials demo.launch.py预期结果:RViz 显示 Panda,move_group 已启动,关节状态持续更新。若使用自行生成的 panda_moveit_config,将命令中的包名改为它,保留 demo.launch.py。
在终端 A 运行自己的节点:
ros2 run moveit_plan_targets plan_targets规划成功时应看到以下关键日志形态(运行时还会有 MoveIt 内部日志):
[INFO] [<TIME>] [plan_targets]: Joint plan OK, points=<N>
[INFO] [<TIME>] [plan_targets]: Pose plan OK, points=<N><N> 是实际轨迹点数,受规划器、起始关节状态和场景影响,并非固定数值。可验收的结果是两次状态均成功、轨迹点数大于零,且没有执行动作;只调用 plan() 不会自动在 RViz 播放这两条轨迹,需要另外发布可视化消息才会显示规划动画。
规划失败时优先检查什么
- 出现
No current robot state:检查演示程序是否正常发布/joint_states、节点是否在旋转处理回调,仿真时间配置是否一致;可执行ros2 topic hz /joint_states检查消息频率,正常应持续显示非零频率。 - 出现
Planning group not found或机器人模型加载错误:核对panda_arm与 SRDF 规划组,确认robot_description和robot_description_semantic已正确提供。 - 关节目标越限:先检查
joints.at(0) = -0.4对目标机器人的限位是否有效;不要将角度值误当成弧度,也不要在setJointValueTarget()返回false后继续规划。 - 位姿规划失败:检查
arm.getPlanningFrame()、末端链路、位置单位米和单位四元数;确认目标位姿有 IK 解并且路径无碰撞。可先把规划时间由5.0秒提高到10.0秒排除采样不足,但超时延长不能修复不可达目标。
读取返回值时应先判断 MoveItErrorCode::SUCCESS,再使用 Plan::trajectory。规划轨迹是候选结果,不代表执行器已经跟踪完成,真实硬件应在碰撞场景、控制器及安全限位独立确认后再考虑执行。