MoveIt! demo的學習結果

有這兩個的基礎就可以完成下面的部分了：
- 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();

---