MoveIt! 学习笔记6-MotionPlanningAPI -MoveIt 中的轨迹规划API

此博文主要是用来记录ROS-Kinetic 中，用于机器人轨迹规划的MoveIt功能包的学习记录。 

英文原版教程见此链接：http://docs.ros.org/en/kinetic/api/moveit_tutorials/html/doc/move_group_interface/move_group_interface_tutorial.html

引： MotionPlanning API的主要功能是提供一个轨迹规划接口，加载机器人planner，然后设定目标位置，使用planner进行轨迹规划+执行。

    这个例子有一个优点：详细介绍了对象创建过程 + 轨迹规划过程，也同时展示了，如何在RVIZ中显示机器人轨迹+目标点信息+机器人状态。 同时引入了RVIZ的 GUI工具包，可以在程序运行的时候，手工控制程序运行步骤（中途阻塞），使程序不用一直从头执行到尾。

官方教程主要以代码实例为主，所以，在下边的代码中，主要通过注释的方式，解释代码含义，通过代码实例，学习MoveIt内部内容。

/*********************************************************************
 * Software License Agreement (BSD License)
 *
 *  Copyright (c) 2012, Willow Garage, Inc.
 *  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 Willow Garage 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, Michael Lautman */

#include <pluginlib/class_loader.h>
#include <ros/ros.h>

// MoveIt!
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/planning_interface/planning_interface.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/kinematic_constraints/utils.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/PlanningScene.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

#include <boost/scoped_ptr.hpp>

int main(int argc, char** argv)
{
  const std::string node_name = "motion_planning_tutorial";
  ros::init(argc, argv, node_name);
  ros::AsyncSpinner spinner(1);
  spinner.start();
  ros::NodeHandle node_handle("~");

  //引：这个教程主要实现了四个运动学规划：移动机器人到指定轴角位置，机器人返回原点，机器人在受约束条件下的运动（直线运动）
  //同时，这个教程也包含了：RVIZ中，显示目标点名称，显示机器人末端执行器轨迹，仿真机器人运动等功能，很值得学习！
  
  
  
  //Part0: 总介绍
  //这个教程主要包含几部分：
  // 1.加载ROS的运动学规划器planner
  // 2. 创建一个RobotModel对象，来从ROS-Server上加载机器人模型；
  // 3. 创建一个PlanningScene对象，用于制定Robot运行场景。
  




  //Part1： 创建一个RobotModelLoader对象，并从ROS-Server上加载机器人模型；并设置模型相关参数
  const std::string PLANNING_GROUP = "panda_arm";                               //（1）设定规划组名称
  robot_model_loader::RobotModelLoader robot_model_loader("robot_description"); //（2）创建robot_model_loader，从"robot_description"这个topic里面加载模型
  robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();   //从loader中，获取机器人模型，并存在robot_model内
  
  //×Step 1.1 创建一个RobotState和JointModelGroup对象，分别保存机器人当前状态+机器人运动规划组panda_arm的关节状态
  robot_state::RobotStatePtr robot_state(new robot_state::RobotState(robot_model));
  const robot_state::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);

  //×Step 1.2 使用moveit_core核心的“RobotModel”， 可以创建一个规划场景（planning_scene）；
  //          这个规划场景可以维持当前仿真环境的状态（包括机器人的状态）

  planning_scene::PlanningScenePtr planning_scene(new planning_scene::PlanningScene(robot_model));

  //×Step 1.3 设置规划场景对象，并将当前的机器人关节状态设置为“ready”状态
  planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");

 





  //Part2： 加载ROS运动学规划的plugIn，加载一个运动学规划器（通过Plugin的名称加载）（注意！ 此处使用了ROS-pluginlib库）
  boost::scoped_ptr<pluginlib::ClassLoader<planning_interface::PlannerManager>> planner_plugin_loader;//loader
  planning_interface::PlannerManagerPtr planner_instance; //？
  std::string planner_plugin_name;  //存储plugin名称

  // ×Step2.1 通过ROS server中，找到正在运行的指定plugin，并将其加载。
  //          其主要功能是，将”planning_plugin“这个topic里面的plugin名称读取出来，  并存在planner_plugin_name对象内。
  if (!node_handle.getParam("planning_plugin", planner_plugin_name))
    ROS_FATAL_STREAM("Could not find planner plugin name");
  try
  {
    planner_plugin_loader.reset(new pluginlib::ClassLoader<planning_interface::PlannerManager>(
        "moveit_core", "planning_interface::PlannerManager"));
  }
  catch (pluginlib::PluginlibException& ex)
  {
    ROS_FATAL_STREAM("Exception while creating planning plugin loader " << ex.what());
  }
  try
  {
    planner_instance.reset(planner_plugin_loader->createUnmanagedInstance(planner_plugin_name));
    if (!planner_instance->initialize(robot_model, node_handle.getNamespace()))
      ROS_FATAL_STREAM("Could not initialize planner instance");
    ROS_INFO_STREAM("Using planning interface '" << planner_instance->getDescription() << "'");
  }
  catch (pluginlib::PluginlibException& ex)
  {
    const std::vector<std::string>& classes = planner_plugin_loader->getDeclaredClasses();
    std::stringstream ss;
    for (std::size_t i = 0; i < classes.size(); ++i)
      ss << classes[i] << " ";
    ROS_ERROR_STREAM("Exception while loading planner '" << planner_plugin_name << "': " << ex.what() << std::endl
                                                         << "Available plugins: " << ss.str());
  }







  //Part3： 可视化（创建在RVIZ中，显示坐标及轨迹的信息对象）
  //功能：MoveItVisualTools这个工具对象具有很多的功能，包括：
  //     -可以可视化显示物体，显示机器人；
  //     -可以可视化显示规划器规划出来的机器人轨迹；
  //     -可以加载RVIZ内部的GUI按钮，点击按钮进行脚本的点动调试。（总体的程序一步一步执行，而不是一次全部执行完）     

  //×Step3.1： 创建MoveItVisualTools对象，创建一个topic，发布机器人状态
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("panda_link0");
  visual_tools.loadRobotStatePub("/display_robot_state");   
  visual_tools.enableBatchPublishing(); //重要！ 这条语句的作用是，为了防止发送信息过于频繁导致的堵塞问题，将信息打包，在trigger时，统一发送。
  visual_tools.deleteAllMarkers();  // clear all old markers
  visual_tools.trigger();

  //*Step3.2 ： 加载remoteControl，主要功能是激活RVIZ里面的“按钮GUI”，操作人员可以通过GUI按钮/键盘来控制程序运行下一步。 
  visual_tools.loadRemoteControl();


  //提示：RVIZ中，有很多种方式来显示标记： 文字+圆柱（就是XYZ坐标轴）+球体（沿着轨迹线均匀分布）
  /* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/



  //×Step3.3：重要！ ！！展示如何在RVIZ中，显示点的坐标信息！！
  Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
  text_pose.translation().z() = 1.75;
  visual_tools.publishText(text_pose, "Motion Planning API Demo", rvt::WHITE, rvt::XLARGE);//！（点变量名，显示文字，颜色，文字大小）

  //*提示：使用Batch的信息发布策略，若发送的需要显示的内容过多时，可以提高效率 
  visual_tools.trigger(); //显示上边的text——pose点的信息

  //* 提示！！ 这条语句主要是阻塞程序运行，带操作人员点击GUI按钮或者键盘，继续运行后续程序。
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");







  // Part4：设定一个目标点坐标，并显示，然后设置带有约束条件的目标点

  //Step4.1 显示当前机器人状态，并设置目标点坐标+设置允许最大误差

  visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN); //使用绿色显示机器人当前状态
  visual_tools.trigger();                           //关联：enableBatchPublishing, 将上边绿色的标记显示出来。
  planning_interface::MotionPlanRequest req;        //创建一个运动规划”请求“对象和“返回”对象
  planning_interface::MotionPlanResponse res;
  geometry_msgs::PoseStamped pose;                  //创建一个四元数的位姿，并赋值；
  pose.header.frame_id = "panda_link0";
  pose.pose.position.x = 0.3;
  pose.pose.position.y = 0.4;
  pose.pose.position.z = 0.75;
  pose.pose.orientation.w = 1.0;

  std::vector<double> tolerance_pose(3, 0.01);      //设定运动学规划的允许误差，在xyz长度方向上：0.01m误差； 在转角上：0.01rad    
  std::vector<double> tolerance_angle(3, 0.01);     //这个3是什么意思？

  //Step4.2 设置一个带有约束条件的，目标点对象；这个对象将作为输入值，传入到plannar进行轨迹规划

  // .. _kinematic_constraints:
  //     http://docs.ros.org/indigo/api/moveit_core/html/namespacekinematic__constraints.html#a88becba14be9ced36fefc7980271e132
  moveit_msgs::Constraints pose_goal =
      kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle); //（link名称，目标点坐标，位置允许误差，转角允许误差）





  // Part5： 进行轨迹规划

  //Step5.1 将前边带有约束的目标点输入规划器
  req.group_name = PLANNING_GROUP;            //设置运动规划组名称
  req.goal_constraints.push_back(pose_goal);  //将带有约束条件的目标点，输入到MotionPlanRequest req对象中

  
  
  //Step5.2 创建一个‘规划环境’（planning context），并将规划场景，规划请求，规划反馈值封住在里面；
  //        然后使用这个‘规划环境’进行轨迹规划。

  planning_interface::PlanningContextPtr context =
      planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);                         //重要！！这句是进行轨迹规划的执行指令
  if (res.error_code_.val != res.error_code_.SUCCESS)
  {
    ROS_ERROR("Could not compute plan successfully");
    return 0;
  }


   // Part6： 在RVIZ中，显示规划成功的轨迹结果
  
   //×Step6.1 创建一个publisher，向其中“”topic，发送moveit_msgs::DisplayTrajectory类型的轨迹内容
  ros::Publisher display_publisher =
      node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
  moveit_msgs::DisplayTrajectory display_trajectory;               //！！！！创建将被publish的对象

  moveit_msgs::MotionPlanResponse response; //创建一个存储轨迹的response对象，并将res中的轨迹保存至response对象中。
  res.getMessage(response);


  display_trajectory.trajectory_start = response.trajectory_start;  //向被publish的对象中，存储轨迹起点+轨迹信息
  display_trajectory.trajectory.push_back(response.trajectory);

  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group); //设置轨迹可视化
  visual_tools.trigger();
  display_publisher.publish(display_trajectory);//向topic中，发送轨迹信息

  //*Step6.2 将规划好的轨迹，加载到robot——state里面，然后使用指令，控制机器人运动至相应位置
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_state.get());

  //× Step6.3 显示机器人当前位置（坐标点，坐标名称） 
  visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
  visual_tools.publishAxisLabeled(pose.pose, "goal_1");
  visual_tools.publishText(text_pose, "Pose Goal (1)", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  //× Step6.4 使用RIVZ visual tool，等待操作人员点击GUI的按钮，或者按下键盘N，以进行后续内容（此处阻塞程序运行）
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");






  // Part7 设定一个机器人关节位姿（每个关节的转角），显示轨迹，然后控制机器人到达该位置。
  // ^^^^^^^^^^^^^^^^^
  // Now, setup a joint space goal
  robot_state::RobotState goal_state(robot_model);
  std::vector<double> joint_values = { -1.0, 0.7, 0.7, -1.5, -0.7, 2.0, 0.0 };
  goal_state.setJointGroupPositions(joint_model_group, joint_values);
  moveit_msgs::Constraints joint_goal = kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);
  req.goal_constraints.clear();
  req.goal_constraints.push_back(joint_goal);

  // Call the planner and visualize the trajectory
  /* Re-construct the planning context */
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  /* Call the Planner */
  context->solve(res);  //词句进行轨迹规划
  /* Check that the planning was successful */
  if (res.error_code_.val != res.error_code_.SUCCESS)
  {
    ROS_ERROR("Could not compute plan successfully");
    return 0;
  }


  /* 与上边part6 方法一致， */
  res.getMessage(response);
  display_trajectory.trajectory.push_back(response.trajectory);

  /* Now you should see two planned trajectories in series*/
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();
  display_publisher.publish(display_trajectory);

  /* We will add more goals. But first, set the state in the planning
     scene to the final state of the last plan */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_state.get());  //控制机器人运动到规划的位置

  // Display the goal state 显示轨迹终点信息
  visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
  visual_tools.publishAxisLabeled(pose.pose, "goal_2");
  visual_tools.publishText(text_pose, "Joint Space Goal (2)", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  /* Wait for user input *///同样方法，阻塞程序运行，并且等待输入
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");





  /* Part8 规划当前位置到初始位置的轨迹，并执行
  // 注意！！重要！！下边的轨迹规划过程更加简洁，可以作为模板参考使用
  req.goal_constraints.clear();
  req.goal_constraints.push_back(pose_goal);
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);     //进行轨迹规划
  res.getMessage(response);

  display_trajectory.trajectory.push_back(response.trajectory);
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();
  display_publisher.publish(display_trajectory);

  /* Set the state in the planning scene to the final state of the last plan */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_state.get());  //词句是控制机器人执行轨迹，返回原点

  // Display the goal state
  visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
  visual_tools.trigger();

  /* Wait for user input *///阻塞程序，等待输入
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to continue the demo");






  /* Part9 重要！！ 规划一个带有约束条件的运动轨迹（末端执行器，保持末端方向不变的情况下，沿着直线运动）
  // ^^^^^^^^^^^^^^^^^^^^^^^
  // Let's add a new pose goal again. This time we will also add a path constraint to the motion.
  /* Let's create a new pose goal */

  pose.pose.position.x = 0.32;
  pose.pose.position.y = -0.25;
  pose.pose.position.z = 0.65;
  pose.pose.orientation.w = 1.0;
  moveit_msgs::Constraints pose_goal_2 =
      kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);//仍然使用之前的约束条件

  /* Now, let's try to move to this new pose goal*/
  req.goal_constraints.clear();
  req.goal_constraints.push_back(pose_goal_2);

  /* But, let's impose a path constraint on the motion.
     Here, we are asking for the end-effector to stay level*/
  //重要！！此处是约束了末端沿着一个位姿运动的指令！！！！！
  //方法是：定义一个四元数，只设定旋转位姿，然后将其设置为约束。
  geometry_msgs::QuaternionStamped quaternion;
  quaternion.header.frame_id = "panda_link0";
  quaternion.quaternion.w = 1.0;
  req.path_constraints = kinematic_constraints::constructGoalConstraints("panda_link8", quaternion);

  // Imposing path constraints requires the planner to reason in the space of possible positions of the end-effector
  // (the workspace of the robot)
  // because of this, we need to specify a bound for the allowed planning volume as well;
  // Note: a default bound is automatically filled by the WorkspaceBounds request adapter (part of the OMPL pipeline,
  // but that is not being used in this example).
  // We use a bound that definitely includes the reachable space for the arm. This is fine because sampling is not done
  // in this volume
  // when planning for the arm; the bounds are only used to determine if the sampled configurations are valid.
  req.workspace_parameters.min_corner.x = req.workspace_parameters.min_corner.y =
      req.workspace_parameters.min_corner.z = -2.0;
  req.workspace_parameters.max_corner.x = req.workspace_parameters.max_corner.y =
      req.workspace_parameters.max_corner.z = 2.0;

  // Call the planner and visualize all the plans created so far.
  context = planner_instance->getPlanningContext(planning_scene, req, res.error_code_);
  context->solve(res);  //此处进行轨迹规划
  res.getMessage(response);
  display_trajectory.trajectory.push_back(response.trajectory);
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();
  display_publisher.publish(display_trajectory);//显示机器人受约束轨迹

  /* Set the state in the planning scene to the final state of the last plan */
  robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
  planning_scene->setCurrentState(*robot_state.get()); //机器人运动

  // Display the goal state
  visual_tools.publishRobotState(planning_scene->getCurrentStateNonConst(), rviz_visual_tools::GREEN);
  visual_tools.publishAxisLabeled(pose.pose, "goal_3");
  visual_tools.publishText(text_pose, "Orientation Constrained Motion Plan (3)", rvt::WHITE, rvt::XLARGE);
  visual_tools.trigger();

  // END_TUTORIAL
  /* Wait for user input */
  visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to exit the demo");
  planner_instance.reset();

  return 0;
}