Moveit!中move_group的运动规划C++接口move_group_interface详解

大多数文章介绍move_group_interface都停留在对其成员函数的使用，plan()、execute()、move()等，但对其内部结构流程探究的很少。move_group_interface是Action的消息机制，并连接ompl库进行的运动规划和轨迹生成。下面来对move_group_interface中的plan()函数做深入了解，并借此学习Moveit!的Action。

1、move_group简介

move_group是Moveit!的核心组件，综合机器人的各独立组件，为用户提供一系列的动作指令和服务。
其中用户接口move_group_interface.cpp是C++的程序接口。moveit_commander.py是为Python提供的API 。GUI是rviz的插件，三种接口都一样，可以布置场景，设置碰撞，指定目标点等操作。
ROS参数服务器为move_group提供三种信息，URDF是机器人模型描述，SRDF是模型描述信息，Config是机器人配置的关节限位，运动学插件和运动规划插件等。
move_group和机器人之间的通讯通过Action和Topic。话题通讯很简单不再说明，下面简单介绍动作的通讯。

2、Action简单学习

2.1、action消息定义

Action是一种类似Service的问答通讯机制，不过Action带有连续反馈，可以在执行过程中反馈任务进度，也可中途中止任务。action通过*.action文件定义，格式如下：

int Goal //定义Action的目标
---
char Feedback //定义Action的反馈
---
bool Result //定义Action的结果

将其命名为text.action并链接编译后，会生成一系列.msg文件。

2.2、action使用：客户端

客户端用于发出action的目标，接收反馈和最终结果。

#include <actionlib/client/simple_action_client.h>
#include <ros/ros.h>
#include "move_msgs/text.h"
typedef actionlib::SimpleActionClient<moveit_msgs::text> Client;
void donecb (const moveit_msgs::textGoalState& state,
						const moveit_msgs::textResultConstPtr& re)
{
	ROS_INFO("Finish!");
	ros::shutdown();
}
void activecb()
{
	ROS_INFO("Start")
}
void feedbackcd (const moveit_msgs::textConstPtr& fb)
{
	ROS_INFO("feedback is %c", fb->feedback);
}
int main (int argc, char ** argv)
{
	ros::init(argc, argv, "text_client");
	//定义客户端
	Client client("text", true);
	//等待服务器连接
	client.waitForServer();
	//定义目标变量
	moveit_msgs::textGoal goal;
	//填充目标信息
	goal.Goal = 111;
	//发送目标消息至服务端，并设置三个回调函数
	client.sendGoal(goal, &donecb, &actioncb, &feedbackcb);
	ros::spin();
	return 0;
}

2.3、action使用：服务端

#include <actionlib/client/simple_action_server.h>
#include <ros/ros.h>
#include "move_msgs/text.h"
typedef actionlib::SimpleActionServer<moveit_msgs::text> Server;
void execute(const moveit_msgs::textGoalConstPtr& goal, Sever* as)
{
	ros::Rate r(1);
	moveit_msgs::textFeedback fb;
	for (int i=1; i < 10; i++)
	{
		fb.feedback = 'Y';
		as->publishFeedback(fb);
		r.sleep();
	}
	as->setSucceeded();
}
int main (int argc, char ** argv)
{
	ros::init(argc, argv,"text_server");
	ros::NodeHandle n;
	//定义服务器并设置回调函数
	Server server(n, "text", boost::bind(&execute, _1, %server), false);
	//服务器开始运行
	server.start();
	ros::spin();
	return 0;
}

3、move_group_interface运动规划详解

3.1、机械臂规划例程结构

int main(int argc, char** argv)
{
    ros::init(argc, argv, "plan_text");
    ros::NodeHandle nh;
    ros::AsyncSpinner spin(1);
    spin.start();
	//通过movegroup设置一个机械臂组，对这个名为arm的机械臂组进行控制规划
	moveit::planning_interface::MoveGroupInterface group("arm");
	//创建一个变量，名为my_plan，这个Plan是一个结构体，包含机械臂运动的状态和时间信息。
	moveit::planning_interface::MoveGroupIntreface::Plan my_plan;
	//在设置目标点后，用plan函数，输入为刚刚定义的变量，如果路径成功规划完成，则返回true，并且my_plan是已经规划号的路径。
	moveit::planning_interface::MoveitErrorCode success = group.plan(my_plan);
	if (success)
	//如果成功，则开始执行，rviz中的机械臂开始运动
		group.execute(my_plan);
	return 0;
}

接下来我们深入学习底层代码，因此必须源码安装Moveit!和OMPL，如果是二进制安装要卸载重装，参考我之前写的博客Ubuntu1804下的Melodic版本Moveit和OMPL的源码安装，并自定义规划算法在Moveit上使用

3.2、深入Plan函数

刚刚我们定义group的时候用的就是move_group中的C++接口：MoveGroupInterface这是一个类，那么这个Plan运动规划函数就是MoveGroupInterface中的一个成员函数。打开move_group_interface.cpp，第793行定义了此函数

moveit::core::MoveItErrorCode plan(Plan& plan)

可以看到此函数的输入为结构体Plan的引用，输出为moveit错误编码，如果为真就是成功无错误。plan函数的主干摘选如下：

moveit::core::MoveItErrorCode plan(Plan& plan)
  {
    if (!move_action_client_->isServerConnected())
      ROS_WARN_STREAM_NAMED(LOGNAME, "move action server not connected");
    moveit_msgs::MoveGroupGoal goal;
    constructGoal(goal);
    goal.planning_options.plan_only = true;
    move_action_client_->sendGoal(goal);  //发送Action的目标，
    if (!move_action_client_->waitForResult()) //等待Action结果
      ROS_INFO_STREAM_NAMED(LOGNAME, "MoveGroup action returned early");
    if (move_action_client_->getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
    {
      plan.trajectory_ = move_action_client_->getResult()->planned_trajectory;
      plan.start_state_ = move_action_client_->getResult()->trajectory_start;
      plan.planning_time_ = move_action_client_->getResult()->planning_time;
      return move_action_client_->getResult()->error_code;
    }
    else
      return move_action_client_->getResult()->error_code;
  }

首先如人眼帘的是move_action_client_。从名字可以看到这是move_group的一个动作客户端。他的定义如下：

std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>> move_action_client_;

unique_ptr是智能指针，只能在这个作用域中使用，且不能被多个unipue_ptr指向，保证内存不被泄漏。智能指针unipue_ptr
继续输入的内容和前面action的定义一样，只不过客户端的类型为moveit_msgs::MoveGroupAction。
这个MoveGroupAction是在我们完成.action编译后自动生成的一个头文件：MoveGroupAction.h。打开这个头文件可以看到这是一个结构体存放着三个变量，分别是goal、feedback、result，对应于我们.action的三组变量。
回到plan函数，接下来定义一个变量goal，其类型MoveGroupGoal是这个Action的目标，是一个结构体。

moveit_msgs::MoveGroupGoal goal;
constructGoal(goal);

如果我们在刚刚的MoveGroupAction.h里面一直深入的话。可以看到他也包含有MoveGroupGoal这个结构体。这个结构体包含两个变量request和planning_options，construceGoal函数是对变量goal的request做一些初始化赋值。具体赋值内容不做深究了。
再对goal赋值完毕后，再发送action目标至服务器。sendGoal()函数。然后waitForResult()等待服务端的结果，如果有结果了并且getState()有值，则开始对Plan的引用做赋值拷贝。将Action的getResult结果赋予给plan，最后把error_code输出。完成plan。move_group客户端基本正清楚了，后面看服务端的源码。

3.3、MoveGroupAction的服务端

服务端在接受到客户端发送的目标后，开始进行运动规划，待规划完成后把规划信息保存并返回完成的信号。
打开move_cation_capability.cpp,其中的initialize()函数定义了服务器并设置了回调函数：executeMoveCallback()。
进入executeMoveCallback后，判断是只规划还是规划&执行。对应于rviz上的选项。我们这里只做运动规划，因此进入executeMoveCallbackPlanOnly()函数，此函数的输入为goal和action_res，其中action_res为MoveGroupResult类型。程序摘选如下：

void MoveGroupMoveAction::executeMoveCallback(const moveit_msgs::MoveGroupGoalConstPtr& goal)
{
  moveit_msgs::MoveGroupResult action_res;
  if (goal->planning_options.plan_only || !context_->allow_trajectory_execution_)
    executeMoveCallbackPlanOnly(goal, action_res);
  else
    executeMoveCallbackPlanAndExecute(goal, action_res);
  if (action_res.error_code.val == moveit_msgs::MoveItErrorCodes::SUCCESS)
    move_action_server_->setSucceeded(action_res, response);
}

继续查看executeMoveCallbackPlanOnly函数：

void MoveGroupMoveAction::executeMoveCallbackPlanOnly(const moveit_msgs::MoveGroupGoalConstPtr& goal,
                                                      moveit_msgs::MoveGroupResult& action_res)
{
  ROS_INFO_NAMED(getName(), "Planning request received for MoveGroup action. Forwarding to planning pipeline.");
 //运动规划的场景信息
  const planning_scene::PlanningSceneConstPtr& the_scene =
      (moveit::core::isEmpty(goal->planning_options.planning_scene_diff)) ?
          static_cast<const planning_scene::PlanningSceneConstPtr&>(lscene) :
          lscene->diff(goal->planning_options.planning_scene_diff);
  planning_interface::MotionPlanResponse res;
  // Select planning_pipeline to handle request
  const planning_pipeline::PlanningPipelinePtr planning_pipeline = resolvePlanningPipeline(goal->request.pipeline_id);
  try
  {
    planning_pipeline->generatePlan(the_scene, goal->request, res);
  }
  convertToMsg(res.trajectory_, action_res.trajectory_start, action_res.planned_trajectory);
  action_res.error_code = res.error_code_;
  action_res.planning_time = res.planning_time_;
}

先定义一个规划通道，用于连接ompl库，很明显可以找到generatePlan函数，用于生成路径的。进入generatePlan函数又是一大串程序，核心应该是类planning_interface::PlanningContextPtr的操作和slove函数的执行。实在弄不动了，不过已知的是，要配置并连接ompl库。

3.4、OMPL库的配置和自定义运动规划算法

进入model_based_planning_conterxt.cpp，有一个config函数，此函数用于ompl相关设置，最后程序落脚于useConfig()函数，这个函数就是判断你采用的哪种运动规划算法，并且将一些参数和场景规划信息加载出来。设置好目标点后，最后和运动规划的setup()函数连接。
在跳出useConfig()函数后，就开执行运动规划的setup()，setup又调用他的slove()函数，进行运动规划。

void ompl_interface::ModelBasedPlanningContext::configure(const ros::NodeHandle& nh, bool use_constraints_approximations)
{
  loadConstraintApproximations(nh);
  // convert the input state to the corresponding OMPL state
  ompl::base::ScopedState<> ompl_start_state(spec_.state_space_);
  spec_.state_space_->copyToOMPLState(ompl_start_state.get(), getCompleteInitialRobotState());
  ompl_simple_setup_->setStartState(ompl_start_state);
  ompl_simple_setup_->setStateValidityChecker(ob::StateValidityCheckerPtr(new StateValidityChecker(this)));

  useConfig();
  if (ompl_simple_setup_->getGoal())
    ompl_simple_setup_->setup();
}

你的路径规划算法主要就在对应的solve()函数填写就行了。

4、总结

最后 的generatePlan函数怎么ompl库连接还没有找到，有时间再深入以下。
其实move_group_interface.cpp共定义了四个Action：

  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::MoveGroupAction>> move_action_client_;
  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::ExecuteTrajectoryAction>> execute_action_client_;
  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PickupAction>> pick_action_client_;
  std::unique_ptr<actionlib::SimpleActionClient<moveit_msgs::PlaceAction>> place_action_client_;

今天我们只看了move，主要是运动规划生成路径。后面还有执行、抓取、放置，不过思路都是一样的。在move_group_interface.cpp定义其动作的客户端，然后在另外的程序上定义对应的动作服务端。放一张节点图，可以看到确实有四个动作节点。