Ros2 - Moveit2 - Motion Planning Pipeline(运动规划管道)

在Moveit中， 运动规划器被设置为规划路径。 但是， 我们经常需要预处理运动规划请求和后处理规划路径(例如， 用于时间参数化)。 在这种情况下， 我们使用规划管道，它将运动规划器与预处理和后处理阶段链接在一起。预处理和后处理阶段称为规划请求适配器，可以通过 ROS 参数服务器的名称进行配置。在本教程中，我们将向您介绍 C++ 代码，以实例化和调用此类规划管道。

入门

如果您还没有这样做，请确保您已经完成入门指南中的步骤。

运行代码

打开两个 shell。在第一个 shell 中启动 RViz 并等待所有内容完成加载： 

ros2 launch moveit2_tutorials move_group.launch.py

在第二个 shell 中，运行启动文件：

ros2 launch moveit2_tutorials motion_planning_pipeline_tutorial.launch.py

注意：本教程使用RvizVisualToolsGui面板逐步完成演示。要将此面板添加到 RViz，请按照可视化教程中的说明进行操作。

片刻之后，RViz 窗口应会出现，其外观与本页顶部的窗口类似。要完成每个演示步骤，请按屏幕底部RvizVisualToolsGui面板中的“下一步”按钮，或选择屏幕顶部“工具”面板中的“ Key Tool”，然后在 RViz 处于焦点时按键盘上的N 键。

在 RViz 中，我们最终应该能够看到三条轨迹被重播：

1. 机器人将右臂移动到前方的姿势目标，

2. 机器人将右臂移至侧面的关节目标，

3. 机器人将右臂移回其前方的原始姿势目标，

完整代码

完整代码可以在 MoveIt GitHub 项目中看到。

开始

设置开始使用规划管道非常简单。 在加载规划器之前， 我们需要两个对象， 一个RobotModel和一个PlanningScene。

我们将首先实例化一个 RobotModelLoader 对象，它将在 ROS 参数服务器上查找机器人描述并构建一个 RobotModel 供我们使用。

robot_model_loader::RobotModelLoaderPtr robot_model_loader(
    new robot_model_loader::RobotModelLoader(node, "robot_description"));

使用 RobotModelLoader，我们可以构建一个规划场景监视器，它将创建一个规划场景，监视规划场景差异，并将差异应用于其内部规划场景。然后我们调用startSceneMonitor、startWorldGeometryMonitor和startStateMonitor来完全初始化规划场景监视器：

planning_scene_monitor::PlanningSceneMonitorPtr psm(
    new planning_scene_monitor::PlanningSceneMonitor(node, robot_model_loader));

/* listen for planning scene messages on topic /XXX and apply them to the internal planning scene
                     the internal planning scene accordingly */
psm->startSceneMonitor();
/* listens to changes of world geometry, collision objects, and (optionally) octomaps
                              world geometry, collision objects and optionally octomaps */
psm->startWorldGeometryMonitor();
/* listen to joint state updates as well as changes in attached collision objects
                      and update the internal planning scene accordingly*/
psm->startStateMonitor();

/* We can also use the RobotModelLoader to get a robot model which contains the robot's kinematic information */
moveit::core::RobotModelPtr robot_model = robot_model_loader->getModel();

/* We can get the most up to date robot state from the PlanningSceneMonitor by locking the internal planning scene
   for reading. This lock ensures that the underlying scene isn't updated while we are reading it's state.
   RobotState's are useful for computing the forward and inverse kinematics of the robot among many other uses */
moveit::core::RobotStatePtr robot_state(
    new moveit::core::RobotState(planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentState()));

/* Create a JointModelGroup to keep track of the current robot pose and planning group. The Joint Model
   group is useful for dealing with one set of joints at a time such as a left arm or a end effector */
const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup("panda_arm");

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

planning_pipeline::PlanningPipelinePtr planning_pipeline(
    new planning_pipeline::PlanningPipeline(robot_model, node, "ompl"));

可视化

MoveItVisualTools 包提供了许多在 RViz 中可视化对象、机器人和轨迹的功能，以及调试工具。

namespace rvt = rviz_visual_tools;
moveit_visual_tools::MoveItVisualTools visual_tools(node, "panda_link0", "move_group_tutorial", psm);
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::Isometry3d text_pose = Eigen::Isometry3d::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();

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

姿势目标

我们现在将为熊猫的右臂创建一个运动计划请求，指定末端执行器的所需姿势作为输入。

planning_interface::MotionPlanRequest req;
req.pipeline_id = "ompl";
req.planner_id = "RRTConnectkConfigDefault";
req.allowed_planning_time = 1.0;
req.max_velocity_scaling_factor = 1.0;
req.max_acceleration_scaling_factor = 1.0;
planning_interface::MotionPlanResponse res;
geometry_msgs::msg::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.01 米，方向公差为 0.01 弧度

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

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

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

在规划之前，我们需要对规划场景添加一个只读锁，这样规划时它就不会修改世界表示：

{
  planning_scene_monitor::LockedPlanningSceneRO lscene(psm);
  /* Now, call the pipeline and check whether planning was successful. */
  /* Check that the planning was successful */
  if (!planning_pipeline->generatePlan(lscene, req, res) || res.error_code.val != res.error_code.SUCCESS)
  {
    RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
    rclcpp::shutdown();
    return -1;
  }
}

可视化结果

rclcpp::Publisher<moveit_msgs::msg::DisplayTrajectory>::SharedPtr display_publisher =
    node->create_publisher<moveit_msgs::msg::DisplayTrajectory>("/display_planned_path", 1);
moveit_msgs::msg::DisplayTrajectory display_trajectory;

/* Visualize the trajectory */
RCLCPP_INFO(LOGGER, "Visualizing the trajectory");
moveit_msgs::msg::MotionPlanResponse response;
res.getMessage(response);

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

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

关节空间目标 

/* First, set the state in the planning scene to the final state of the last plan */
robot_state = planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentStateUpdated(response.trajectory_start);
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
moveit::core::robotStateToRobotStateMsg(*robot_state, req.start_state);

现在，设立关节空间目标

moveit::core::RobotState goal_state(*robot_state);
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::msg::Constraints joint_goal =
    kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group);

req.goal_constraints.clear();
req.goal_constraints.push_back(joint_goal);

在规划之前，我们需要对规划场景添加一个只读锁，这样规划时它就不会修改世界表示

{
  planning_scene_monitor::LockedPlanningSceneRO lscene(psm);
  /* Now, call the pipeline and check whether planning was successful. */
  if (!planning_pipeline->generatePlan(lscene, req, res) || res.error_code.val != res.error_code.SUCCESS)
  {
    RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
    rclcpp::shutdown();
    return -1;
  }
}
/* Visualize the trajectory */
RCLCPP_INFO(LOGGER, "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);
visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
visual_tools.trigger();

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

使用计划请求适配器

规划请求适配器允许我们指定一系列操作，这些操作应该在规划进行之前或在结果路径上完成规划之后发生

/* First, set the state in the planning scene to the final state of the last plan */
robot_state = planning_scene_monitor::LockedPlanningSceneRO(psm)->getCurrentStateUpdated(response.trajectory_start);
robot_state->setJointGroupPositions(joint_model_group, response.trajectory.joint_trajectory.points.back().positions);
moveit::core::robotStateToRobotStateMsg(*robot_state, req.start_state);

现在，将其中一个关节稍微设置在其上限之外：

const moveit::core::JointModel* joint_model = joint_model_group->getJointModel("panda_joint3");
const moveit::core::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);

在规划之前，我们需要对规划场景添加一个只读锁，这样规划时它就不会修改世界表示：

  {
    planning_scene_monitor::LockedPlanningSceneRO lscene(psm);
    /* Now, call the pipeline and check whether planning was successful. */
    if (!planning_pipeline->generatePlan(lscene, req, res) || res.error_code.val != res.error_code.SUCCESS)
    {
      RCLCPP_ERROR(LOGGER, "Could not compute plan successfully");
      rclcpp::shutdown();
      return -1;
    }
  }
  /* Visualize the trajectory */
  RCLCPP_INFO(LOGGER, "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);
  visual_tools.publishTrajectoryLine(display_trajectory.trajectory.back(), joint_model_group);
  visual_tools.trigger();

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

  RCLCPP_INFO(LOGGER, "Done");

  rclcpp::shutdown();
  return 0;
}

启动文件

本教程中的所有代码都可以从 MoveIt 设置中的 moveit_tutorials 包中编译和运行。