MoveIt! 學習筆記1- MoveGroup C++ Interface

此博文主要是用來記錄ROS-Kinetic 中，用於機器人軌跡規劃的MoveIt功能包的學習記錄。 

英文原版教程見此連結：http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/move_group_interface/move_group_interface_tutorial.html

 引： MoveIt是基於MoveGroup這個類，MoveIt提供了一個相對簡單的方式，令操作人員可以較為容易的操作機器人。 操作人員僅需傳送各個關節的指定角度或者TCP的目標位置，即可控制機器人執行指令，移動到位。 MoveIt是通過ROS內部的Topic/Service和Action機制，向MoveGroup的節點傳送指令。

主要內容：在上面的連結教程中，主要涉及到了C++的MoveIt！的介面。

                  以及如何建立規劃組Move_Group，如何建立關節型別/目標點型別的軌跡，並使用Moveit自帶規劃器進行軌跡規劃。 並且在RVIZ中顯示出來規劃完成的軌跡。

官方教程主要以程式碼例項為主，所以，在下邊的程式碼中，主要通過註釋的方式，解釋程式碼含義，通過程式碼例項，學習MoveIt內部內容。

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2013, SRI International
 *  All rights reserved.
 *
 *  Redistribution and use in source and binary forms, with or without
 *  modification, are permitted provided that the following conditions
 *  are met:
 *
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above
 *     copyright notice, this list of conditions and the following
 *     disclaimer in the documentation and/or other materials provided
 *     with the distribution.
 *   * Neither the name of SRI International nor the names of its
 *     contributors may be used to endorse or promote products derived
 *     from this software without specific prior written permission.
 *
 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 *  POSSIBILITY OF SUCH DAMAGE.
 *********************************************************************/

/* Author: Sachin Chitta, Dave Coleman, Mike Lautman */

#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, "move_group_interface_tutorial");
  ros::NodeHandle node_handle;
  ros::AsyncSpinner spinner(1);
  spinner.start();

  // BEGIN_TUTORIAL
  //
  // Setup
  // ^^^^^
  //
  // MoveIt! operates on sets of joints called "planning groups" and stores them in an object called
  // the `JointModelGroup`. Throughout MoveIt! the terms "planning group" and "joint model group"
  // are used interchangably.設定運動規劃組名稱
  static const std::string PLANNING_GROUP = "panda_arm";

  // The :move_group_interface:`MoveGroup` class can be easily
  // setup using just the name of the planning group you would like to control and plan for.
  //初始化運動規劃組，並將前面設定的名稱輸入進來
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);

  // We will use the :planning_scene_interface:`PlanningSceneInterface`
  // class to add and remove collision objects in our "virtual world" scene
  // 建立一個物件，用於新增和移除在模擬環境中的‘碰撞元件’（用來進行避障的軌跡模擬）
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

  // Raw pointers are frequently used to refer to the planning group for improved performance.
  // 建立一個指標型別變數，用來表示機器人在當前規劃組的狀態（可以提高效率）
  const robot_state::JointModelGroup* joint_model_group =
      move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  // Visualization
  // ^^^^^^^^^^^^^
  //
  // The package MoveItVisualTools provides many capabilties for visualizing objects, robots,
  // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script
  //RVIZ視覺化工具
  namespace rvt = rviz_visual_tools;// 使用rvt來代表 程式庫中的 rviz_visual_tools，以便簡潔
  moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0"); //設定RVIZ visualtool
  visual_tools.deleteAllMarkers(); //刪除rviz內所有的標記

  // Remote control is an introspection tool that allows users to step through a high level script
  // via buttons and keyboard shortcuts in RViz
  //Remote control的主要作用是： 允許操作人員通過RVIZ內建的按鈕和鍵盤快捷鍵進行控制
  visual_tools.loadRemoteControl();

  // RViz provides many types of markers, in this demo we will use text, cylinders, and spheres
  //RVIZ中，可以使用很多種類的標記（標識型別）， 在Demo中，使用了文字+圓柱標記+表面型別。

  Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();  //建立一個text_pose 型別是仿射矩陣，並賦值為單位陣
  text_pose.translation().z() = 1.75;   //設定translation z=1.75
  visual_tools.publishText(text_pose, "MoveGroupInterface Demo", rvt::WHITE, rvt::XLARGE);

  // Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations
  //通過visual——tool物件釋出資訊（彙總一批資訊後，統一傳送可以減少資料量，提升效率）
  visual_tools.trigger();

  // Getting Basic Information
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  //使用ROS_INFO這個功能，在命令列中顯示move_group的資訊（規劃組名稱/末端執行器連桿名稱等）
  // We can print the name of the reference frame for this robot.
  ROS_INFO_NAMED("tutorial", "Reference frame: %s", move_group.getPlanningFrame().c_str());

  // We can also print the name of the end-effector link for this group.
  ROS_INFO_NAMED("tutorial", "End effector link: %s", move_group.getEndEffectorLink().c_str());

  // Start the demo
  // ^^^^^^^^^^^^^^^^^^^^^^^^^
  //！！至此正式開始模擬，下邊的語句會在命令列中顯示“等待按下next鍵”，並等待使用者按下rviz內部的next按鍵，以便繼續執行。
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");



  /**Section 1: 規劃一個三維空間XYZW四元數座標點，並規劃執行**/
  /**Part2 重要！！！ 此後主要是進行目標點設定+規劃，並採集路徑規劃成功與否**/

  // Planning to a Pose goal
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // We can plan a motion for this group to a desired pose for the
  // end-effector.
  // Step1：建立一個geometry_msgs::Pose物件，用於儲存目標點位置（四元數）
  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);//！！將設定的目標點，作為輸入引數存入move_group規劃組中

  // Now, we call the planner to compute the plan and visualize it.
  // Note that we are just planning, not asking move_group
  // to actually move the robot.
  // Step2：建立一個‘Plan物件’，用於規劃到上邊設定的目標點軌跡
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;

  // Step3： 執行move_group.中的軌跡規劃指令，並且採集ERROR_Code ,檢查是否執行成功
  // ！！實際上在執行move_group.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");


  /**Part3 上邊的軌跡規劃完畢之後，需要進行視覺化顯示**/
  // 具體過程為： 在命令列中顯示目標點資訊/目標點內容/
  // Visualizing plans
  // ^^^^^^^^^^^^^^^^^
  // We can also visualize the plan as a line with markers in RViz.
  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 as trajectory line");
  visual_tools.publishAxisLabeled(target_pose1, "pose1");                      //在rviz中，顯示目標點，顯示名稱為“pose1”
  visual_tools.publishText(text_pose, "Pose Goal", rvt::WHITE, rvt::XLARGE);   //在RVIZ中，顯示目標點資訊
  visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);  //在RVIZ中，顯示機器人軌跡線
  visual_tools.trigger();                                                       //將上邊三條語句，統一執行
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");//在GUI介面中，按按鈕繼續執行程式。
 
 
   /**Part4 上邊語句已經完成了軌跡規劃+在rviz中顯示機器人軌跡，下邊的內容控制機器人執行這個軌跡**/
  // Moving to a pose goal
  // Moving to a pose goal is similar to the step above
  // except we now use the move() function. Note that
  // the pose goal we had set earlier is still active
  // and so the robot will try to move to that goal. We will
  // not use that function in this tutorial since it is
  // a blocking function and requires a controller to be active
  // and report success on execution of a trajectory.
  // 注意！！ 下邊的語句是一個阻塞型語句，需要真實的機器人執行，執行完畢後需要回傳完成訊號才可以；
  //         因此在這個教程中，不使用這個語句
  /* Uncomment below line when working with a real robot */
  /* move_group.move(); */










  /**Section 2: 規劃一個軸空間目標點（規定每個軸的轉角），並執行**/
  // Part1：設定一個軸空間座標點，並存到Move_Group中（將會把之前的pose點給替代掉）

  // Planning to a joint-space goal
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //
  // Let's set a joint space goal and move towards it.  This will replace the
  // pose target we set above.
  //
  // To start, we'll create an pointer that references the current robot's state.
  // RobotState is the object that contains all the current position/velocity/acceleration data.
  // Step1.1： 首先建立一個指標物件current_state，並將當前機器人位置/速度/加速度等設定資訊進行儲存。 
     moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
  
  // Step1.2  建立一個double型別陣列，並將上邊得到的current_state中的機器人各軸座標提取出來，存入其中
  // Next get the current set of joint values for the group.
  std::vector<double> joint_group_positions;
  current_state->copyJointGroupPositions(joint_model_group, joint_group_positions);

  // Step1.2  在上邊得到的存有機器人各關節資訊的陣列，將第1個軸的座標改為-1.0；
  //          之後使用修改後的關節陣列，作為軸空間的目標點，設定到move_group中
  //          之後使用move_group.plan這個方法，進行規劃

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

  // Visualize the plan in RViz
  visual_tools.deleteAllMarkers();//清除RVIZ環境內的其他痕跡
  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");//等待操作人員按鍵

  
  







  
  /**Section 3: 規劃一個座標空間的軌跡，並且設定軌跡的約束條件，並執行**/
  // Step3.1：定義一個軌跡規劃的約束條件
  // Planning with Path Constraints
  // Path constraints can easily be specified for a link on the robot.
  // Let's specify a path constraint and a pose goal for our group.
  // First define the path constraint.
  moveit_msgs::OrientationConstraint ocm;
  ocm.link_name = "panda_link7";           //設定被約束的link
  ocm.header.frame_id = "panda_link0";     //設定base_link，就是約束是想對於哪個指定的？
  ocm.orientation.w = 1.0;                 //設定角度約束條件？ TODO：W是什麼？
  ocm.absolute_x_axis_tolerance = 0.1;     //設定xyz軸的各軸允許的最大誤差
  ocm.absolute_y_axis_tolerance = 0.1;  
  ocm.absolute_z_axis_tolerance = 0.1;    
  ocm.weight = 1.0;                        //設定這個約束所佔的比重，當有很多其他約束時，所佔比重越高的，優先順序越高。
                                           //越接近0,所佔優先順序越低

  // Step3.2：將上邊設定好的約束條件，應用在規劃組中。
  // Now, set it as the path constraint for the group.
  moveit_msgs::Constraints test_constraints; //定義一個總的約束物件，並將上邊方向約束新增到其中。
  test_constraints.orientation_constraints.push_back(ocm);
  move_group.setPathConstraints(test_constraints);  //！！此處是給move_group設定約束！！

  // Step3.3：設定新的起始點座標
  // We will reuse the old goal that we had and plan to it.
  // Note that this will only work if the current state already
  // satisfies the path constraints. So, we need to set the start
  // state to a new pose.
  robot_state::RobotState start_state(*move_group.getCurrentState()); //建立一個start_state物件，並將當前的機器人座標設定為下一個運動的起點
  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,然後設定新的位姿start——pose2作為新的起點;
  //                 嘗試通過運動學逆解，從當前位置，回到設定的start_pose2這個座標點
  start_state.setFromIK(joint_model_group, start_pose2);  
  move_group.setStartState(start_state); //將新定義的start_pose2作為新的起始點。



  // Step3.4： 設定新的終點位姿座標，並將其載入到move_group裡面（這個終點位姿與第一個運動規劃終點一致）
  // Now we will plan to the earlier pose target from the new
  // start state that we have just created.
  move_group.setPoseTarget(target_pose1);

  // Step3.5： 由於設定了move_group的約束條件，路徑規劃的時間會變長（因為從當前位姿移動到新的起點時，已經應用了約束條件，所以每次均需要進行逆運動學求解，時間長）
  //         同時，預設的規劃時間為5秒，可能不夠，所以把路徑規劃時間延長至10s
  // Planning with constraints can be slow because every sample must call an inverse kinematics solver.
  // Lets increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.
  move_group.setPlanningTime(10.0);

  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);  //把move_group中的軌跡進行規劃，並儲存在my_plan內！！！！
  ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");

// Step3.6：將規劃好的起始點/目標點+軌跡顯示在rviz中
  // Visualize the plan in RViz
  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");  //等待操作人員操作按鈕或者鍵盤

  // Step3.7：當規劃完帶有約束條件的軌跡時，並且執行完畢後，記得一定要清除所有的約束條件
  // When done with the path constraint be sure to clear it.
  move_group.clearPathConstraints();
  
  // Step3.8：清除start state以便於能夠進行後續的規劃
  // Since we set the start state we have to clear it before planning other paths
  move_group.setStartStateToCurrentState();





  /**Section 4: 規劃一個迪卡爾座標系下的機器人軌跡，這個實現方式是設定很多路徑點，讓機器人依次執行！！！重要**/
  // Cartesian Paths
  // ^^^^^^^^^^^^^^^
  // You can plan a Cartesian path directly by specifying a list of waypoints
  // for the end-effector to go through. Note that we are starting
  // from the new start state above.  The initial pose (start state) does not
  // need to be added to the waypoint list but adding it can help with visualizations

  // Step4.1：設定路徑點
  geometry_msgs::Pose target_pose3 = move_group.getCurrentPose().pose;  //首先將當前位姿，存入新的pose物件中。

  std::vector<geometry_msgs::Pose> waypoints;  //設定一個儲存格式為<geometry_msgs::Pose> 的vector物件，用於儲存路徑點！！
  waypoints.push_back(target_pose3);

  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


  // Step4.2：設定機器人運動減速指標（展示瞭如何通過標量引數，設定每個Joint的最大速度）

  // Cartesian motions are frequently needed to be slower for actions such as approach and retreat
  // grasp motions. Here we demonstrate how to reduce the speed of the robot arm via a scaling factor
  // of the maxiumum speed of each joint. Note this is not the speed of the end effector point.
  move_group.setMaxVelocityScalingFactor(0.1);  //設定一個速度的標量係數！

  // Step4.3：設定機器人運動解析度，設定1cm是機器人的運動步長（也就是下邊的0.01的意思）
  // We want the Cartesian path to be interpolated at a resolution of 1 cm
  // which is why we will specify 0.01 as the max step in Cartesian
  // translation.  We will specify the jump threshold as 0.0, effectively disabling it.
  // Warning - disabling the jump threshold while operating real hardware can cause
  // large unpredictable motions of redundant joints and could be a safety issue
  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);// 將上邊的步長+引數+軌跡點輸入； 最終的軌跡存入到trajectory中！！
  ROS_INFO_NAMED("tutorial", "Visualizing plan 4 (Cartesian path) (%.2f%% acheived)", fraction * 100.0);

  // Step4.4：在RVIZ中，顯示規劃好的軌跡
  // Visualize the plan in RViz
  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");











  /**Section 5: 在 RVIZ的模擬環境中，新增collison的object，就是模擬裡面的box，用來做軌跡規劃時的避障！！！重要**/

  // Adding/Removing Objects and Attaching/Detaching Objects
  // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
  //

  // Step5.1 ：首先定義一個模擬障礙物的物件
  // Define a collision object ROS message.
  moveit_msgs::CollisionObject collision_object;
  collision_object.header.frame_id = move_group.getPlanningFrame();  //定義障礙物的frame id 用來確定障礙物放置位置

  // The id of the object is used to identify it.
  collision_object.id = "box1";                                      //定義障礙物的本體id，用來定位，並且用來區分


  // Step5.2 ：建立一個Box物件，包含：大小尺寸，放置位置
  // 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;
  

  //Step5.3: 設定box的放置位置
  // 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 = 0.4;
  box_pose.position.y = -0.2;
  box_pose.position.z = 1.0;


  //Step5.4: 將剛才設定好的box尺寸，位置，分別載入到障礙物物件裡（collision_object）
  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);  //在RVIZ環境中，載入這個障礙物

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

  // Wait for MoveGroup to recieve and process the collision object message
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to once the collision object appears in RViz");






  //Step5.5: 障礙物設定好之後，規劃一個避障軌跡
  // Now when we plan a trajectory it will avoid the obstacle
  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");

  // Visualize the plan in RViz
  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");

  // 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, 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 attaches to the "
                      "robot");

  // 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, 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 detaches to the "
                      "robot");

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

  // END_TUTORIAL

  ros::shutdown();
  return 0;
}