开发: C++知识库 Java知识库 JavaScript Python PHP知识库人工智能区块链大数据移动开发嵌入式开发工具数据结构与算法开发测试游戏开发网络协议系统运维
教程: HTML教程 CSS教程 JavaScript教程 Go语言教程 JQuery教程 VUE教程 VUE3教程 Bootstrap教程 SQL数据库教程 C语言教程 C++教程 Java教程 Python教程 Python3教程 C#教程
数码: 电脑笔记本显卡显示器固态硬盘硬盘耳机手机 iphone vivo oppo 小米华为单反装机图拉丁

-> C++知识库 -> Moveit!中move_group的运动规划C++接口move_group_interface详解 -> 正文阅读

[C++知识库]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_;