路徑規劃與避障算法(五)---DWA算法流程之一

版權聲明：本文爲博主原創文章，轉載請聯繫博主。https://mp.csdn.net/mdeditor/84305617
DWA算法流程圖如下:

1.讀入地圖

  base_planner_.setPlan(orig_global_traj);

std::vector<geometry_msgs::PoseStamped> global_plan_;//頭文件裏定義 global_plan_變量

bool LocalPlannerUtil::setPlan(const std::vector<geometry_msgs::PoseStamped>& orig_global_plan) {

  global_plan_.clear();//reset the global plan

  global_plan_ = orig_global_plan;

  return true;
}

2.截取本地地圖（local_plan）

/****
將參考路徑根據costmap大小進行截取,獲得適配costmap大小的參考路徑地圖
****/
base_planner_.getLocalPlan(pose1, transformed_plan);

3.根據獲得的地圖更新costmap參數

dwa_planner_->updatePlanAndLocalCosts(pose1, transformed_plan);

4.尋找最優軌跡

/**
* @brief  find best trajectory in the generated trajectories
* @pose1  current position of vehicle, including x,y,orientation(yaw/heading)
* @pose2  current velocity of vehicle, including linear velocity; linear accelerated velocity; angular velocity; accelerated angular velocity
* @drive_velocityies  the velocity selected to send to next module
* @footprint_spec     footprint of the vehicle
*/
    dwa_planner_->findBestPath(
      pose1,
      pose2,
      drive_velocities,
      footprint_spec);

4.1尋找最優軌跡

bool SimpleScoredSamplingPlanner::findBestTrajectory(Trajectory &traj, std::vector<Trajectory> *all_explored)//利用此函數尋找最優軌跡
/****
Find Best Trajectory
利用速度採樣空間生成軌跡空間,同時利用設定的評分標準對每條軌跡進行評分,選取最小分數的軌跡作爲最佳軌跡,發佈到循跡控制層
****/
    for (std::vector<TrajectorySampleGenerator*>::iterator loop_gen = gen_list_.begin(); loop_gen != gen_list_.end(); ++loop_gen) {
      count = 0;
      count_valid = 0;
      TrajectorySampleGenerator* gen_ = *loop_gen;
    
      while (gen_->hasMoreTrajectories()) {
 
        gen_success = gen_->nextTrajectory(loop_traj);
      
        if (gen_success == false) {
          continue;
        }
  
        loop_traj_cost = scoreTrajectory(loop_traj, best_traj_cost);
      
        if (all_explored != NULL) {
          loop_traj.cost_ = loop_traj_cost;
          all_explored->push_back(loop_traj);
        
        }
      

        if (loop_traj_cost >= 0) {
          count_valid++;
          if (best_traj_cost < 0 || loop_traj_cost < best_traj_cost) {
            best_traj_cost = loop_traj_cost;
            best_traj = loop_traj;
          }
        }
        count++;
        if (max_samples_ > 0 && count >= max_samples_) {
          break;
        }
      }
      if (best_traj_cost >= 0) {
      
        traj.xv_ = best_traj.xv_;
        traj.yv_ = best_traj.yv_;
        traj.thetav_ = best_traj.thetav_;
        traj.cost_ = best_traj_cost;
      
        traj.resetPoints();
        traj_point.traj.clear();
        double px, py, pth;

        for (unsigned int i = 0; i < best_traj.getPointsSize(); i++) {
       
          best_traj.getPoint(i, px, py, pth);
       
          traj.addPoint(px, py, pth);
          
            dwa_local_planner::traj traj_obj;
            traj_obj.x = float(px);
            traj_obj.y = float(py);
            traj_obj.th = float(pth);
            traj_point.traj.push_back(traj_obj);
        }
      }
      
      ROS_DEBUG("Evaluated %d trajectories, found %d valid", count, count_valid);
      if (best_traj_cost >= 0) {
        break;
      }
    }
    return best_traj_cost >= 0;
  }

---

下部分將會介紹DWA具體流程中:如何生成速度採樣空間,以及如何生成軌跡採樣空間