创建 MoveIt 插件

本页详细介绍了在 ROS 中添加插件的一般方法。两个必要元素是基类和插件类：插件类继承自基类并重写其虚函数。为此所用的主要库是 pluginlib。本教程涵盖三种不同类型的插件：运动规划器、控制器管理器和约束采样器。

运动规划器插件

本节展示如何将一个新的运动规划器作为插件添加到 MoveIt 中。MoveIt 中的基类是 planning_interface，任何新的规划器插件都应继承自它。出于演示目的，我们创建了一个线性插值规划器（lerp），它在关节空间中规划两个状态之间的运动。由于包含了必要的基础内容，这个规划器可以作为添加任何新规划器的起点。本教程的完整源文件可在此处获取。下图简要展示了在 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);

接下来，我们创建重写 PlanningContext 类函数的 LERPPlanningContext 类。该类会重写 solve 函数，规划器在此函数中求解问题并返回解。由于 solve 函数的实现可能涉及规划器代码库中的许多类，因此单独创建一个 LERPInterface 类会更具可读性，solve 方法的实际实现将在该类中完成。这个类本质上就是新运动规划算法的入口点。solve 函数的响应首先以 moveit_msgs::MotionPlanDetailedResponse 类型构造，然后在 LERPPlanningContext 类中转换为 planning_interface::MotionPlanDetailedResponse。

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

导出插件

首先，需要让 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>

然后，在 package.xml 文件中通过 export 标签引用上述 xml 文件的路径来导出插件：

<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 节点。第一步是获取与请求的规划组相关的机器人状态和关节组，以及规划场景：

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_);

最后，要运行这个节点，需要启动 launch 文件夹中的 lerp_example.launch。该 launch 文件通过传入 lerp 作为规划器名称来启动 panda_moveit_config 包的 demo.launch。随后 lerp_example 节点被启动，同时加载 lerp_planning.yaml，将 lerp 特有的参数设置到 ROS 参数服务器上。

控制器管理器插件示例

MoveIt 控制器管理器——在某种程度上说是一个误称——是你连接自定义底层控制器的接口。把它理解为 控制器接口 更为恰当。对于大多数用例，如果你的机器人控制器已经为 FollowJointTrajectory 提供了 ROS action，那么自带的 MoveItSimpleControllerManager 就足够了。如果使用 ros_control，自带的 MoveItRosControlInterface 也是理想之选。

然而，对于某些应用，你可能需要一个更定制化的控制器管理器。此处提供了一个用于编写自定义控制器管理器的示例模板。

约束采样器插件示例

创建一个 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 后，它应该会默认使用你新的约束采样器。