为MoveIt MotionPlanning插件添加Waypoints功能

---

背景

MoveIt 的 MotionPlanning 插件在 RViz 中通过可视化的方式对机械臂进行路径规划和仿真运行，且当挂载了机械臂物理控制器后，其能够实现路径规划以及对机械臂的物理控制，实现了当下较火的“数字孪生”概念。

其中“Planning”页面的使用频率最高，用户通过对 RViz 中仿真机械臂模型的拖拽交互能够实现对机械臂末端的路径规划和运动执行。有时候用户希望通过给出一系列点并规划为一个完整路径，而该场景功能 MotionPlanning plugin 暂时没有提供，因此本文目的在于为 MotionPlanning 创建一个能够接收用户输入一系列点，即 Waypoints，然后基于这些点做路径规划并执行的功能。

添加 waypoints

Waypoints 的 Cartesian 路径规划

本文的源码实现都放入了 GitHub 读者可先行下载一并参考。

---

1. 实现路径

MotionPlanning 是 RViz 的一个插件，因此为 Waypoints 再构建一个 RViz 插件是最自然的想法，由于有用户与界面间的交互过程，可以选择使用 panel 的方式进行构建。另一种方式是重构 MotionPlanning ，为其添加一个 tab 页面来实现 Waypoints 的功能。

RViz插件开发基础可以参看官网教程：
http://wiki.ros.org/pluginlib/Tutorials/Writing%20and%20Using%20a%20Simple%20Plugin
https://ros-planning.github.io/moveit_tutorials/doc/creating_moveit_plugins/plugin_tutorial.html

两种方式各有利弊，第一中方式程序更加独立，但会占用 RViz 整个界面额外更多的空间，让本不富裕的控制空间雪上加霜；第二种方式能够避免第一种方式带来的问题，且能够充分重用 MotionPlanning 已有的接口和函数，缺点是需要对 MotionPlanning 源码有充分的理解。

本文选择了第二种方式，为 MotionPlanning 插件添加另一个 Waypoints 页面，这样能充分利用已有的 move group interface 的基础设施及环境配置。

2. MotionPlanning 插件结构

目标插件源码路径

MotionPlanning 插件在源码包中完整路径为“moveit\moveit_ros\visualization”

visualization package 中实现了四个 RViz 插件，分别为：

• motion_planning_rviz_plugin_description.xml
• planning_scene_rviz_plugin_description.xml
• robot_state_rviz_plugin_description.xml
• trajectory_rviz_plugin_description.xml

即我们在添加插件时所看父节点 moveit_ros_visualization 下的四个插件：

其中 MotionPlanning 即是本文要重构拓展的目标插件。

目标插件属性

回忆 RViz 插件构建过程，当我们要导出某个插件时，需要在其 package.xml 中显示的注明要导出插件的描述文件，本文目标插件 MotionPlanning 其描述文件为“motion_planning_rviz_plugin_description.xml”：

  
    
      Displays information about a planning scene and can animate solution paths within that planning scene.
    
  

可以从该文件得知，插件的文件名为“libmoveit_motion_planning_rviz_plugin.so”、插件在RViz中显示名为“MotionPlanning”、插件类名为“MotionPlanningDisplay”、插件的基类名为“Display”。得到上述信息后，能够帮助我们在阅读源码前构建一个宏观的层次结构。

目标插件代码结构

目标插件 MotionPlanning 所在目录“motion_planning_rviz_plugin”中有11个 cpp 文件，但其类结构层次关系要简单得多，如下图所示：

类“MotionPlanningDisplay”负责 Rviz 中 Display 的属性和行为设置、类“MotionPlanningFrame”负责创建插件各个页面及其中控件属性和行为设置，其中包括了用于机械臂路径规划及执行的 move group interface 逻辑操作、类“MotionPlanningFrameJointsWidget”区别于其他页面，没有将其控件的属性和行为设置放于类“MotionPlanningFrame”中，而是作为一个独立的存在，这也是本文实现 Waypoints 页面的形式。

命名空间“Ui”的类为 Qt Designer 设计界面所自动生成的类，其 ui 文件存储于目录“visualization\motion_planning_rviz_plugin\src\ui”，命名为“motion_planning_rviz_plugin_frame.ui”。使用“Qt Designer”的打开方式即能打开编辑

---

3. 添加 Waypoints 页面

前面提到整个 MotionPlanning 插件的各个页面中，页面 Joints 是一个独立存在的页面，即其有独立的 ui，有独立的类。本文欲新增的 Waypoints 页面将完全参照其来实现。

Qt ui

首先是为 Waypoints 添加自己的界面 ui，通过 Qt Designer 实现，完成后将 ui 文件存储到目录“visualization\motion_planning_rviz_plugin\src\ui”。

ui 文件生成后，为了能够自动生成其对应的类头文件，需要将其 ui 文件添加到 CMakeLists 中，打开文件“visualization\motion_planning_rviz_plugin\CMakeLists.txt”，添加 ui 文件在 package 中的相对路径及其名称：

src/ui/motion_planning_rviz_plugin_frame_waypoints.ui

在终端中输入命令进行编译，以生成 ui 的类头文件：

cd <your_workspace>
catkin build
source <your_workspace>/devel/setup.bash

编译完成后在目录“/build/moveit_ros_visualization/motion_planning_rviz_plugin/”中看到自动生成的 ui 的类头文件“ui_motion_planning_rviz_plugin_frame_waypoints.h”。

编译命令 catkin build 或者 catkin_make 请根据自身 workspace 的编译环境选择

Waypionts 页面实现

Waypoints 的页面实现大部分内容为 Qt 控件的属性和行为，本文不再详述，读者可以到 GitHub 下载完整源代码详细参考。这里只把前面提到的选择以加入页面的实现方式所涉及到复用 MotionPlanning 插件中 move group interface 的过程做说明。

类构建

首先参照页面 Joints，为 Waypoints 构建类“MotionPlanningFrameWaypointsWidget”，形成如图的层次关系：

集成页面

前述提到所有页面的创建都由类“MotionPlanningFrame”负责，在其实现文件“motion_planning_frame.cpp”中，可以看到集成页面 Joints 的代码过程：

  // add more tabs
  joints_tab_ = new MotionPlanningFrameJointsWidget(planning_display_, ui_->tabWidget);
  ui_->tabWidget->insertTab(2, joints_tab_, "Joints"); // I prefer joints following planning tab
  connect(planning_display_, SIGNAL(queryStartStateChanged()), joints_tab_, SLOT(queryStartStateChanged()));
  connect(planning_display_, SIGNAL(queryGoalStateChanged()), joints_tab_, SLOT(queryGoalStateChanged()));

新建的 Waypoints 页面也通过类似方式进行集成：

  // tab waypoints
  waypoints_tab_ = new MotionPlanningFrameWaypointsWidget(planning_display_, ui_->tabWidget);
  ui_->tabWidget->insertTab(2, waypoints_tab_, "Waypoints");
  waypoints_tab_->setPlanningGroupName(QString::fromStdString(planning_display_->getCurrentPlanningGroup()));

最后，为了编译能包括新增类，需要将其添加到 CMakeLists 文件中：

在终端中输入命令编译新增类：

cd <your_workspace>
catkin build

编译完成后，运行一个 moveit_config 的 launch 文件，将能看到新增加的 Waypoints 页面。以 panda_moveit_config 的 demo.launch 为例，运行后将看到 Waypoints 页面跟随在 Planning 页面之后：

move group interface 的复用

如前述提到这是新增页面最大的优势，不需要我们再次编写路径规划及控制的逻辑代码，直接使用 MotionPlanning 已有的 move group interface 即可，本文是通过回调函数的方式实现的。

Plan、Execute、PlanAndExecute、Stop 四个功能按钮是 Waypoints 规划路径和执行的核心，且他们在页面 Planning 中都有完备的实现。因此，目标是复用他们在页面 Planning 的已有实现。为了不形成循环依赖的局面，本文使用了回调函数，即将 Planning 页面所实现的上述四个过程的函数注册给 Waypoints 的类。当 Waypoints 自身的信号触发时将调用所注册的函数，即 Planning 页面中的函数。

首先在 Waypoints 类中声明回调函数的类型，这里传入的参数有系列点的 vector，以及 Waypoints 类自身的类指针：

using PoseUiCallback = std::function<void(const std::vector<geometry_msgs::Pose>&, Ui::MotionPlanningFrameWaypointsUI*)>;
using UiCallback = std::function<void(Ui::MotionPlanningFrameWaypointsUI*)>;
using NullCallback = std::function<void(void)>;

随后在 Waypoints 类中添加注册函数的接口及其实现（实现只示例了 Plan 函数）：

void registerPlan(const PoseUiCallback& Func);
void registerExecute(const UiCallback& Func);
void registerPlanAndExecute(const PoseUiCallback& Func);
void registerStop(const NullCallback& Func);

...
void MotionPlanningFrameWaypointsWidget::registerPlan(const PoseUiCallback& Func)
{
	func_plan_ = Func;
}
...

随后，在创建 Waypoints 类的过程中调用函数注册的接口，将 MotionPlanning 的函数注册给类 Waypoints。为了避免在源码中造成大面积的修改，从而造成后续更新合并困难的不利局面，注册过程本文使用了 lamda 表达式，把重构内容集中到同一个地方（只示例了 Plan 函数的注册过程）：

  // register plan and execute related functions
  waypoints_tab_->registerPlan([=](const std::vector<geometry_msgs::Pose>& Waypoints, Ui::MotionPlanningFrameWaypointsUI* UiPtr)
  {
    publishSceneIfNeeded();

    planning_display_->addBackgroundJob([this, &Waypoints, UiPtr]
    {
      if (!move_group_)
      {
        return;
      }

      waypoints_tab_->configureForPlanning(move_group_);
      configureWorkspace();
      if (static_cast<bool>(planning_display_))
      {
        planning_display_->dropVisualizedTrajectory();
      }
      planning_display_->rememberPreviousStartState();

      UiPtr->result_label->setText("Planning...");
      if (computeCartesianPlan(Waypoints))
      {
        UiPtr->execute_button->setEnabled(true);
        UiPtr->result_label->setText(QString("Time: ").append(QString::number(current_plan_->planning_time_, 'f', 3)));
      }
      else
      {
        current_plan_.reset();
        UiPtr->result_label->setText("Failed");
      }

      Q_EMIT planningFinished();
    }, "compute plan");
  });

当 Waypoints 类的控件信号被触发时，所被注册的类 MotionPlanning 中的实现过程将被调用并执行：

void MotionPlanningFrameWaypointsWidget::planButtonClicked()
{
	std::vector<geometry_msgs::Pose> waypoints;
	getWaypoints(waypoints);

	if (func_plan_)
	{
		func_plan_(waypoints, ui_);
	}
}

---

4. 使用

依据MoveIt安装方式，使用时会有少许准备工作。

MoveIt 源码安装

如果是源码安装的 MoveIt 则事情会简单很多，只需要 clone 本文 GitHub 上源码替换原有部分，然后编译即可。

MoveIt 包安装

如果是采用包方式的安装，则先需要将之前的动态库“libmoveit_motion_planning_rviz_plugin.so”以及“libmoveit_motion_planning_rviz_plugin_core.so”失效，本文采用为其添加一个 bk 后缀，这样后续回复会非常便利：

sudo mv /opt/ros/melodic/lib/libmoveit_motion_planning_rviz_plugin.so /opt/ros/melodic/lib/libmoveit_motion_planning_rviz_plugin.so.bk
sudo mv /opt/ros/melodic/lib/libmoveit_motion_planning_rviz_plugin_core.so /opt/ros/melodic/lib/libmoveit_motion_planning_rviz_plugin_core.so.bk

随后，clone 本文 GitHub 上源码到 workspace\src：

cd /src
git clone -b melodic https://github.com/xinjuezou-whi/moveit_ros_visualization.git

编译：

cd ..
catkin build
source /devel/setup.bash

编译命令 catkin build 或者 catkin_make 请根据自身 workspace 的编译环境选择

编译完成后，即可通过 moveit config 的 launch 文件运行查看 Waypoints 页面。

添加 waypoints 并规划路径

在 Waypoints 列表中添加目标点后，即可开始路径的规划，“current”选项勾选后将获取当前终端坐标并添加到 Waypoints 中：

循环执行

当和现实中的机械臂进行控制时，可以勾选“Loop Execution”实现对 Waypoints 路径的重复执行过程：

---

总结

ROS 精神在于开源贡献，人人都可以分享自身的成果，同时也享受他人成果带来的便利。本文将自定义的 Waypoints 插件实现过程做了一个分享，希望借此能够给同样运用 ROS 平台的读者多少带来一些参考。

本文说明的源代码，可以完整的在 GitHub 下载，如果文中出现了描述错误以及不清晰的地方，欢迎指正，共同交流：[email protected]