eband_local_planner源碼架構分析

一、文件結構（代碼組成）

由5部分組成，第一部分負責數據類型的轉換；第二部分負責具體的局部規劃的具體實現，是核心代碼；第三部分是move_base局部規劃器和本局部規劃器具體實現的中間轉換接口；第四部分是將局部規劃轉成速度（cmd_vel或twist_cmd）控制指令；第五部分是RVIZ可視化設置，方便調試；

程序流程：eband_local_planner_ros ----->eband_local_planner ----->eband_trajectory_controller;

move_base.cpp中直接調用這些函數（繼承nav_core::BaseLocalPlanner），最終通過方法computeVelocityCommands獲得運動控制指令cmd_vel或者，併發布出去！那麼具體的cmd_vel是如何得出來的呢？

二、類之間的關係

step1：move_base與eband_local_planner_ros

move_base.cpp調用局部規劃算法時，調用的對象是boost::shared_ptr<nav_core::BaseLocalPlanner> tc_；

controller_plan_ = latest_plan_;
.......
tc_->setPlan(*controller_plan_);

 

通過使用對象 tc_的成員函數（方法）setPlan，將全局規劃器得到的latest_plan_傳遞到局部規劃器中，即本案例中的eband_local_planner_ros；

對於本package，這個對象的類型就是eband_local_planner::EBandPlannerROS，該類是nav_core::BaseLocalPlanner的派生類；

step2:eband_local_planner_ros與eband_local_planner 

eband_local_planner_ros.cpp調用eband_local_planner.cpp，是通過聲明對象  boost::shared_ptr<EBandPlanner> eband_;

該智能指針的數據類型EBandPlanner，是eband_local_planner.h中定義的類；

// set plan - as this is fresh from the global planner robot pose should be identical to start frame
if(!eband_->setPlan(transformed_plan_))   //transformed_plan_是由controller_plan_轉換而來

 

通過使用對象eband_的成員函數（方法）setPlan，將eband_local_planner_ros得到的transformed_plan_傳遞到eband_local_planner 中，那麼eband_local_planner 做了這麼工作？看下圖大致能知道，它主要工作是將plan轉換成了band（convertPlanToBand），同時band又會反向影響plan（convertBandToPlan）；其次還有優化band，插入/刪除bubble等；

step3:eband_local_planner_ros與eband_trajectory_controller

eband_local_planner_ros .cpp調用eband_trajectory_controller.cpp,boost::shared_ptr<EBandTrajectoryCtrl> eband_trj_ctrl_;

該智能指針的數據類型EBandTrajectoryCtrl，是eband_trajectory_controller.h中定義的類；

 

實現計算cmd_vel是通過調用對象eband_trj_ctrl_，boost::shared_ptr<EBandTrajectoryCtrl> eband_trj_ctrl_，eband_trj_ctrl_是類EBandTrajectoryCtrl的對象，在初始化（initialize）時被創建。

// create the according controller
        eband_trj_ctrl_ = boost::shared_ptr<EBandTrajectoryCtrl>(new EBandTrajectoryCtrl(name, costmap_ros_));

在將eband_local_planner_ros與eband_trajectory_controller關聯之前，需要進行一些準備工作：

1.將eband_local_planner 中得到的band傳遞到eband_trajectory_controller中，這些是通過以下函數實現：（即eband_local_planner 與eband_trajectory_controller的關聯）

// get current Elastic Band and
      eband_->getBand(current_band);
      // set it to the controller
      if(!eband_trj_ctrl_->setBand(current_band))
      {
        ROS_DEBUG("Failed to to set current band to Trajectory Controller");
        return false;
      }

2.將當前eband_local_planner_ros速度控制數據base_odom_ 傳遞給eband_trajectory_controller中速度控制數據odom_vel_；

 // set Odometry to controller
      if(!eband_trj_ctrl_->setOdometry(base_odom_))
      {
        ROS_DEBUG("Failed to to set current odometry to Trajectory Controller");
        return false;
      }

準備工作之後核心工作，通過使用對象的成員函數（方法）getTwist，得到速度控制指令cmd_vel，即在源碼中的cmd_twist；

// get resulting commands from the controller
      geometry_msgs::Twist cmd_twist;
      if(!eband_trj_ctrl_->getTwist(cmd_twist, goal_reached_))
      {
        ROS_DEBUG("Failed to calculate Twist from band in Trajectory Controller");
        return false;
      }

以上即爲eband_local_planner獲取全局規劃器中的latest_plan_並將其轉換爲具體的速度控制指令cmd_twist的過程！！！

 

總結：

程序流程：eband_local_planner_ros ----->eband_local_planner ----->eband_trajectory_controller對應的核心函數調用關係；

boost::shared_ptr<nav_core::BaseLocalPlanner> tc_即boost::shared_ptr<eband_local_planner::EBandPlannerROS> tc_；

準備工作：

 

轉換工作：