使用 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();

此外，还需要设置：code: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);

提示

当您想要并行使用同一管道的不同规划器（例如带有 PTP 和 LIN 的 Pilz 规划器）时，建议在 MoveItCpp 中初始化多个规划管道，而不是在多个并行规划请求中使用同一个管道。在此示例中，加载了两个 OMPL 管道。