有这两个的基础就可以完成下面的部分了:
MoveIt! Setup Assistant tutorial——MoveIt!设置助手
MoveIt! Tutorials, MoveIt! 教程——demo
首先是各种头文件,主程序接口

#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>

int main(int argc, char **argv)

然后初始化该节点并设置其名称,名称必须唯一

ros::init(argc, argv, "mymove_group_interface");


下面是进程处理程序,它允许我们与环境交互

ros::NodeHandle node_handle;


下面是ROS多线程订阅消息,这里开了一个线程,然后启动线程。

ros::AsyncSpinner spinner(1);
spinner.start();


对一组称为 “planning groups”的关节进行操作,并将其存储在名为JointModelGroup的对象中。在整个MoveIt!中 “planning group”和”joint model group”这些术语可以互换使用。MoveGroup类可以很容易的用你想规划和控制的planning group来进行设置

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


这个类用于在我们的虚拟环境中增加/移除碰撞检测障碍物

moveit::planning_interface::PlanningSceneInterface planning_scene_interface;


原始指针经常被用来指代计划组以提高性能。

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


MoveItVisualTools包在rviz中提供了许多障碍物、机器人和轨迹的可视化的能力,还有像脚本的一步一步的自我检查的调试工具。

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


Remote control 远程控制是一种自查工具,允许用户通过Rviz中的按钮和键盘快捷键来浏览高级脚本

visual_tools.loadRemoteControl();


Rviz 提供了许多marker的类型, 这里我们主要用到了 text, cylinders, and spheres。publishText文字的大小什么的,参考http://docs.ros.org/kinetic/api/moveit_visual_tools/html/classmoveit__visual__tools_1_1MoveItVisualTools.html

Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
text_pose.translation().z() = 2.75; // above head of PR2
geometry_msgs::Vector3 scale;
scale.x=0.3;
scale.y=0.3;
scale.z=0.3;
visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, scale);


Batch publishing,批量发布用于减少发送到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());


我们可以为这个group制定一个动作,使其成为末端所需的姿势。

  geometry_msgs::Pose target_pose1;
  target_pose1.position.x = -1.6;
  target_pose1.position.y = -0.18;
  target_pose1.position.z = 0.47;
  target_pose1.orientation.x = 0;
  target_pose1.orientation.y = 1;
  target_pose1.orientation.z = 0;
  target_pose1.orientation.w = 0;
  move_group.setPoseTarget(target_pose1);


现在,我们打电话给planner来计算计划并将其可视化。 请注意,我们只是在计划,而不是要求move_group实际移动机器人。

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


我们也可以把这个计划在Rviz中用一条带有标记的线来进行可视化。

  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
  visual_tools.publishAxisLabeled(target_pose1, "pose1",rvt::XLARGE);
  visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, scale);//rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("next step");


下面是是机器人执行规划运动的命令,它是一个阻塞函数,需要一个控制器处于活动状态,并在执行轨迹时报告成功。

/* 在实际控制机器人时取消下面这一行的注释 */
/* move_group.move() */


规划效果如下图。

关节空间的运动规划。这将取代我们上面设置的姿态目标。首先,我们将创建一个引用当前机器人状态的指针。RobotState是包含所有当前位置/速度/加速度数据的对象。

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

接下来获取组的当前关节值集合。

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


现在,让我们修改其中一个关节,计划到新的关节空间目标并将计划可视化。

  joint_group_positions[0] = 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");


让整个规划过程可视化

  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, scale);//rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("next step");


如图

可以轻松地为机器人上的连杆指定路径约束。让我们为我们的组指定路径约束和姿势目标。首先定义路径约束。

  moveit_msgs::OrientationConstraint ocm;
  ocm.link_name = "Joint6";
  ocm.header.frame_id = "base_link";
  ocm.orientation.x = 0;
  ocm.orientation.y = 1;
  ocm.orientation.z = 0;
  ocm.orientation.w = 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 test_constraints;
  test_constraints.orientation_constraints.push_back(ocm);
  move_group.setPathConstraints(test_constraints);


我们将重复使用我们已经计划的旧目标。 请注意,只有在当前状态已经满足路径约束的情况下,这才会起作用。(当前状态是符合约束的) 但是,我们也设置一个新的姿势。

  robot_state::RobotState start_state(*move_group.getCurrentState());
  geometry_msgs::Pose start_pose2;
  start_pose2.orientation.x = 0;
  start_pose2.orientation.y = 1;
  start_pose2.orientation.z = 0.0;
  start_pose2.orientation.w = 0.0;
  start_pose2.position.x = -0.971;
  start_pose2.position.y = -1.623;
  start_pose2.position.z = 0.313;
  start_state.setFromIK(joint_model_group, start_pose2);
  move_group.setStartState(start_state);


按照新的起始位置和原来的目标位置进行规划

  move_group.setPoseTarget(target_pose1);


用约束进行规划可能会很慢,因为每个样本必须调用一个反向运动学求解器。让我们从默认的5秒增加计划时间,以确保计划者有足够的时间成功。然后让规划过程可视化。

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

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

当完成路径约束时,一定要清除约束。

  move_group.clearPathConstraints();


如图

可以通过指定末端路点(waypoint)列表直接规划笛卡尔路径。 请注意,我们从上面的新开始状态开始。 初始姿势(开始状态)不需要添加到航点(waypoint)列表,但添加它可以帮助进行可视化

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

  geometry_msgs::Pose target_pose3 = start_pose2;

  target_pose3.position.z += 0.6;
  waypoints.push_back(target_pose3);  // up 

  target_pose3.position.y -= 0.3;
  waypoints.push_back(target_pose3);  // left

  target_pose3.position.z -= 0.6;
  target_pose3.position.y += 0.6;
  target_pose3.position.x -= 0.6;
  waypoints.push_back(target_pose3);  // down and right 

通过每个关节的最大速度的缩放因子来降低机器人手臂的速度。 请注意,这不是最终效应器的速度。

  move_group.setMaxVelocityScalingFactor(0.2);


我们希望以1厘米的分辨率插入笛卡尔路径,这就是为什么我们将0.01指定为笛卡儿平移中的最大步长。 (我们将跳转阈值指定为0.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);

  // Visualize the plan in Rviz
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Joint Space Goal", rvt::WHITE, scale);//rvt::XLARGE);
  visual_tools.publishPath(waypoints, rvt::LIME_GREEN, rvt::XLARGE);//rvt::SMALL);
  for (std::size_t i = 0; i < waypoints.size(); ++i)
    visual_tools.publishAxisLabeled(waypoints[i], "pt" + std::to_string(i), rvt::XLARGE);//SMALL);
  visual_tools.trigger();
  visual_tools.prompt("next step");

如图

以下,是带障碍的及其他

  // Define a collision object ROS message.
  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = move_group.getPlanningFrame();

  // The id of the object is used to identify it.
  collision_object.id = "box1";

  // Define a box to add to the world.
  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;

  //Define a pose for the box (specified relative to frame_id)
  geometry_msgs::Pose box_pose;
  box_pose.orientation.w = 1.0;
  box_pose.position.x = -1.6;
  box_pose.position.y = -0.4;
  box_pose.position.z = 0.4;

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

  // Now, let's add the collision object into the world
  ROS_INFO_NAMED("tutorial", "Add an object into the world");
  planning_scene_interface.addCollisionObjects(collision_objects);

  // Show text in Rviz of status
  visual_tools.publishText(text_pose, "Add object", rvt::WHITE, scale);//rvt::XLARGE);
  visual_tools.trigger();

  // Sleep to allow MoveGroup to recieve and process the collision object message
  ros::Duration(1.0).sleep();

  // Now when we plan a trajectory it will avoid the obstacle
  move_group.setStartState(*move_group.getCurrentState());
  //move_group.setPoseTarget(target_pose1);
  move_group.setJointValueTarget(joint_group_positions);

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

  // Visualize the plan in Rviz
  visual_tools.deleteAllMarkers();
  visual_tools.publishText(text_pose, "Obstacle Goal", rvt::WHITE, scale);//rvt::XLARGE);
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);
  visual_tools.trigger();
  visual_tools.prompt("next step");

  // Now, let's attach the collision object to the robot.
  ROS_INFO_NAMED("tutorial", "Attach the object to the robot");
  move_group.attachObject(collision_object.id);

  // Show text in Rviz of status
  visual_tools.publishText(text_pose, "Object attached to robot", rvt::WHITE, scale);//rvt::XLARGE);
  visual_tools.trigger();

  /* Sleep to allow MoveGroup to recieve and process the attached collision object message */
  ros::Duration(1.0).sleep();
  visual_tools.prompt("next step");//按钮?

  // Now, let's detach the collision object from the robot.
  ROS_INFO_NAMED("tutorial", "Detach the object from the robot");
  move_group.detachObject(collision_object.id);

  // Show text in Rviz of status
  visual_tools.publishText(text_pose, "Object dettached from robot", rvt::WHITE, scale);//rvt::XLARGE);
  visual_tools.trigger();

  /* Sleep to allow MoveGroup to recieve and process the detach collision object message */
  ros::Duration(1.0).sleep();
  visual_tools.prompt("next step");//按钮?

  // Now, let's remove the collision object from the world.
  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);

  // Show text in Rviz of status
  visual_tools.publishText(text_pose, "Object removed", rvt::WHITE, scale);//rvt::XLARGE);
  visual_tools.trigger();

  /* Sleep to give Rviz time to show the object is no longer there.*/
  ros::Duration(1.0).sleep();

  // END_TUTORIAL

  ros::shutdown();