沉默杀手HomeArchivesTagsCategories
(八) Homotopy planner
|路径规划TEB算法|
Word count: 515|Reading time: 2 min

实际中用的是Homotopy planner，这里做一些简单说明

算法初始化

构造函数就一句initialize(cfg, obstacles, robot_model, visual, via_points);， 代码比较绕:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
void HomotopyClassPlanner::initialize(const TebConfig& cfg, ObstContainer* obstacles,
          RobotFootprintModelPtr robot_model, TebVisualizationPtr visual, 
          const ViaPointContainer* via_points)
{
  cfg_ = &cfg;
  obstacles_ = obstacles;
  via_points_ = via_points;
  robot_model_ = robot_model;

  if (cfg_->hcp.simple_exploration)
    graph_search_ = boost::shared_ptr<GraphSearchInterface>(new lrKeyPointGraph(*cfg_, this));
  else
    graph_search_ = boost::shared_ptr<GraphSearchInterface>(new ProbRoadmapGraph(*cfg_, this));

  initialized_ = true;
  setVisualization(visual);     // 只有一句，对 visualization_ 赋值
}

simple_exploration默认false，也就是用复杂的ProbRoadmapGraph类。

plan 函数

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
// initial_plan 其实就是 transformed_plan
bool HomotopyClassPlanner::plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, 
          const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
  ROS_ASSERT_MSG(initialized_, "Call initialize() first.");

  // store initial plan for further initializations (must be valid for the 
  // lifetime of this object or clearPlanner() is called!)
  initial_plan_ = &initial_plan;

  PoseSE2 start(initial_plan.front().pose);
  PoseSE2 goal(initial_plan.back().pose);

  return plan(start, goal, start_vel, free_goal_vel);
}

// 然后是下面重要的重载函数
bool HomotopyClassPlanner::plan(const PoseSE2& start, const PoseSE2& goal, 
          const geometry_msgs::Twist* start_vel, bool free_goal_vel)
{
  ROS_ASSERT_MSG(initialized_, "Call initialize() first.");

  // Update old TEBs with new start, goal and velocity
  updateAllTEBs(&start, &goal, start_vel);

  // Init new TEBs based on newly explored homotopy classes
  exploreEquivalenceClassesAndInitTebs(start, goal, cfg_->obstacles.min_obstacle_dist, start_vel);
  // update via-points if activated
  updateReferenceTrajectoryViaPoints(cfg_->hcp.viapoints_all_candidates);
  // Optimize all trajectories in alternative homotopy classes
  optimizeAllTEBs(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations);
  // Select which candidate (based on alternative homotopy classes) should be used
  selectBestTeb();

  initial_plan_ = nullptr; // clear pointer to any previous initial plan (any previous plan is useless regarding the h-signature);
  return true;
}

HomotopyClassPlanner::plan —— exploreEquivalenceClassesAndInitTebs —— createGraph() —— PointObstacle::checkLineIntersection —— PointObstacle::checkCollision

1
2
3
4
5
6
7
8
9
PointObstacle::checkLineIntersection
{
  return getMinimumDistance(point) < min_dist;
}

virtual bool checkCollision(const Eigen::Vector2d& point, double min_dist) const
{
      return getMinimumDistance(point) < min_dist;
}

optimizeAllTEBs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
void HomotopyClassPlanner::optimizeAllTEBs(int iter_innerloop, int iter_outerloop)
{
  // optimize TEBs in parallel since they are independend of each other

  if (cfg_->hcp.enable_multithreading)
  {
    // Must prevent .join_all() from throwing exception if interruption was
    // requested, as this can lead to multiple threads operating on the same
    // TEB, which leads to SIGSEGV
    boost::this_thread::disable_interruption di;

    boost::thread_group teb_threads;
    for (TebOptPlannerContainer::iterator it_teb = tebs_.begin(); it_teb != tebs_.end(); ++it_teb)
    {
      teb_threads.create_thread( boost::bind(&TebOptimalPlanner::optimizeTEB, 
        it_teb->get(), iter_innerloop, iter_outerloop,  true, 
        cfg_->hcp.selection_obst_cost_scale, cfg_->hcp.selection_viapoint_cost_scale,
        cfg_->hcp.selection_alternative_time_cost)  );
    }
    teb_threads.join_all();
  }
}

EquivalenceClassContainer equivalence_classes_; //!< Store all known quivalence classes (e.g. h-signatures) to allow checking for duplicates after finding and adding new ones.

// The second parameter denotes whether to exclude the class from detour deletion or not (true: force keeping).

typedef boost::shared_ptr<TebOptimalPlanner> TebOptimalPlannerPtr;

typedef std::vector< TebOptimalPlannerPtr > TebOptPlannerContainer;

teb()函数 返回 const TimedElasticBand&