ROS操作系统 | MoveIt! Mov Group C++ Interface 源码解析

0.说明

原文地址：http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_interface/move_group_interface_tutorial.html
完整代码：https://github.com/ros-planning/moveit_tutorials/blob/kinetic-devel/doc/move_group_interface/src/move_group_interface_tutorial.cpp

参考：

• Move Group C++ Interface 源码解析：https://blog.csdn.net/qq_26565435/article/details/90246367

准备知识：
C++程序设计（四）—— 类和对象
C++中的namespace

通用办法：哪句不懂，直接百度

详细解析

初始化节点并设置名称，名称要唯一。
然后进入进程处理程序，可使用户与环境交互。
ros::AsyncSpinner spinner(1);表示ROS多线程订阅消息，这里开了一个线程。
最后是启动线程

ros::init(argc, argv, "move_group_interface_tutorial");
ros::NodeHandle node_handle;
ros::AsyncSpinner spinner(1);
spinner.start();

初始设置

将"panda_arm"存储在字符串变量PLANNING_GROUP中，这个组已经提前在moveit_setup_assistant中设置了。在整个moveit！中，PLANNING_GROUP和joint model group可互换使用。

static const std::string PLANNING_GROUP = "panda_arm";

查看源代码，简单理解这句话的结构，moveit和planning_interface均为namespace，MoveGroupInterface为类，move_group属于该类的对象。
这句话的意思为，利用提前设置好的PLANNING_GROUP即可设置move_group。

moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

声明对象planning_scene_interface，该对象属于类PlanningSceneInterface，其所包含的成员函数可查看源代码。
这句话的含义为，我们将使用PlanningSceneInterface类在“虚拟世界”场景中添加和删除碰撞对象。

moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

上面说了，PLANNING_GROUP和joint model group可互换使用。

const robot_state::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

可视化代码设置

MoveItVisualTools包在rviz中提供了许多障碍物、机器人和轨迹的可视化的能力，还有像脚本的一步一步的自我检查的调试工具。
namespace rvt = rviz_visual_tools;这句话我不是太懂，从程序本身来看，是导入rviz_visual_tools这个namespace，同时使用别名rvt，至于为啥要导入我有个猜测，就是下面使用的部分成员函数是从rviz_visual_tools::RvizVisualTools中继承得到，例如下面的deleteAllMarkers()、loadRemoteControl()、publishText()等。这是源代码，deleteAllMarker()函数的意思是，告诉Rviz清除特定显示器上的所有标记。

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
visual_tools.deleteAllMarkers();

loadRemoteControl()的含义是预加载遥控器，允许用户通过RViz中的按钮和键盘快捷键逐步执行高级脚本。

visual_tools.loadRemoteControl();

帮助文档使kinetic版本，使用的是Eigen::Affine3d。我查过一些资料，在后来的版本中，更新后包中代码全换成了Eigen::Isometry3d，因为tf变换是等距变换而不是仿射变换，具体内容可见该地址。
Eigen库源码地址

pulishText()也是从rviz_visual_tools::RvizVisualTools中继承得到，使用方法与参数含义见下图

Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
text_pose.translation().z() = 1.75;//text_pose的坐标位置在机器人上方
visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);

批量发布用于减少发送到RViz的消息数，用于大型可视化。

visual_tools.trigger();

终端显示基本信息

在终端打印规划组参考坐标系名称和末端执行器连杆的名称。

ROS_INFO_NAMED("tutorial", "Reference frame: %s", move_group.getPlanningFrame().c_str());
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());
ROS_INFO_NAMED("tutorial", "Available Planning Groups:");

启动demo

说实话，我没找到promt()函数在哪，我猜测它和print的作用类似。

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

工作空间规划

为规划组设定一个目标姿态，使规划组末端运动到该位置。geometry_msgs::Pose具体定义见链接。

moveit::planning_interface::MoveGroupInterface::setPoseTarget()函数定义如下，

geometry_msgs::Pose target_pose1;
target_pose1.orientation.w = 1.0;
target_pose1.position.x = 0.28;
target_pose1.position.y = -0.2;
target_pose1.position.z = 0.5;
move_group.setPoseTarget(target_pose1);

Plan类，这里我们将调用规划器对目标进行规划计算并将其可视化，注意这里我们只是对目标进行规划，并没有要求move_group节点去移动实际机器人。

MoveItErrorCode类，将move_group节点规划得到的bool类型结果返回给success，如果规划成功则终端打印Visualizing plan 1 (pose goal),失败则打印Visualizing plan 1 (pose goal)FAILED。

moveit::planning_interface::MoveGroupInterface::Plan my_plan;

bool success = (move_group.plan(my_plan) ==
                moveit::planning_interface::MoveItErrorCode::SUCCESS);

ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");

规划结果可视化

上文已经得到text_pose的位置，这里在该位置显示Pose Goal字样。在规划目标位置target_pose1处显示pose1坐标系，同时将规划轨迹以一条线的形式展示。

ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
visual_tools.publishAxisLabeled(target_pose1, "pose1");
visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

执行运动规划命令

文档中将该命令注释掉了，给的理由是移动到目标与上面的步骤类似，不同点是我们现在使用move()函数。 请注意，我们之前设置的姿势目标仍处于活动状态，因此机器人将尝试移动到该目标。 我们不会在本教程中使用该函数，因为它是一个阻塞函数，需要一个控制器处于活动状态并报告执行轨迹的成功。

/* Uncomment below line when working with a real robot */
/* move_group.move(); */

关节空间规划

关节空间的规划结果会覆盖掉上文的工作空间规划结果。

moveit::core类，首先，我们将创建一个引用当前机器人状态的指针RobotStatePtr。 RobotState是包含所有当前位置/速度/加速度数据的对象。

moveit::core::RobotStatePtr current_state = move_group.getCurrentState();

现在获取当前规划组中各关节状态值的集合。对于给定的规划组，按照在组中找到变量的顺序，将组成组的变量的位置值复制到另一个位置。 这不一定是RobotState本身的连续内存块，因此我们只是复制结果而不是返回指针。

std::vector<double> joint_group_positions;
current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

前面说到，工作空间规划在设定末端位姿后，规划路径使用的函数为moveit::planning_interface::MoveGroupInterface::setPoseTarget()，而此处是关节空间规划，使用到的函数为moveit::planning_interface::MoveGroupInterface::setJointValueTarget ()(具体定义见地址)，源码中这个函数的格式有很多种，主要是参数的类型不一样，作用是设置JointValueTarget并将其用于将来的规划请求上。

后面的代码和前文一样，目的是终端显示进程。

joint_group_positions[0] = -1.0;  // radians
move_group.setJointValueTarget(joint_group_positions);

success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 2 (joint space goal) %s", success ? "" : "FAILED");

和前面一样，在text_pose位置显示Joint Space Goal字样，同时将规划路径以一条线的形式展现出来。

visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

指定路径约束进行工作空间规划

moveit_msgs包地址，moveit_msgs Msg/Srv Documentation

moveit_msgs/OrientationConstraint Message，该消息包含方向约束的定义，各项参数定义见链接地址。

moveit_msgs::OrientationConstraint ocm;
ocm.link_name = "panda_link7";
ocm.header.frame_id = "panda_link0";
ocm.orientation.w = 1.0;
ocm.absolute_x_axis_tolerance = 0.1;
ocm.absolute_y_axis_tolerance = 0.1;
ocm.absolute_z_axis_tolerance = 0.1;
ocm.weight = 1.0;

将上面设定的约束作为规划组的路径约束，moveit_msgs/Constraints Message

moveit_msgs::Constraints test_constraints;
test_constraints.orientation_constraints.push_back(ocm);
move_group.setPathConstraints(test_constraints);

这段代码的意思是，为规划组设定一个新的起始位姿，并且该新位姿需要满足路径约束。

setFromIK()
这个函数我是在moveit::core::RobotState Class Reference
中找到的，觉得有点奇怪，为什么下面代码提到的是robot_state这个包而不是moveit……

start_state.setFromIK(joint_model_group, start_pose2); 的含义是，如果start_pose2对应的规划组joint_model_group是链和解算器可用（实际上是可用的），则可通过moveit中自带的逆运动学求解器进行计算得到规划组中各关节位置值，前提是设置的start_pose2要在运动模型的参考系中。

定义及参数含义见下图

robot_state::RobotState start_state(*move_group.getCurrentState());
geometry_msgs::Pose start_pose2;
start_pose2.orientation.w = 1.0;
start_pose2.position.x = 0.55;
start_pose2.position.y = -0.05;
start_pose2.position.z = 0.8;
start_state.setFromIK(joint_model_group, start_pose2);
move_group.setStartState(start_state);//由于改变了规划组的起始位姿，需要将设定的位姿作为规划组的启动状态。

在新的启动位姿上，进行带有路径约束的工作空间规划。

move_group.setPoseTarget(target_pose1);

move_group.setPlanningTime(10.0);该语句的意思是，使用约束进行规划可能会很慢，因为每个样本都必须调用反向运动学求解器。 这是需要将计划时间从默认的5秒增加到10秒，以保证规划器完成规划。后面的语句和前面一样，此处不赘述了

move_group.setPlanningTime(10.0);

success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");

和前面一样，规划结果可视化。

visual_tools.deleteAllMarkers();
visual_tools.publishAxisLabeled(start_pose2, "start");
visual_tools.publishAxisLabeled(target_pose1, "goal");
visual_tools.publishText(text_pose, "Constrained Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("next step");

轨迹规划完成，清除约束。

move_group.clearPathConstraints();

笛卡尔运动规划

这里提到一个waypoints的概念，也就是路点。waypoints是一个变长的路点数组，意味着笛卡尔路径中需要经过的每个位姿点，相邻两个路点之间使用直线轨迹运动，该数组中每个点均为geometry_msgs::Pose类型。

将起始位姿start_pose2、三次变化的target_pose3依次加入到waypoints数组中，

std::vector<geometry_msgs::Pose> waypoints;
waypoints.push_back(start_pose2);

geometry_msgs::Pose target_pose3 = start_pose2;

target_pose3.position.z -= 0.2;
waypoints.push_back(target_pose3);  // down

target_pose3.position.y -= 0.2;
waypoints.push_back(target_pose3);  // right

target_pose3.position.z += 0.2;
target_pose3.position.y += 0.2;
target_pose3.position.x -= 0.2;
waypoints.push_back(target_pose3);  // up and left

moveit_msgs/RobotTrajectory Message

本文程序想要笛卡尔路径以1cm的分辨率进行插值，因此将笛卡尔平移的最大步长设置为0.01。

moveit::planning_interface::MoveGroupInterface::computeCartesianPath(),该函数用来计算笛卡尔路径，该路径需要经过之前设置好的waypoints，步长为eef_step，允许不超过jump_threshold的值作为机器人配置空间中距离的变化，将该值设置为0可以有效禁用该变化（但是在操作真实硬件时将jump_threshold设为0可能导致冗余连接发生大量不可预测的动作，这可能是一个安全问题 ）。

moveit_msgs::RobotTrajectory trajectory;
const double jump_threshold = 0.0;
const double eef_step = 0.01;
double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory);
ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::SMALL);
for (std::size_t i = 0; i < waypoints.size(); ++i)//每个坐标系均可视化
	visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::SMALL);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");

避障规划

moveit_msgs/CollisionObject Message，shape_msgs/SolidPrimitive Message

moveit_msgs::CollisionObject collision_object;
collision_object.header.frame_id = move_group.getPlanningFrame();
collision_object.id = "box1";
//定义一个盒子的长宽高，作为障碍
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.4;
primitive.dimensions[1] = 0.1;
primitive.dimensions[2] = 0.4;
//为障碍盒子指定位置（相对于frame_id）
geometry_msgs::Pose box_pose;
box_pose.orientation.w = 1.0;
box_pose.position.x = 0.4;
box_pose.position.y = -0.2;
box_pose.position.z = 1.0;

collision_object.primitives.push_back(primitive);//几何特性：长宽高
collision_object.primitive_poses.push_back(box_pose);//位置
collision_object.operation = collision_object.ADD;//???这里还不懂

std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(collision_object);

将上面定义的障碍添加到world中。

ROS_INFO_NAMED("tutorial", "Add an object into the world");
planning_scene_interface.addCollisionObjects(collision_objects);

可视化，并等待Move Group接收并处理冲突对象消息。

visual_tools.publishText(text_pose, "Add object", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");

将当前规划组的位姿状态设为起始状态，然后设置末端位姿进行规划（此时并未执行，只是规划，并且末端位姿避开障碍）。

move_group.setStartState(*move_group.getCurrentState());
geometry_msgs::Pose another_pose;
another_pose.orientation.w = 1.0;
another_pose.position.x = 0.4;
another_pose.position.y = -0.4;
another_pose.position.z = 0.9;
move_group.setPoseTarget(another_pose);

success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
ROS_INFO_NAMED("tutorial", "Visualizing plan 5 (pose goal move around cuboid) %s", success ? "" : "FAILED");
//可视化
visual_tools.deleteAllMarkers();
visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, rvt::XLARGE);
visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
visual_tools.trigger();
visual_tools.prompt("next step");

将碰撞对象附加到机器人上。

moveit::planning_interface::MoveGroupInterface::attachObject(),在该函数中，给定障碍的名称，将其附加到指定的机器人连杆上。如果没有指定机器人连杆，则默认为附加到末端执行器上。如果没有末端执行器，则使用当前规划组中的第一个连杆。

ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
move_group.attachObject(collision_object.id);

visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object attaches to the robot");

移除障碍，结束规划。

ROS_INFO_NAMED("tutorial", "Remove the object from the world");
std::vector<std::string> object_ids;
object_ids.push_back(collision_object.id);
planning_scene_interface.removeCollisionObjects(object_ids);

visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, rvt::XLARGE);
visual_tools.trigger();

/* Wait for MoveGroup to recieve and process the attached collision object message */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object disapears");
ros::shutdown();
return 0;