使用 MoveItCpp 并行运行多个规划管线

MoveItCpp 提供了一个 API，支持以下功能：

使用不同的规划器并行运行多个规划管线
定义自定义停止准则，以终止尚未找到解决方案的规划管线
定义自定义函数来选择最合适的解决方案

使用多个规划管线的好处包括：

事先并不知道哪个规划器会产生最佳解决方案
首选规划器有可能失败，因此需要备选方案

MoveItCpp 的总体介绍可以在 MoveItCpp 教程中找到。

并行规划接口

使用 MoveItCpp 进行并行规划与单管线规划类似，区别在于规划组件的 plan(...) 函数使用了不同的实现：

MotionPlanResponse PlanningComponent::plan(
  const MultiPipelinePlanRequestParameters& parameters,
  SolutionCallbackFunction solution_selection_callback,
  StoppingCriterionFunction stopping_criterion_callback)

此函数尝试规划一条从起始状态到满足一组约束的目标状态的轨迹。根据 parameters 提供的配置，会启动多个线程，每个线程使用不同的规划管线来尝试解决规划问题。请注意，没有解决方案也是一种可能的结果。一旦所有规划管线都被终止，就会调用 solution_selection_callback 来确定将哪个解决方案作为 MotionPlanResponse 返回。默认情况下，所有管线都使用由 MultiPipelinePlanRequestParameters 的 planning_time 字段定义的时间预算，但也可以通过 stopping_criterion_callback 提前终止并行规划。每当某个管线在并行规划过程中产生解决方案时，都会调用此函数；如果满足停止准则，则会终止尚未找到解决方案的管线。

示例

以下演示展示了如何配置和使用 MoveItCpp 的并行规划接口。首先运行演示：

ros2 launch moveit2_tutorials parallel_planning_example.launch.py

会加载一个复杂的厨房场景并解决两个规划问题。第一个问题是末端执行器朝地面方向的小幅运动，很可能被所有三个规划器解决，但规划时间差异显著。第二个问题要困难得多，很可能只有 RRTConnect 规划器会成功。该演示表明，配置良好的并行规划设置具有通用性，能够应对各种运动规划问题。

使用并行规划需要哪些代码？

首先，你需要初始化 moveit_cpp 和一个用于解决规划问题的规划组件。接下来设置起始状态和目标约束：

planning_component_->setGoal(*goal_state);
planning_component_->setStartStateToCurrentState();

此外，还需要设置 MultiPipelinePlanRequestParameters。

moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters multi_pipeline_plan_request{
  node_, { "ompl_rrtc", "pilz_lin", "chomp" }
};

该类的构造函数会根据节点参数命名空间 "ompl_rrtc"、"pilz_lin" 和 "chomp" 中提供的配置，初始化多个 PlanningRequestParameter 类成员。要提供这些参数，你可以扩展 moveit_cpp.yaml 文件：

  # PlanRequestParameters for the first parallel pipeline that uses OMPL - RRTConnect
  ompl_rrtc:
    plan_request_params:
      planning_attempts: 1
      planning_pipeline: ompl
      planner_id: "RRTConnectkConfigDefault"
      max_velocity_scaling_factor: 1.0
      max_acceleration_scaling_factor: 1.0
      planning_time: 0.5

  # PlanRequestParameters for a second parallel pipeline using Pilz with the LIN planner
  pilz_lin:
    plan_request_params:
      planning_attempts: 1
      planning_pipeline: pilz_industrial_motion_planner
      planner_id: "LIN"
      max_velocity_scaling_factor: 1.0
      max_acceleration_scaling_factor: 1.0
      planning_time: 0.8

  # PlanRequestParameters for a third parallel pipeline that uses CHOMP as planner
  chomp:
    plan_request_params:
      planning_attempts: 1
      planning_pipeline: chomp
      max_velocity_scaling_factor: 1.0
      max_acceleration_scaling_factor: 1.0
      planning_time: 1.5

# Another OMPL planner using a second OMPL pipeline named 'ompl_rrt_star'
ompl_rrt_star:
  plan_request_params:
    planning_attempts: 1
    planning_pipeline: ompl_rrt_star # Different OMPL pipeline name!
    planner_id: "PRMkConfigDefault"
    max_velocity_scaling_factor: 1.0
    max_acceleration_scaling_factor: 1.0
    planning_time: 1.5

（可选）你可以定义自定义停止准则和/或解决方案选择函数。如果没有将任何函数作为参数传给 plan(...)，所有管线都会使用其完整的规划时间预算，之后选择最短路径。

在本例中，我们使用默认的停止准则和一个选择最短解决方案的解决方案选择函数：

planning_interface::MotionPlanResponse getShortestSolution(const std::vector<planning_interface::MotionPlanResponse>& solutions)
{
  // Find trajectory with minimal path
  auto const shortest_solution = std::min_element(solutions.begin(), solutions.end(),
    [](const planning_interface::MotionPlanResponse& solution_a,
       const planning_interface::MotionPlanResponse& solution_b) {
      // If both solutions were successful, check which path is shorter
      if (solution_a && solution_b)
      {
        return robot_trajectory::pathLength(*solution_a.trajectory_) <
               robot_trajectory::pathLength(*solution_b.trajectory_);
      }
      // If only solution a is successful, return a
      else if (solution_a)
      {
        return true;
      }
      // Else return solution b, either because it is successful or not
      return false;
    });
  return *shortest_solution;
}

下面是一个自定义停止准则的示例——一旦 RRTConnect 找到解决方案，就终止其他规划管线：

// Stop parallel planning as soon as RRTConnect finds a solution
bool stoppingCriterion(
    moveit_cpp::PlanningComponent::PlanSolutions const& plan_solutions,
    moveit_cpp::PlanningComponent::MultiPipelinePlanRequestParameters const& plan_request_parameters)
{
  // Read solutions that are found up to this point from a thread safe storage
  auto const& solutions = plan_solutions.getSolutions();

  // Stop parallel planning if the pipeline using RRTConnect finds a solution
  for (auto const& solution : solutions)
  {
      if (solution.planner_id_ == "RRTConnectkConfigDefault")
      {
        // Return true to abort the other pipelines
        return true;
      }
  }
  // Return false when parallel planning should continue
  return false;
}

定义好 MultiPipelinePlanRequestParameters 以及可选的 SolutionCallbackFunction 和/或 StoppingCriterionFunction 后，我们就可以调用 plan(...)：

auto plan_solution = planning_component_->plan(multi_pipeline_plan_request, &getShortestSolution);

提示

当你想并行使用同一管线的不同规划器（例如 Pilz 规划器的 PTP 和 LIN）时，建议在 MoveItCpp 中初始化多个规划管线，而不是在多个并行规划请求中复用同一条管线。在本例中就加载了两条 OMPL 管线。