路径规划[1/4]：PlanningPipeline::generatePlan

ancientcc
  • 适配器必须实现基类PlanningRequestAdapter的虚操作adaptAndPlan#3，它的第一个参数是个函数，对这个函数，传给组中前N-2个的是callAdapter2，第N-1个callAdapter1，最后一个则是callPlannerInterfaceSolve。
  • 对callAdapter1和callAdapter2，顾名思义，它们的作用是调用下一个适配器。区别是callAdapter2调用该适配器的adaptAndPlan#3方法，callAdapter1则是adaptAndPlan#1。
  • 规划逻辑（适配器组中适配器个数>=2）。generatePlan调用PlanningRequestAdapterChain::adaptAndPlan。2）adaptAndPlan执行callAdapter2，callAdapter2调用下一个（即第一个）适配器的adaptAndPlan#3。在adaptAndPlan#3中，因为本适配器不是组中第N-1个或最后那个，第一个参数指向的函数是callAdapter2，callAdapter2继续调用下一个适配器的adaptAndPlan#3。3）依此逻辑不断在adapter_chain_向前处理适配器，直到执行第N-1个适配器的adaptAndPlan#3，此时第一个参数指向的函数是callAdapter1，callAdapter1继续调用最后一个适配器的adaptAndPlan#1。在这个adaptAndPlan#1，参数planner指向的函数会执行ompl规划器ModelBasedPlanningContext::solve(...)。4）执行完ModelBasedPlanningContext::solve后，由于各适配器的adaptAndPlan#3还被逆序压在Call Stack，参考图2，于是接下就是逆序执行各适配器callAdapter1或callAdapter2后的那些操作。作为特殊情况，如果组中适配器只有一个，那不会调用到callAdapter2。
  • 执行PlanningPipeline::generatePlan后，规划出的路径存放在MotionPlanResponse::trajectory_，类型是robot_trajectory::RobotTrajectory。

路径规划，也称运动规划、轨迹规划，是给定一个目标关节状态或目标位姿，找到一条从机器人当前关节状态到路径。指定的是目标位姿时，期间会用逆运动学求解器(IK)逆解出关节状态。

 

一、PlanningPipeline

PlanningPipeline::generatePlan是执行路径规划的顶层函数，参数req存放着，参数res要存放规划出的路径。既然要调用generatePlan方法，首先说下如何得到PlanningPipeline对象。系统创建的PlanningPipeline集中放置在moveit_cpp::MoveItCpp，这可说是顶层class，具体是它的planning_pipelines_成员。

class MoveItCpp
{
private:
  std::map<std::string, planning_pipeline::PlanningPipelinePtr> planning_pipelines_;
};

planning_pipelines_是个std::map，first是字符串表示的标识，second是对应该标识创建的PlanningPipeline。当中有个特殊标识，值来自param变量“/move_group/default_planning_pipeline”，称它为默认PlanningPipeline。当前launcher这个值是空，根据moveit_ros_move_group__move_group取出该值时代码，空可能是不允许的。虽然planning_pipelines_是个std::map，但是很多时候整系统可能也就这么个默认PlanningPipeline。这个PlanningPipeline如何被要执行路径规划的对象得到，这分两种情况，一是关节、位姿规划时，二是抓取、放置时。注：笛卡儿规划没有路径规划阶段。

第一类：关节、位姿规划

顶层是class MoveGroupMoveAction的executeMoveCallbackPlanOnly或executeMoveCallbackPlanAndExecute，内中通过方法resolvePlanningPipeline(const std::string& pipeline_id)得到PlanningPipeline。其中参数pipeline_id是表示要获取moveit_cpp::MoveItCpp中哪个PlanningPipeline。是空时，得到的就是默认PlanningPipeline。因为pipeline_id来自于goal->request.pipeline_id，也就是说，client填的request.pipeline_id是空时，将使用默认PlanningPipeline。

此种情况下，是在ActionServerThread线程执行PlanningPipeline::generatePlan。

第二类：抓取、放置

顶层是class ManipulationPipeline，它的Start方法会运行新线程processingThread。在processingThread，有三个3个stages_，最后一个stage是PlanStage。PlanStage有个PlanningPipelinePtr类型的成员planning_pipeline_，通过它调用PlanningPipeline::generatePlan。这个planning_pipeline_就是默认PlanningPipeline。

虽然ManipulationPipeline和PlanningPipeline都含有Pipeline字符串，但它们可说没关系。此种情况下，是在新建的processingThread线程执行PlanningPipeline::generatePlan，不是在处理action的ActionServerThread。

 

二、PlanningPipeline::generatePlan

在深入PlanningPipeline::generatePlan前，先看下规划请求适配器基类PlanningRequestAdapter中三个版本的adaptAndPlan方法，知道文中提到的adaptAndPlan#1、adaptAndPlan#3是指哪两个函数。

第一个版本，缩写为“adaptAndPlan#1”。规划过程中只有callAdapter1会调用它，而且只一次。它把callPlannerInterfaceSolve作为下一个适配器adaptAndPlan#1方法的第一个参数。

bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                                          const planning_scene::PlanningSceneConstPtr& planning_scene,
                                          const planning_interface::MotionPlanRequest& req,
                                          planning_interface::MotionPlanResponse& res,
                                          std::vector<std::size_t>& added_path_index) const
{
  return adaptAndPlan(std::bind(&callPlannerInterfaceSolve, planner.get(), std::placeholders::_1, std::placeholders::_2,
                                std::placeholders::_3),
                      planning_scene, req, res, added_path_index);
}

第二个版本，规划过程不会调用到这个版本。

bool PlanningRequestAdapter::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                                          const planning_scene::PlanningSceneConstPtr& planning_scene,
                                          const planning_interface::MotionPlanRequest& req,
                                          planning_interface::MotionPlanResponse& res) const
{
  std::vector<std::size_t> dummy;
  return adaptAndPlan(planner, planning_scene, req, res, dummy);
}

第三个版本，适配器必须重载并实现的版本。缩写为“adaptAndPlan#3”。

virtual bool adaptAndPlan(const PlannerFn& planner, const planning_scene::PlanningSceneConstPtr& planning_scene,
                            const planning_interface::MotionPlanRequest& req,
                            planning_interface::MotionPlanResponse& res,
                            std::vector<std::size_t>& added_path_index) const = 0;

在函数声明上，adaptAndPlan#1和adaptAndPlan#3的差别只在第一个参数，#1是PlannerManager对象，#3则是一个函数。对这个函数，后面可知道，传给组中前N-2个适配器的是callAdapter2，第N-1个callAdapter1，最后一个则是callPlannerInterfaceSolve。

图1 PlanningPipeline::generatePlan

图1显示了generatePlan大致流程，为直观会结合一个示例进行描述。示例中有5个规划请求适配器，按着以下的次序添加到规划请求适配器组(adapter_chain_)。

  • default_planner_request_adapters/AddTimeParameterization
  • default_planner_request_adapters/FixWorkspaceBounds
  • default_planner_request_adapters/FixStartStateBounds
  • default_planner_request_adapters/FixStartStateCollision
  • default_planner_request_adapters/FixStartStatePathConstraints

参数req存放着，参数res要存放规划出的路径。规划出的路径存放在MotionPlanResponse::trajectory_，类型是robot_trajectory::RobotTrajectory。

bool planning_pipeline::PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& planning_scene,
                                                       const planning_interface::MotionPlanRequest& req,
                                                       planning_interface::MotionPlanResponse& res,
                                                       std::vector<std::size_t>& adapter_added_state_index) const
{
  adapter_added_state_index.clear();

  ...

  bool solved = false;
  try
  {
    if (adapter_chain_)
    {
      solved = adapter_chain_->adaptAndPlan(planner_instance_, planning_scene, req, res, adapter_added_state_index);

adapter_chain_是适配器组，它不为nullptr，意味着为此次规划至少添加了一个规划请求适配器。正常工作时，为让得到的路点有时间戳，就须要添加能产生时间戳的适配器，必然会进这入口。

adapter_chain_->adaptAndPlan执行着具体的路径规划操作。

      if (!adapter_added_state_index.empty())
      {
        std::stringstream ss;
        for (std::size_t added_index : adapter_added_state_index)
          ss << added_index << " ";
        ROS_INFO("Planning adapters have added states at index positions: [ %s]", ss.str().c_str());
      }
    }
    else
    {

没有为此次规划添加规划请求适配器，像调试时。结合图1，它就是没有前、后适配器，只留中间的ModelBasedPlanningContext::solve(...)。

      planning_interface::PlanningContextPtr context =
          planner_instance_->getPlanningContext(planning_scene, req, res.error_code_);
      solved = context ? context->solve(res) : false;
    }
  }
  ...
  return solved && valid;
}

 

三、PlanningRequestAdapterChain::adaptAndPlan

已确定此次规划至少添加了一个适配器，adaptAndPlan执行着具体的路径规划操作。

bool PlanningRequestAdapterChain::adaptAndPlan(const planning_interface::PlannerManagerPtr& planner,
                                               const planning_scene::PlanningSceneConstPtr& planning_scene,
                                               const planning_interface::MotionPlanRequest& req,
                                               planning_interface::MotionPlanResponse& res,
                                               std::vector<std::size_t>& added_path_index) const
{
  // if there are no adapters, run the planner directly
  if (adapters_.empty())
  {
    added_path_index.clear();
    return callPlannerInterfaceSolve(planner.get(), planning_scene, req, res);
  }
  else
  {
    // the index values added by each adapter
    std::vector<std::vector<std::size_t> > added_path_index_each(adapters_.size());

    // if there are adapters, construct a function pointer for each, in order,
    // so that in the end we have a nested sequence of function pointers that call the adapters in the correct order.
    PlanningRequestAdapter::PlannerFn fn =
        std::bind(&callAdapter1, adapters_.back().get(), planner, std::placeholders::_1, std::placeholders::_2,
                  std::placeholders::_3, boost::ref(added_path_index_each.back()));

此时得到的fn指定了是用callAdapter1调起第N-1个适配器（不是最一个），调用的是它的adaptAndPlan#1方法。

    for (int i = adapters_.size() - 2; i >= 0; --i)
      fn = std::bind(&callAdapter2, adapters_[i].get(), fn, std::placeholders::_1, std::placeholders::_2,
                     std::placeholders::_3, boost::ref(added_path_index_each[i]));
    bool result = fn(planning_scene, req, res);

for内会执行N-1次fn赋值，示例就是4次。每一个fn对应让回调一次callAdapter2，即每次都是调用下一个适配器的adaptAndPlan#3方法。

执行“fn(planning_scene, req, res)”过程会涉及到较多函数，像callAdapter1、callAdapter2、callPlannerInterfaceSolve、各适配器的adaptAndPlan#3、adaptAndPlan#1，让在ompl::geometric::RRTConnect::solve入口设个断点，看下此时的Call Stack。

图2 PlanningRequestAdapterChain::adaptAndPlan的Call Stack
  1. 顶层“fn(planning_scene, req, res)”中的fn是个callAdapter2，图2中高亮显示，它将调用adapters_中下一个适配器的adaptAndPlan#3方法，此时“下一个”就是第一个，即AddTimeParameterization。
  2. 在AddTimeParameterization，它必须执行传给它的第一个参数planner指定的函数。这个planner也是callAdapter2，它将调用下一个适配器的adaptAndPlan#3，即FixWorkspaceBounds。
  3. 在FixWorkspaceBounds，它必须执行传给它的第一个参数planner指定的函数。这个planner也是callAdapter2，它将调用下一个适配器的adaptAndPlan#3，即FixStartStateBounds。
  4. 在FixStartStateBounds，它必须执行传给它的第一个参数planner指定的函数。这个planner也是callAdapter2，它将调用下一个适配器的adaptAndPlan#3，即FixStartStateCollision。
  5. 在FixStartStateCollision，它必须执行传给它的第一个参数planner指定的函数。这个planner是callAdapter1，它将调用下一个适配器的adaptAndPlan#1，即FixStartStatePathConstraints。
  6. 在FixStartStatePathConstraints，它必须执行传给它的第一个参数planner指定的函数。这个planner是callPlannerInterfaceSolve，它会调用ompl规划器ModelBasedPlanningContext::solve(...)。
  7. 执行完solve(...)后，各适配器的adaptAndPlan#3还被逆序压在Call Stack，于是接下就是逆序执行各适配器planner(...)后的那些操作。
图3 只一个适配器时的adaptAndPlan

图3显示了组中只有一个“default_planner_request_adapters/AddIterativeSplineParameterization”适配器时的Call Stack。可以看到，这时不会调用到callAdapter2。

    added_path_index.clear();

    // merge the index values from each adapter
    for (std::vector<std::size_t>& added_states_by_each_adapter : added_path_index_each)
      for (std::size_t& added_index : added_states_by_each_adapter)
      {
        for (std::size_t& index_in_path : added_path_index)
          if (added_index <= index_in_path)
            index_in_path++;
        added_path_index.push_back(added_index);
      }
    std::sort(added_path_index.begin(), added_path_index.end());
    return result;
  }
}

 

四、callPlannerInterfaceSolve、callAdapter1和callAdapter2

bool callPlannerInterfaceSolve(const planning_interface::PlannerManager* planner,
                               const planning_scene::PlanningSceneConstPtr& planning_scene,
                               const planning_interface::MotionPlanRequest& req,
                               planning_interface::MotionPlanResponse& res)
{
  planning_interface::PlanningContextPtr context = planner->getPlanningContext(planning_scene, req, res.error_code_);
  if (context)
    // context->solve就是ompl路径规划器ModelBasedPlanningContext::solve。
    return context->solve(res);
  else
    return false;
}
  • @adapter。下一个要调起的适配器。就adapters_集合中第N-1个适配器。
  • @planner。PlannerManager对象。
  • @planning_scene。PlanningScene对象。
  • @req[IN]。规划请求，存储着目标关态/位姿。
  • @res[OUT]。存储着规划出的路径、
bool callAdapter1(const PlanningRequestAdapter* adapter, const planning_interface::PlannerManagerPtr& planner,
                  const planning_scene::PlanningSceneConstPtr& planning_scene,
                  const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
                  std::vector<std::size_t>& added_path_index)
{
  try
  {
    // 调用adaptAndPlan#1
    return adapter->adaptAndPlan(planner, planning_scene, req, res, added_path_index);
  }
  catch (std::exception& ex)
  {
    ROS_ERROR_NAMED("planning_request_adapter", "Exception caught executing *final* adapter '%s': %s",
                    adapter->getDescription().c_str(), ex.what());
    added_path_index.clear();
    return callPlannerInterfaceSolve(planner.get(), planning_scene, req, res);
  }
}
  • @adapter。下一个要调起的适配器。是adapters_集合中1个到第N-2个适配器任意一个。
  • @planner。PlanningRequestAdapter::PlannerFn，这是个函数。不是callAdapter1中的PlannerManager对象。
bool callAdapter2(const PlanningRequestAdapter* adapter, const PlanningRequestAdapter::PlannerFn& planner,
                  const planning_scene::PlanningSceneConstPtr& planning_scene,
                  const planning_interface::MotionPlanRequest& req, planning_interface::MotionPlanResponse& res,
                  std::vector<std::size_t>& added_path_index)
{
  try
  {
    // 调用adaptAndPlan#3
    return adapter->adaptAndPlan(planner, planning_scene, req, res, added_path_index);
  }
  catch (std::exception& ex)
  {
    ROS_ERROR_NAMED("planning_request_adapter", "Exception caught executing *next* adapter '%s': %s",
                    adapter->getDescription().c_str(), ex.what());
    added_path_index.clear();
    return planner(planning_scene, req, res);
  }
}

callAdapter1会调用adaptAndPlan#1，callAdapter2会调用adaptAndPlan#3。

 

四、两种编写规则

适配器编写规则

  1. 主要任务是实现虚操作PlanningRequestAdapter::adaptAndPlan。PlanningRequestAdapter还有另外两个版本的adaptAndPlan，它们是public方法。
  2. 在adaptAndPlan，如果我是前适配器，那处理完自已的事后，调用planner(planning_scene, req, res)。如果是我是后适配器，先调用planner(planning_scene, req, res)，然后干自已的事。如果我前、后都有，那先干前适配器工作，然后调用planner(...)，最后干后适配器工作。
  3. 对适配器来说，可简单认为planner(planning_scene, req, res)执行着路径规划操作：ModelBasedPlanningContext::solve(...)。

“/move_group/request_adapters”值规则。

  1. 先添加后适配器，再添加前适配器。
  2. 执行时间上，越要在后面执行的适配器越添加在apapters_的前面。
发布于 2022-05-30 11:33:01

全部评论: 0 条

    写评论: