创建 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);

接下来，我们创建 LERPPlanningContext 类，它会重写 PlanningContext 的函数。这个类将重写 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 文件中使用上述 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 节点。第一步是通过以下代码行获取与请求的规划组以及规划场景相关的机器人的状态和关节组：

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并调用其求解函数来完成的。请记住，传递给此求解函数的响应应为以下类型planning_interface::MotionPlanResponse：

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

要运行这个节点，我们需要在 launch 文件夹中执行 roslaunch lerp_example.launch。这个启动文件会启动 panda_moveit_config 包的 demo.launch，并将 lerp 作为规划器的名称传递给它。接着，lerp_example 会被启动，同时 lerp_planning.yaml 文件也会被加载，以将 LERP 特定的参数设置到 ROS 参数服务器中。

控制器管理器插件示例

MoveIt 控制器管理器，有点用词不当，是自定义低级控制器的接口。更好的理解方式是将它们称为控制器接口。对于大多数用例，如果您的机器人控制器已经为 FollowJointTrajectory 提供了 ROS 操作，则随附的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 时，它应该默认为您的新约束采样器。