Moveit！编程C++ API详解（一）

Moveit中，MoveGroup类提供给我们丰富的函数可以用来设计机器人姿态、进行运动规划。本文以官方API文档以及古月的ROS探索总结为参照，详解Moveit中的C++接口编程。

1. 包含头文件

#include <ros/ros.h>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/DisplayTrajectory.h>

#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>

#include <moveit_visual_tools/moveit_visual_tools.h>

2. 初始化节点

ros::init(argc, argv, "node_name");      //初始化节点
ros::NodeHandle node_handle;         //创建句柄

3. 多线程

  ros::AsyncSpinner spinner(1); //单线程
  spinner.start();
  

//多线程相关疑问请查阅：
ROS多线程

4. Setup你的Movegroup类

static const std::string PLANNING_GROUP = "panda_arm";
moveit :: planning_interface::MoveGroupInterface  move_group(PLANNING_GROUP);

//setup了一个名为panda_arm的Movegroup类
//MoveIt对一组称为“planning group”的 joints 进行操作，并将它们存储在一个名为“JointModelGroup”的对象中。
//在整个MoveIt中，术语“planning group”和“JointModelGroup”是可互换使用的。

move_group.allowReplanning(true);//允许重新规划

5. 创建虚拟情景并等待完成

moveit::planning_interface::PlanningSceneInterface current_scene;
sleep(5.0);

6. 输出基本信息（非必须）

//打印参考框架的名称（基本用不到）
ROS_INFO_NAMED("tutorial", "Planning frame: %s", move_group.getPlanningFrame().c_str());

//打印末端执行器名称（爪子什么的）
ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

//打印出机器人的所有group
ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(),
          std::ostream_iterator<std::string>(std::cout, ", "));

7. 设置姿势

1. 随机姿势

move_group.setRandomTarget();

2. 自定义姿势

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

8. 运动规划器进行规划

//定义一个plan
moveit::planning_interface::MoveGroupInterface::Plan my_plan;

//用布尔型变量标记运动规划是否成功
bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);

//打印结果
ROS_INFO("Visualizing plan 1 (pose goal) %s",success?"":"FAILED");

9. 让机械臂按照规划的轨迹开始运动

move_group.execute(my_plan);

//此处不建议使用move(),因为它是一种阻塞函数，并且要求控制器处于活动状态并在执行轨迹时报告成功。对实体机器人进行操作时，可以使用如下的move()语句。

move_group.move();

10. 规划关节空间目标

//所谓关节空间就是以关节角度为控制量的机器人运动。
//各个关节相互独立，互不影响。
//使用包含所有当前位置/速度/加速度数据的RobotState指针引用当前机器人状态。

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

//然后获取group中所有关节的当前值

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

//修改其中一个关节，将修改后的关节空间

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");

11.笛卡尔路径规划

//笛卡尔路径规划需要用到waypoints的概念，也就是路点列表
//“路点”即笛卡尔路径中需要经过的每个位姿点，相邻两个路点之间使用直线轨迹运动。
//通过类似于祖冲之“割圆术”的思想，用小段直线逼近圆弧，可以实现圆弧路径规划。

//定义路点列表

std::vector<geometry_msgs::Pose> waypoints;

//初始姿态可加可不加

waypoints.push_back(start_pose2);

//定义一个基于start_pose2的Pose变量，用来建立路点列表

geometry_msgs::Pose target_pose3 = start_pose2;

//移动Z轴形成新Pose，并压入路点列表

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

//同上，移动各轴形成新Pose并依次压入路点列表

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

//定义一个RobotTrajectory，用来存储规划出的路径

moveit_msgs::RobotTrajectory trajectory;

//设定跳跃阈值，一般情况下禁用。如果实体联调时禁用可能会出现问题

const double jump_threshold = 0.0;

//设定终端步进值

const double eef_step = 0.01;

//调用computeCartesianPath函数，注意参数列表的顺序

double fraction = move_group.computeCartesianPath(waypoints, eef_step, jump_threshold, trajectory,ture);

//fraction是computeCartesianPath的返回值，描述规划后的运动轨迹在路点列表中的覆盖率。如果fraction小于1则有路点没有进入规划；等于1的话就可以用execute()执行了。
//true代表避障规划。

my_plan.trajectory_=trajectory;
move_group.execute(my_plan);

暂时整理到这，后续会发布新博文进行补充。
如有错误，欢迎更正。