【MoveIt 2】直接通过 C++ API 使用 MoveIt : 创建 MoveIt 插件

此页面详细解释了如何在 ROS 中添加插件。两个必要的元素是基类和插件类。插件类继承自基类并重写其虚函数。用于此目的的主要库是 pluginlib。本教程包含三种不同类型的插件，即运动规划器、控制器管理器和约束采样器。

 运动规划插件 

在本节中，我们将展示如何将一个新的运动规划器作为插件添加到 MoveIt 中。MoveIt 中的基类是 planning_interface ，任何新的规划器插件都应该继承自该基类。为了演示目的，创建了一个线性插值规划器（lerp），它在关节空间中规划两个状态之间的运动。这个规划器可以作为添加任何新规划器的起点，因为它包含了必要的基础知识。本教程中设计的最终源文件可在此处找到 https://github.com/moveit/moveit2_tutorials/tree/main/doc/examples/creating_moveit_plugins/lerp_motion_planner/src 。

cxy@cxy-Ubuntu2404:~/ws_moveit/src/moveit2_tutorials/doc/examples/creating_moveit_plugins/lerp_motion_planner$ tree
.
├── CMakeLists.txt
├── include
│   └── lerp_interface
│       ├── lerp_interface.h
│       └── lerp_planning_context.h
├── launch
│   └── lerp_example.launch
├── lerp_interface_plugin_description.xml
├── lerp_planner.png
└── src
    ├── lerp_example.cpp
    ├── lerp_interface.cpp
    ├── lerp_planner_manager.cpp
    └── lerp_planning_context.cpp


5 directories, 10 files

下图显示了在 MoveIt 中添加新规划器的类之间关系的简要总体视图。

首先，我们在 moveit_tutorials 包中创建插件类。要为 lerp 创建插件类，请在 src 文件夹中创建一个名为 lerp_planner_manager.cpp 的文件。在此文件中， LERPPlanPlannerManager 覆盖了 planning_interface 中的 PlannerManager 类的函数。在此文件的末尾，我们需要将 LERPPlanPlannerManager 类注册为插件，这是通过 class_loader 中的 CLASS_LOADER_REGISTER_CLASS 宏完成的。

CLASS_LOADER_REGISTER_CLASS(emptyplan_interface::EmptyPlanPlannerManager, planning_interface::PlannerManager);

#include <moveit/planning_interface/planning_interface.h> // 包含MoveIt!规划接口头文件
#include <moveit/planning_interface/planning_response.h> // 包含MoveIt!规划响应头文件
#include <moveit/planning_scene/planning_scene.h> // 包含MoveIt!规划场景头文件
#include <moveit/collision_detection_fcl/collision_detector_allocator_fcl.h> // 包含FCL碰撞检测器分配器头文件
#include <class_loader/class_loader.hpp> // 包含类加载器头文件
#include "lerp_interface/lerp_planning_context.h" // 包含LERP规划上下文头文件


namespace lerp_interface // 定义命名空间lerp_interface
{
class LERPPlannerManager : public planning_interface::PlannerManager // 定义LERPPlannerManager类，继承自PlannerManager
{
public:
  LERPPlannerManager() : planning_interface::PlannerManager() // 构造函数
  {
  }


  bool initialize(const moveit::core::RobotModelConstPtr& model, const std::string& /*ns*/) override // 初始化函数
{
    for (const std::string& gpName : model->getJointModelGroupNames()) // 遍历机器人模型的关节组名称
    {
      std::cout << "group name " << gpName << std::endl << "robot model  " << model->getName() << std::endl; // 输出组名称和机器人模型名称
      planning_contexts_[gpName] =
          LERPPlanningContextPtr(new LERPPlanningContext("lerp_planning_context", gpName, model)); // 创建LERP规划上下文并存储在map中
    }
    return true; // 返回true表示初始化成功
  }


  bool canServiceRequest(const moveit_msgs::MotionPlanRequest& req) const override // 检查是否可以处理请求
{
    return req.trajectory_constraints.constraints.empty(); // 如果轨迹约束为空，则返回true
  }


  std::string getDescription() const override // 获取描述
{
    return "LERP"; // 返回描述字符串
  }


  void getPlanningAlgorithms(std::vector<std::string>& algs) const override // 获取规划算法
{
    algs.clear(); // 清空算法列表
    algs.push_back("lerp"); // 添加LERP算法
  }


  planning_interface::PlanningContextPtr getPlanningContext(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                            const planning_interface::MotionPlanRequest& req,
                                                            moveit_msgs::MoveItErrorCodes& error_code) const override // 获取规划上下文
{
    error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; // 设置错误码为成功


    if (req.group_name.empty()) // 如果请求的组名称为空
    {
      ROS_ERROR("No group specified to plan for"); // 输出错误信息
      error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME; // 设置错误码为无效组名称
      return planning_interface::PlanningContextPtr(); // 返回空指针
    }


    if (!planning_scene) // 如果规划场景为空
    {
      ROS_ERROR("No planning scene supplied as input"); // 输出错误信息
      error_code.val = moveit_msgs::MoveItErrorCodes::FAILURE; // 设置错误码为失败
      return planning_interface::PlanningContextPtr(); // 返回空指针
    }


    // 创建使用混合碰撞检测器的规划场景
    planning_scene::PlanningScenePtr ps = planning_scene->diff();


    // 设置FCL为分配器
    ps->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create(), true);


    // 检索并配置现有上下文
    const LERPPlanningContextPtr& context = planning_contexts_.at(req.group_name);
    ROS_INFO_STREAM_NAMED("lerp_planner_manager", "===>>> context is made "); // 输出信息


    context->setPlanningScene(ps); // 设置规划场景
    context->setMotionPlanRequest(req); // 设置运动规划请求


    error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; // 设置错误码为成功


    return context; // 返回上下文
  }


protected:
  std::map<std::string, LERPPlanningContextPtr> planning_contexts_; // 规划上下文map
};


}  // namespace lerp_interface


// 注册LERPPlannerManager类为插件
CLASS_LOADER_REGISTER_CLASS(lerp_interface::LERPPlannerManager, planning_interface::PlannerManager);

接下来我们创建 LERPPlanningContext 类，该类将覆盖 PlanningContext 的功能。这个类将覆盖 solve 函数，其中规划器解决问题并返回解决方案。

#include <moveit/robot_state/conversions.h> // 包含MoveIt!机器人状态转换头文件
#include <moveit/planning_interface/planning_interface.h> // 包含MoveIt!规划接口头文件
#include <moveit_msgs/MotionPlanRequest.h> // 包含MoveIt!运动规划请求头文件
#include <moveit/planning_scene/planning_scene.h> // 包含MoveIt!规划场景头文件


#include "lerp_interface/lerp_planning_context.h" // 包含LERP规划上下文头文件
#include "lerp_interface/lerp_interface.h" // 包含LERP接口头文件


namespace lerp_interface // 定义命名空间lerp_interface
{
LERPPlanningContext::LERPPlanningContext(const std::string& context_name, const std::string& group_name,
                                         const moveit::core::RobotModelConstPtr& model) // 构造函数
  : planning_interface::PlanningContext(context_name, group_name), robot_model_(model) // 初始化列表
{
  lerp_interface_ = LERPInterfacePtr(new LERPInterface()); // 创建LERP接口实例
}


bool LERPPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) // 解决规划问题
{
  moveit_msgs::MotionPlanDetailedResponse res_msg; // 创建详细响应消息
  bool lerp_solved = lerp_interface_->solve(planning_scene_, request_, res_msg); // 调用LERP接口解决规划问题


  if (lerp_solved) // 如果解决成功
  {
    res.trajectory_.resize(1); // 调整轨迹大小
    res.trajectory_[0] =
        robot_trajectory::RobotTrajectoryPtr(new robot_trajectory::RobotTrajectory(robot_model_, getGroupName())); // 创建机器人轨迹实例


    moveit::core::RobotState start_state(robot_model_); // 创建机器人状态实例
    moveit::core::robotStateMsgToRobotState(res_msg.trajectory_start, start_state); // 将消息转换为机器人状态


    res.trajectory_[0]->setRobotTrajectoryMsg(start_state, res_msg.trajectory[0]); // 设置机器人轨迹消息
    res.description_.push_back("plan"); // 添加描述
    res.processing_time_ = res_msg.processing_time; // 设置处理时间
    res.error_code_ = res_msg.error_code; // 设置错误码


    return true; // 返回true表示成功
  }


  res.error_code_ = res_msg.error_code; // 设置错误码
  return false; // 返回false表示失败
};


bool LERPPlanningContext::solve(planning_interface::MotionPlanResponse& res) // 解决规划问题
{
  planning_interface::MotionPlanDetailedResponse res_detailed; // 创建详细响应
  bool planning_success = solve(res_detailed); // 调用详细解决函数


  res.error_code_ = res_detailed.error_code_; // 设置错误码


  if (planning_success) // 如果规划成功
  {
    res.trajectory_ = res_detailed.trajectory_[0]; // 设置轨迹
    res.planning_time_ = res_detailed.processing_time_[0]; // 设置规划时间
  }


  return planning_success; // 返回规划成功状态
}


bool LERPPlanningContext::terminate() // 终止函数
{
  return true; // 返回true表示成功
}


void LERPPlanningContext::clear() // 清除函数
{
  // 该规划器没有状态，因此没有需要清除的内容
}


}  // namespace lerp_interface

由于 solve 函数的实现可能需要许多来自规划器代码库的类，因此更具可读性的是创建另一个名为 LERPInterface 的类，其中将进行规划器 solve 方法的实际实现。基本上，这个类是新运动规划算法的入口点。

#include <moveit/planning_interface/planning_interface.h> // 包含MoveIt!规划接口头文件
#include <moveit/planning_scene/planning_scene.h> // 包含MoveIt!规划场景头文件
#include <moveit/robot_state/conversions.h> // 包含MoveIt!机器人状态转换头文件


#include <moveit_msgs/MotionPlanRequest.h> // 包含MoveIt!运动规划请求头文件


#include <ros/ros.h> // 包含ROS头文件


#include <limits> // 包含标准库limits头文件
#include <vector> // 包含标准库vector头文件
#include <Eigen/Geometry> // 包含Eigen几何头文件
#include <unordered_map> // 包含标准库unordered_map头文件


#include "lerp_interface/lerp_interface.h" // 包含LERP接口头文件


namespace lerp_interface // 定义命名空间lerp_interface
{
LERPInterface::LERPInterface(const ros::NodeHandle& nh) : nh_(nh), name_("LERPInterface") // 构造函数
{
}


bool LERPInterface::solve(const planning_scene::PlanningSceneConstPtr& planning_scene,
                          const planning_interface::MotionPlanRequest& req,
                          moveit_msgs::MotionPlanDetailedResponse& res) // 解决规划问题
{
  // 加载规划器特定参数
  nh_.getParam("num_steps", num_steps_);


  ros::WallTime start_time = ros::WallTime::now(); // 获取当前时间
  moveit::core::RobotModelConstPtr robot_model = planning_scene->getRobotModel(); // 获取机器人模型
  moveit::core::RobotStatePtr start_state(new moveit::core::RobotState(robot_model)); // 创建机器人状态实例
  *start_state = planning_scene->getCurrentState(); // 获取当前状态
  const moveit::core::JointModelGroup* joint_model_group = start_state->getJointModelGroup(req.group_name); // 获取关节模型组
  std::vector<std::string> joint_names = joint_model_group->getVariableNames(); // 获取关节名称
  dof_ = joint_names.size(); // 获取自由度
  std::vector<double> start_joint_values; // 定义起始关节值
  start_state->copyJointGroupPositions(joint_model_group, start_joint_values); // 复制关节组位置


  // 该规划器仅支持一个目标约束
  std::vector<moveit_msgs::Constraints> goal_constraints = req.goal_constraints; // 获取目标约束
  std::vector<moveit_msgs::JointConstraint> goal_joint_constraint = goal_constraints[0].joint_constraints; // 获取关节约束


  std::vector<double> goal_joint_values; // 定义目标关节值
  goal_joint_values.reserve(goal_joint_constraint.size()); // 预留空间


  for (const auto& constraint : goal_joint_constraint) // 遍历关节约束
  {
    goal_joint_values.push_back(constraint.position); // 获取约束位置
  }


  // ==================== 插值
  trajectory_msgs::JointTrajectory joint_trajectory; // 定义关节轨迹
  interpolate(joint_names, start_state, joint_model_group, start_joint_values, goal_joint_values, joint_trajectory); // 调用插值函数


  // ==================== 填充响应
  res.trajectory.resize(1); // 调整轨迹大小
  res.trajectory[0].joint_trajectory.joint_names = joint_names; // 设置关节名称
  res.trajectory[0].joint_trajectory.header = req.start_state.joint_state.header; // 设置头信息
  res.trajectory[0].joint_trajectory = joint_trajectory; // 设置关节轨迹


  res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS; // 设置错误码
  res.processing_time.push_back((ros::WallTime::now() - start_time).toSec()); // 设置处理时间


  res.group_name = req.group_name; // 设置组名称
  res.trajectory_start.joint_state.name = joint_names; // 设置起始关节名称
  res.trajectory_start.joint_state.position = start_joint_values; // 设置起始关节位置


  return true; // 返回true表示成功
}


void LERPInterface::interpolate(const std::vector<std::string>& joint_names, moveit::core::RobotStatePtr& rob_state,
                                const moveit::core::JointModelGroup* joint_model_group,
                                const std::vector<double>& start_joint_vals, const std::vector<double>& goal_joint_vals,
                                trajectory_msgs::JointTrajectory& joint_trajectory) // 插值函数
{
  joint_trajectory.points.resize(num_steps_ + 1); // 调整轨迹点大小


  std::vector<double> dt_vector; // 定义时间步长向量
  for (int joint_index = 0; joint_index < dof_; ++joint_index) // 遍历关节
  {
    double dt = (goal_joint_vals[joint_index] - start_joint_vals[joint_index]) / num_steps_; // 计算时间步长
    dt_vector.push_back(dt); // 添加时间步长
  }


  for (int step = 0; step <= num_steps_; ++step) // 遍历步数
  {
    std::vector<double> joint_values; // 定义关节值
    for (int k = 0; k < dof_; ++k) // 遍历自由度
    {
      double joint_value = start_joint_vals[k] + step * dt_vector[k]; // 计算关节值
      joint_values.push_back(joint_value); // 添加关节值
    }
    rob_state->setJointGroupPositions(joint_model_group, joint_values); // 设置关节组位置
    rob_state->update(); // 更新状态


    joint_trajectory.joint_names = joint_names; // 设置关节名称
    joint_trajectory.points[step].positions = joint_values; // 设置轨迹点位置
  }
}


}  // namespace lerp_interface

这个 solve 函数中的响应是以 moveit_msgs::MotionPlanDetailedResponse 类型准备的，并在 LERPPlanningContext 类中转换为 planning_interface::MotionPlanDetailedResponse 。

此外， PlannerConfigurationSettings 可以用来传递特定于规划器的参数。另一种传递这些参数的方法是使用 ROS 参数服务器，该服务器从 yaml 文件中读取。在本教程中，对于我们的 lerp 规划器，我们在 panda_moveit_config 包中使用 lerp_planning.yaml ，该包仅包含一个参数 num_steps ，该参数在每次调用其 solve 函数时都会加载到 lerp_interface 中。

导出插件 

首先，我们需要使插件可用于 ROS 工具链。为此，应创建一个包含以下选项的插件描述 xml 文件（ emptyplan_interface_plugin_description.xml ），其中包含 library 标签：

<library  path="libmoveit_emptyplan_planner_plugin">
  <class name="emptyplan_interface/EmptyPlanPlanner" type="emptyplan_interface::EmptyPlanPlannerManager" base_class_type="planning_interface::PlannerManager">
   <description>
   </description>
  </class>
</library>

<library path="libmoveit_lerp_planner_plugin">
  <class name="lerp_interface/LERPPlanner" type="lerp_interface::LERPPlannerManager" base_class_type="planning_interface::PlannerManager">
    <description>
      The lerp motion planner plugin which interpolates between two given start and goal joint state values.
    </description>
  </class>
</library>

然后，要导出插件，我们使用上述 xml 文件的地址和 package.xml 文件中的 export 标签：

<export>
   <moveit_core plugin="${prefix}/emptyplan_interface_plugin_description.xml"/>
</export>

请注意，标签的名称 moveit_core 与基类所在的包 planning_interface 的名称相同。

检查插件 

使用以下命令，可以验证新插件是否已正确创建和导出：

rospack plugins --attrib=plugin moveit_core

结果应包含 moveit_planners_lerp 以及插件描述 xml 文件的地址：

moveit_tutorials <ros_workspace>/src/moveit_tutorials/creating_moveit_plugins/lerp_motion_planner/lerp_interface_plugin_description.xml

 插件使用 

在本小节中，我们将解释如何加载和使用我们之前创建的 lerp 规划器。为此，创建了一个名为 lerp_example.cpp 的 ros 节点。

#include <ros/ros.h> // 包含ROS头文件


#include <pluginlib/class_loader.h> // 包含插件库类加载器头文件


// MoveIt
#include <moveit/robot_model_loader/robot_model_loader.h> // 包含MoveIt!机器人模型加载器头文件
#include <moveit/planning_interface/planning_interface.h> // 包含MoveIt!规划接口头文件
#include <moveit/planning_scene/planning_scene.h> // 包含MoveIt!规划场景头文件
#include <moveit/planning_scene_monitor/planning_scene_monitor.h> // 包含MoveIt!规划场景监视器头文件
#include <moveit/planning_pipeline/planning_pipeline.h> // 包含MoveIt!规划管道头文件
#include <moveit/kinematic_constraints/utils.h> // 包含MoveIt!运动学约束工具头文件
#include <moveit_msgs/DisplayTrajectory.h> // 包含MoveIt!显示轨迹消息头文件
#include <moveit_msgs/PlanningScene.h> // 包含MoveIt!规划场景消息头文件
#include <moveit_visual_tools/moveit_visual_tools.h> // 包含MoveIt!可视化工具头文件
#include <moveit/planning_interface/planning_request.h> // 包含MoveIt!规划请求头文件
#include <moveit/robot_model/robot_model.h> // 包含MoveIt!机器人模型头文件
#include <moveit/robot_state/robot_state.h> // 包含MoveIt!机器人状态头文件


int main(int argc, char** argv) // 主函数
{
  const std::string NODE_NAME = "lerp_example"; // 定义节点名称
  ros::init(argc, argv, NODE_NAME); // 初始化ROS节点
  ros::AsyncSpinner spinner(1); // 创建异步旋转器
  spinner.start(); // 启动旋转器
  ros::NodeHandle node_handle("~"); // 创建节点句柄


  const std::string PLANNING_GROUP = "panda_arm"; // 定义规划组名称
  const std::string ROBOT_DESCRIPTION = "robot_description"; // 定义机器人描述参数名称
  robot_model_loader::RobotModelLoaderPtr robot_model_loader(
      new robot_model_loader::RobotModelLoader(ROBOT_DESCRIPTION)); // 创建机器人模型加载器实例


  // 创建规划场景监视器
  planning_scene_monitor::PlanningSceneMonitorPtr psm(
      new planning_scene_monitor::PlanningSceneMonitor(robot_model_loader)); // 创建规划场景监视器实例


  psm->startSceneMonitor(); // 启动场景监视器
  psm->startWorldGeometryMonitor(); // 启动世界几何监视器
  psm->startStateMonitor(); // 启动状态监视器


  moveit::core::RobotModelPtr robot_model = robot_model_loader->getModel(); // 获取机器人模型


  // 创建机器人状态以跟踪当前机器人姿态和规划组
  moveit::core::RobotStatePtr robot_state(
      new moveit::core::RobotState(planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentState())); // 创建机器人状态实例
  robot_state->setToDefaultValues(); // 设置为默认值
  robot_state->update(); // 更新状态


  // 创建关节模型组
  const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP); // 获取关节模型组
  const std::vector<std::string>& link_model_names = joint_model_group->getLinkModelNames(); // 获取链接模型名称
  ROS_INFO_NAMED(NODE_NAME, "end effector name %s\n", link_model_names.back().c_str()); // 输出末端执行器名称


  // 设置规划器
  std::string planner_plugin_name = "lerp_interface/LERPPlanner"; // 定义规划器插件名称
  node_handle.setParam("planning_plugin", planner_plugin_name); // 设置规划器插件参数


  // 创建规划管道
  planning_pipeline::PlanningPipelinePtr planning_pipeline(
      new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters")); // 创建规划管道实例


  // ================================ 设置起始和目标关节状态
  planning_interface::MotionPlanRequest req; // 创建运动规划请求
  planning_interface::MotionPlanResponse res; // 创建运动规划响应
  req.group_name = PLANNING_GROUP; // 设置规划组名称


  // 获取起始状态的关节值并设置到请求中
  std::vector<double> start_joint_values; // 定义起始关节值
  robot_state->copyJointGroupPositions(joint_model_group, start_joint_values); // 复制关节组位置
  req.start_state.joint_state.position = start_joint_values; // 设置起始状态的关节位置


  // 目标约束
  std::vector<double> goal_joint_values = { 0.8, 0.7, 1, 1.3, 1.9, 2.2, 3 }; // 定义目标关节值
  robot_state->setJointGroupPositions(joint_model_group, goal_joint_values); // 设置关节组位置
  robot_state->update(); // 更新状态
  moveit_msgs::Constraints joint_goal =
      kinematic_constraints::constructGoalConstraints(*robot_state, joint_model_group); // 构建目标约束
  req.goal_constraints.clear(); // 清空目标约束
  req.goal_constraints.push_back(joint_goal); // 添加目标约束
  req.goal_constraints[0].name = "goal_pos"; // 设置目标约束名称


  // 设置关节容差
  std::vector<moveit_msgs::JointConstraint> goal_joint_constraint = req.goal_constraints[0].joint_constraints; // 获取关节约束
  for (std::size_t x = 0; x < goal_joint_constraint.size(); ++x) // 遍历关节约束
  {
    ROS_INFO_STREAM_NAMED(NODE_NAME, " ======================================= joint position at goal: "
                                         << goal_joint_constraint[x].position); // 输出目标关节位置
    req.goal_constraints[0].joint_constraints[x].tolerance_above = 0.001; // 设置上容差
    req.goal_constraints[0].joint_constraints[x].tolerance_below = 0.001; // 设置下容差
  }


  // ================================ 可视化工具
  namespace rvt = rviz_visual_tools; // 定义rviz_visual_tools命名空间别名
  moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC, psm); // 创建可视化工具实例
  visual_tools.loadRobotStatePub("/display_robot_state"); // 加载机器人状态发布器
  visual_tools.enableBatchPublishing(); // 启用批量发布
  visual_tools.deleteAllMarkers();  // 清除所有旧标记
  visual_tools.trigger(); // 触发更新
  visual_tools.loadRemoteControl(); // 加载远程控制


  /* RViz提供多种类型的标记，在本演示中我们将使用文本、圆柱体和球体 */
  Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity(); // 定义文本姿态
  text_pose.translation().z() = 1.75; // 设置文本位置
  visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE); // 发布文本


  /* 批量发布用于减少发送到RViz的大型可视化的消息数量 */
  visual_tools.trigger(); // 触发更新


  /* 我们还可以使用visual_tools等待用户输入 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo"); // 提示用户输入


  // ================================ 规划上下文
  {
    planning_scene_monitor::LockedPlanningSceneRO lscene(psm); // 锁定规划场景
    /* 现在，调用管道并检查规划是否成功。*/
    planning_pipeline->generatePlan(lscene, req, res); // 生成规划
  }
  /* 检查规划是否成功 */
  if (res.error_code_.val != res.error_code_.SUCCESS) // 如果规划失败
  {
    ROS_ERROR_STREAM_NAMED(NODE_NAME, "Could not compute plan successfully"); // 输出错误信息
    return 0; // 返回0
  }


  visual_tools.prompt("Press 'next' to visualzie the result"); // 提示用户输入


  // ================================ 可视化轨迹
  //  visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
  // visual_tools.trigger();


  ros::Publisher display_publisher =
      node_handle.advertise<moveit_msgs::DisplayTrajectory>("/display_planned_path", 1, true); // 创建显示轨迹的发布器
  moveit_msgs::DisplayTrajectory display_trajectory; // 定义显示轨迹消息


  /* 可视化轨迹 */
  moveit_msgs::MotionPlanResponse response; // 定义运动规划响应
  res.getMessage(response); // 获取响应消息


  moveit_msgs::RobotTrajectory solution_traj = response.trajectory; // 获取解决方案轨迹
  int number_of_steps = solution_traj.joint_trajectory.points.size(); // 获取轨迹中的步数
  ROS_DEBUG_NAMED(NODE_NAME, "number of timesteps in the solution trajectory: %i", number_of_steps); // 输出轨迹中的步数


  for (int step_num = 0; step_num < number_of_steps; ++step_num) // 遍历轨迹中的每一步
  {
    std::vector<double> solution_positions; // 定义解决方案位置向量
    solution_positions = solution_traj.joint_trajectory.points[step_num].positions; // 获取每一步的关节位置
    std::stringstream sst; // 定义字符串流
    for (double solution_position : solution_positions) // 遍历每个关节位置
    {
      sst << solution_position << " "; // 将关节位置添加到字符串流中
    }
    ROS_INFO_STREAM_NAMED(NODE_NAME, sst.str()); // 输出关节位置
  }


  display_trajectory.trajectory_start = response.trajectory_start; // 设置轨迹起始状态
  display_trajectory.trajectory.push_back(response.trajectory); // 添加轨迹到显示轨迹中
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); // 发布轨迹线
  visual_tools.trigger(); // 触发更新
  display_publisher.publish(display_trajectory); // 发布显示轨迹


  /* 我们还可以使用visual_tools等待用户输入 */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo"); // 提示用户输入
}

第一步是通过以下代码行获取与请求的规划组以及规划场景相关的机器人的状态和关节组：

moveit::core::RobotStatePtr robot_state(new moveit::core::RobotState(robot_model));
const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
const std::vector<std::string>& joint_names = joint_model_group->getVariableNames();
planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

下一步是使用 pluginlib 加载规划器，并将参数 planner_plugin_name 设置为我们创建的参数：

boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;
planning_interface::PlannerManagerPtr planner_instance;
std::string planner_plugin_name =  "lerp_interface/LERPPlanner";
node_handle.setParam("planning_plugin", planner_plugin_name);

加载规划器后，是时候为运动规划问题设置起始状态和目标状态了。起始状态是机器人的当前状态，设置为 req.start_state 。另一方面，目标约束是通过使用目标状态和关节模型组创建 moveit_msgs::Constraints 来设置的。此约束被传递给 req.goal_constraint 。以下代码显示了如何执行这些步骤：

// Get the joint values of the start state and set them in request.start_state
std::vector<double> start_joint_values;
robot_state->copyJointGroupPositions(joint_model_group, start_joint_values);
req.start_state.joint_state.position = start_joint_values;


// Goal constraint
moveit::core::RobotState goal_state(robot_model);
std::vector<double> joint_values = { 0.8, 0.7, 1, 1.3, 1.9, 2.2, 3 };
goal_state.setJointGroupPositions(joint_model_group, joint_values);
moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);

到目前为止，我们已经加载了规划器并为运动规划问题创建了起始状态和目标状态，但我们尚未解决该问题。通过给定的起始状态和目标状态信息，在关节状态下解决运动规划问题是通过创建一个 PlanningContext 实例并调用其 solve 函数来完成的。请记住，传递给此 solve 函数的响应应为 planning_interface::MotionPlanResponse 类型：

planning_interface::PlanningContextPtr context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);

最后，要运行此节点，我们需要在启动文件夹中 roslaunch lerp_example.launch。此启动文件通过传递 lerp 作为规划器的名称来启动包 panda_moveit_config 的 demo.launch 。然后， lerp_example 被启动， lerp_planning.yaml 被加载以将 lerp 特定参数设置到 ROS 参数服务器。

示例控制器管理器插件 

MoveIt 控制器管理器，有点名不副实，是自定义低级控制器的接口。更好的理解方式是将它们视为控制器接口。对于大多数用例，如果您的机器人控制器已经提供了 FollowJointTrajectory 的 ROS 操作，包含的 MoveItSimpleControllerManager https://github.com/moveit/moveit2/tree/main/moveit_plugins/moveit_simple_controller_manager 就足够了。如果您使用 ros_control，包含的 MoveItRosControlInterface  https://github.com/moveit/moveit2/tree/main/moveit_plugins/moveit_ros_control_interface 也是理想的选择。 

但是，对于某些应用程序，您可能需要一个更自定义的控制器管理器。此处提供了一个用于启动自定义控制器管理器的示例模板。https://github.com/moveit/moveit2_tutorials/blob/main/doc/examples/controller_configuration/src/moveit_controller_manager_example.cpp

示例约束采样器插件 

• 创建一个 ROBOT_moveit_plugins 包，并在其中为您的 ROBOT_constraint_sampler 插件创建一个子文件夹。修改 ROBOT_moveit_plugins/ROBOT_moveit_constraint_sampler_plugin 提供的模板。

• 在你的 ROBOT_moveit_config/launch/move_group.launch 文件中，在 <node name="move_group"> 内，添加参数：

<param name="constraint_samplers" value="ROBOT_moveit_constraint_sampler/ROBOTConstraintSamplerAllocator"/>

• 现在当你启动 move_group 时，它应该默认使用你的新约束采样器。