运动规划管道

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

入门

如果您尚未完成，请确保您已完成 Getting Started.

运行代码

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

ros2 launch moveit2_tutorials move_group.launch.py

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

ros2 launch moveit2_tutorials motion_planning_pipeline_tutorial.launch.py

Note: 本教程使用 RvizVisualToolsGui 面板逐步完成演示。要将此面板添加到 RViz，请按照 Visualization Tutorial.

片刻之后，RViz 窗口应会出现，看起来类似于本页顶部的窗口。要完成每个演示步骤，请按 屏幕底部的**RvizVisualToolsGui**面板，或者在屏幕顶部的**工具**面板中选择**Key Tool**，然后在 RViz 处于焦点状态时按下键盘上的**N**。

预期输出

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

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

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

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

完整代码

完整代码可见 here in the MoveIt GitHub project.

Start

Setting up to start using a planning pipeline is pretty easy. Before we can load the planner, we need two objects, a RobotModel and a PlanningScene.

We will start by instantiating a RobotModelLoader object, which will look up the robot description on the ROS parameter server and construct a RobotModel for us to use.

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

Using the RobotModelLoader, we can construct a planning scene monitor that will create a planning scene, monitors planning scene diffs, and apply the diffs to it’s internal planning scene. We then call startSceneMonitor, startWorldGeometryMonitor and startStateMonitor to fully initialize the planning scene monitor

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

We can now setup the PlanningPipeline object, which will use the ROS parameter server to determine the set of request adapters and the planning plugin to use

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

Visualization

The package MoveItVisualTools provides many capabilities for visualizing objects, robots, and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script.

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

Pose Goal

We will now create a motion plan request for the right arm of the Panda specifying the desired pose of the end-effector as input.

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;

A tolerance of 0.01 m is specified in position and 0.01 radians in orientation

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

We will create the request as a constraint using a helper function available from the kinematic_constraints package.

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

Before planning, we will need a Read Only lock on the planning scene so that it does not modify the world representation while planning

{
  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;
  }
}

Visualize the result

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

Joint Space Goals

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

Now, setup a joint space goal

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

Before planning, we will need a Read Only lock on the planning scene so that it does not modify the world representation while planning

{
  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 two 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 continue the demo");

Using a Planning Request Adapter

A planning request adapter allows us to specify a series of operations that should happen either before planning takes place or after the planning has been done on the resultant path

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

Now, set one of the joints slightly outside its upper limit

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

Before planning, we will need a Read Only lock on the planning scene so that it does not modify the world representation while planning

  {
    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;
}

启动文件

整个启动文件是 here 在 GitHub 上。本教程中的所有代码都可以从 MoveIt 设置中的 moveit_tutorials 包中编译和运行。