MoveIt教程[10]：Motion Planning Pipeline

在MoveIt中，motion planners被设置为规划路径。然而，在很多情况下，可能希望对运动规划请求进行预处理，或者对规划的路径进行后处理[例如，对时间参数化]。在这种情况下，使用规划管道，其中链的motion planner与预处理和后处理阶段。预处理和后处理阶段称为计划请求适配器，可以通过ROS参数服务器的名称进行配置。在本教程中，将运行C++代码来实例化和调用这样的规划管道。
一.Running the Code
Roslaunch launch文件运行代码直接从moveit_tutorials：

roslaunch moveit_tutorials motion_planning_pipeline_tutorial.launch

注意：本教程使用RvizVisualToolsGui面板逐步演示。要将此面板添加到RViz，请遵循可视化教程中的说明。
过一会儿，RViz窗口应该会出现，并且看起来与页面顶部的窗口相似。要完成每个演示步骤，要么按下屏幕底部RvizVisualToolsGui面板中的Next按钮，要么在屏幕顶部的Tools面板中选择Key Tool，然后在RViz聚焦时按键盘上的N。

二.Expected Output
在RViz中，应该能够看到三种最终被重放的轨迹：
[1]robot将它的右臂移动到它前面的姿势目标
[2]robot将右臂移动到关节目标的一侧
[3]robot将它的右臂移动回它前面的初始姿势目标

三.The Entire Code
全部代码参考文献[2]。
1.Start
设置开始使用规划管道非常简单。在加载规划器之前，需要两个对象，一个RobotModel和一个PlanningScene。
将从实例化一个RobotModelLoader对象开始，该对象将在ROS参数服务器上查找robot描述并构造一个供使用的RobotModel。

robot_model_loader::RobotModelLoader robot_model_loader("robot_description");
robot_model::RobotModelPtr robot_model = robot_model_loader.getModel();

利用RobotModel，可以构建一个保持世界状态[包括机器人]的PlanningScene。

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

现在可以设置PlanningPipeline对象，它将使用ROS参数服务器来确定要使用的请求适配器集和planning插件。

planning_pipeline::PlanningPipelinePtr planning_pipeline(
    new planning_pipeline::PlanningPipeline(robot_model, node_handle, "planning_plugin", "request_adapters"));

2.Visualization
MoveItVisualTools包提供了许多在RViz中可视化对象、机器人和轨迹的功能，还提供了调试工具，比如脚本的分步内省。

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

/* Remote control is an introspection tool that allows users to step through a high level script
   via buttons and keyboard shortcuts in RViz */
visual_tools.loadRemoteControl();

/* RViz provides many types of markers, in this demo we will use text, cylinders, and spheres*/
Eigen::Affine3d text_pose = Eigen::Affine3d::Identity();
text_pose.translation().z() = 1.75;
visual_tools.publishText(text_pose, "Motion Planning Pipeline Demo", rvt::WHITE, rvt::XLARGE);

/* Batch publishing is used to reduce the number of messages being sent to RViz for large visualizations */
visual_tools.trigger();

/* Sleep a little to allow time to startup rviz, etc..
   This ensures that visual_tools.prompt() isn't lost in a sea of logs*/
ros::Duration(10).sleep();

/* We can also use visual_tools to wait for user input */
visual_tools.prompt("Press 'next' in the RvizVisualToolsGui window to start the demo");

3.Pose Goal
现在，将为Panda的右臂创建一个运动计划请求，指定所需的末端执行器的姿势作为输入。

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.0;
pose.pose.position.z = 0.75;
pose.pose.orientation.w = 1.0;

位置公差0.01m，方向公差0.01弧度。

std::vector<double> tolerance_pose(3, 0.01);
std::vector<double> tolerance_angle(3, 0.01);

将使用kinematic_constraints包中提供的帮助函数创建请求作为约束。

req.group_name = "panda_arm";
moveit_msgs::Constraints pose_goal =
    kinematic_constraints::constructGoalConstraints("panda_link8", pose, tolerance_pose, tolerance_angle);
req.goal_constraints.push_back(pose_goal);

现在，调用管道并检查规划是否成功。

planning_pipeline->generatePlan(planning_scene, req, 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;
}

4.Visualize the result

ros::Publisher display_publisher =
    node_handle.advertise<moveit_msgs::DisplayTrajectory>("/move_group/display_planned_path", 1, true);
moveit_msgs::DisplayTrajectory display_trajectory;

/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
moveit_msgs::MotionPlanResponse response;
res.getMessage(response);

display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);
display_publisher.publish(display_trajectory);

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

5.Joint Space Goals

/* First, set the state in the planning scene to the final state of the last plan */
robot_state::RobotState& robot_state = planning_scene->getCurrentStateNonConst();
planning_scene->setCurrentState(response.trajectory_start);
const robot_model::JointModelGroup* joint_model_group = robot_state.getJointModelGroup("panda_arm");
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);

现在，建立一个关节空间目标：

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

调用管道并可视化轨迹：

planning_pipeline->generatePlan(planning_scene, req, 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;
}
/* Visualize the trajectory */
ROS_INFO("Visualizing the trajectory");
res.getMessage(response);
display_trajectory.trajectory_start = response.trajectory_start;
display_trajectory.trajectory.push_back(response.trajectory);

现在应该看到两个规划的轨迹串联在一起：

display_publisher.publish(display_trajectory);

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

6.Using a Planning Request Adapter
规划请求适配器允许指定一系列操作，这些操作应该在规划发生之前或在对结果路径进行规划之后发生。

/* First, set the state in the planning scene to the final state of the last plan */
robot_state = planning_scene->getCurrentStateNonConst();
planning_scene->setCurrentState(response.trajectory_start);
robot_state.setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);

现在，设置一个关节稍微超出其上限：

const robot_model::JointModel* joint_model = joint_model_group->getJointModel("panda_joint3");
const robot_model::JointModel::Bounds& joint_bounds = joint_model->getVariableBounds();
std::vector<double> tmp_values(1, 0.0);
tmp_values[0] = joint_bounds[0].min_position_ - 0.01;
robot_state.setJointPositions(joint_model, tmp_values);
req.goal_constraints.clear();
req.goal_constraints.push_back(pose_goal);

再次调用planner，并可视化轨迹：

 planning_pipeline->generatePlan(planning_scene, req, res);
  if (res.error_code_.val != res.error_code_.SUCCESS)
  {
    ROS_ERROR("Could not compute plan successfully");
    return 0;
  }
  /* Visualize the trajectory */
  ROS_INFO("Visualizing the trajectory");
  res.getMessage(response);
  display_trajectory.trajectory_start = response.trajectory_start;
  display_trajectory.trajectory.push_back(response.trajectory);
  /* Now you should see three planned trajectories in series*/
  display_publisher.publish(display_trajectory);

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

  ROS_INFO("Done");
  return 0;
}

四.The Launch File
全部launch文件参考文献[3]。本教程中的所有代码都可以从moveit_tutorials包[MoveIt的一部分]编译并运行设置。

参考文献：
[1]Motion Planning Pipeline：http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/motion_planning_pipeline/motion_planning_pipeline_tutorial.html
[2]motion_planning_pipeline：https://github.com/ros-planning/moveit_tutorials/tree/kinetic-devel/doc/motion_planning_pipeline
[3]motion_planning_pipeline_tutorial：https://github.com/ros-planning/moveit_tutorials/tree/kinetic-devel/doc/motion_planning_pipeline/launch/motion_planning_pipeline_tutorial.launch