一、概述
目測是全網最全的解析，花了幾個小時通讀並整理的，供大家參考學習。
本篇是直接源碼配注釋的，所以邏輯性不夠強，我還寫了一篇按照代碼執行邏輯讀代碼的文章，個人認為比這篇有用得多，以下為鏈接，可以配合着看：https://www.cnblogs.com/JuiceCat/p/13040552.html 。
概況的話可以看下古月居 https://www.guyuehome.com/270，其實它是翻譯官方的，英語ok的可以去ros wiki翻看原版。
重點：navfn包 全局規划（Dji算法）、base_local_planner包 局部規划（Trajectory Rollout 和Dynamic Window approaches算法）
二、move_base.h
1 #ifndef NAV_MOVE_BASE_ACTION_H_ 2 #define NAV_MOVE_BASE_ACTION_H_ 3 4 #include <vector> 5 #include <string> 6 7 #include <ros/ros.h> 8 9 #include <actionlib/server/simple_action_server.h> 10 #include <move_base_msgs/MoveBaseAction.h> 11 12 #include <nav_core/base_local_planner.h> 13 #include <nav_core/base_global_planner.h> 14 #include <nav_core/recovery_behavior.h> 15 #include <geometry_msgs/PoseStamped.h> 16 #include <costmap_2d/costmap_2d_ros.h> 17 #include <costmap_2d/costmap_2d.h> 18 #include <nav_msgs/GetPlan.h> 19 20 #include <pluginlib/class_loader.hpp> 21 #include <std_srvs/Empty.h> 22 23 #include <dynamic_reconfigure/server.h> 24 #include "move_base/MoveBaseConfig.h" 25 26 namespace move_base { 27 //聲明server端，消息類型是move_base_msgs::MoveBaseAction 28 typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer; 29 //movebase狀態表示 30 enum MoveBaseState { 31 PLANNING, 32 CONTROLLING, 33 CLEARING 34 }; 35 //觸發恢復模式 36 enum RecoveryTrigger 37 { 38 PLANNING_R, 39 CONTROLLING_R, 40 OSCILLATION_R 41 }; 42 43 //使用actionlib：：ActionServer接口的類，該接口將robot移動到目標位置。 44 class MoveBase { 45 public: 46 // 構造函數，傳入的參數是tf 47 MoveBase(tf2_ros::Buffer& tf); 48 49 //析構函數 50 virtual ~MoveBase(); 51 52 //控制閉環、全局規划、 到達目標返回true，沒有到達返回false 53 bool executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan); 54 55 private: 56 /** 57 * @brief 清除costmap的server端 58 * @param req 表示server的request 59 * @param resp 表示server的response 60 * @return 如果server端被成功調用則為True，否則false 61 */ 62 bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp); 63 64 /** 65 * @brief 當action不活躍時，調用此函數，返回plan 66 * @param req 表示goal的request 67 * @param resp 表示plan的request 68 * @return 規划成功返回reue，否則返回false 69 */ 70 bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp); 71 72 /** 73 * @brief 新的全局規划 74 * @param goal 規划的目標點 75 * @param plan 規划 76 * @return 規划成功則返回True 否則false 77 */ 78 bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan); 79 80 /** 81 * @brief 從參數服務器加載導航參數Load the recovery behaviors 82 * @param node 表示 ros::NodeHandle 加載的參數 83 * @return 加載成功返回True 否則 false 84 */ 85 bool loadRecoveryBehaviors(ros::NodeHandle node); 86 87 // 加載默認導航參數 88 void loadDefaultRecoveryBehaviors(); 89 90 /** 91 * @brief 清楚機器人局部規划框的障礙物 92 * @param size_x 局部規划框的長x 93 * @param size_y 局部規划框的寬y 94 */ 95 void clearCostmapWindows(double size_x, double size_y); 96 97 //發布速度為0的指令 98 void publishZeroVelocity(); 99 100 // 重置move_base action的狀態，設置速度為0 101 void resetState(); 102 103 void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal); 104 105 void planThread(); 106 107 void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal); 108 109 bool isQuaternionValid(const geometry_msgs::Quaternion& q); 110 111 bool getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap); 112 113 double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2); 114 115 geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg); 116 117 //周期性地喚醒規划器 118 void wakePlanner(const ros::TimerEvent& event); 119 120 tf2_ros::Buffer& tf_; 121 122 MoveBaseActionServer* as_; //就是actionlib的server端 123 124 boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;//局部規划器，加載並創建實例后的指針 125 costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;//costmap的實例化指針 126 127 boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;//全局規划器，加載並創建實例后的指針 128 std::string robot_base_frame_, global_frame_; 129 130 std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> > recovery_behaviors_;//可能是出錯后的恢復 131 unsigned int recovery_index_; 132 133 geometry_msgs::PoseStamped global_pose_; 134 double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_; 135 double planner_patience_, controller_patience_; 136 int32_t max_planning_retries_; 137 uint32_t planning_retries_; 138 double conservative_reset_dist_, clearing_radius_; 139 ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_; 140 ros::Subscriber goal_sub_; 141 ros::ServiceServer make_plan_srv_, clear_costmaps_srv_; 142 bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_; 143 double oscillation_timeout_, oscillation_distance_; 144 145 MoveBaseState state_; 146 RecoveryTrigger recovery_trigger_; 147 148 ros::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_; 149 geometry_msgs::PoseStamped oscillation_pose_; 150 pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_; 151 pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_; 152 pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_; 153 154 //觸發哪種規划器 155 std::vector<geometry_msgs::PoseStamped>* planner_plan_;//保存最新規划的路徑，傳給latest_plan_ 156 std::vector<geometry_msgs::PoseStamped>* latest_plan_;//在executeCycle中傳給controller_plan_ 157 std::vector<geometry_msgs::PoseStamped>* controller_plan_; 158 159 //規划器線程 160 bool runPlanner_; 161 boost::recursive_mutex planner_mutex_; 162 boost::condition_variable_any planner_cond_; 163 geometry_msgs::PoseStamped planner_goal_; 164 boost::thread* planner_thread_; 165 166 167 boost::recursive_mutex configuration_mutex_; 168 dynamic_reconfigure::Server<move_base::MoveBaseConfig> *dsrv_; 169 170 void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level); 171 172 move_base::MoveBaseConfig last_config_; 173 move_base::MoveBaseConfig default_config_; 174 bool setup_, p_freq_change_, c_freq_change_; 175 bool new_global_plan_; 176 }; 177 }; 178 #endif
三、move_base_node.cpp
1 #include <move_base/move_base.h> 2 #include <tf2_ros/transform_listener.h> 3 4 int main(int argc, char** argv){ 5 ros::init(argc, argv, "move_base_node"); 6 tf2_ros::Buffer buffer(ros::Duration(10)); 7 tf2_ros::TransformListener tf(buffer); 8 9 move_base::MoveBase move_base( buffer );//本cpp中只調用了這個構造函數 10 11 //ros::MultiThreadedSpinner s; 12 ros::spin(); 13 14 return(0); 15 }
四、move_base.cpp
1 #include <move_base/move_base.h> 2 #include <cmath> 3 4 #include <boost/algorithm/string.hpp> 5 #include <boost/thread.hpp> 6 7 #include <geometry_msgs/Twist.h> 8 9 #include <tf2_geometry_msgs/tf2_geometry_msgs.h> 10 11 namespace move_base { 12 13 MoveBase::MoveBase(tf2_ros::Buffer& tf) : 14 tf_(tf), 15 as_(NULL), 16 planner_costmap_ros_(NULL), controller_costmap_ros_(NULL), 17 bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"), 18 blp_loader_("nav_core", "nav_core::BaseLocalPlanner"), 19 recovery_loader_("nav_core", "nav_core::RecoveryBehavior"), 20 planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL), 21 runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) { 22 23 24 //1. 創建move_base action,綁定回調函數。定義一個名為move_base的SimpleActionServer。該服務器的Callback為MoveBase::executeCb。 25 as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false); 26 27 ros::NodeHandle private_nh("~"); 28 ros::NodeHandle nh; 29 30 31 recovery_trigger_ = PLANNING_R; 32 33 //2.加載參數。從參數服務器獲取一些參數，包括兩個規划器名稱、代價地圖坐標系、規划頻率、控制周期等 34 std::string global_planner, local_planner; 35 private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS")); 36 private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS")); 37 private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link")); 38 private_nh.param("global_costmap/global_frame", global_frame_, std::string("map")); 39 private_nh.param("planner_frequency", planner_frequency_, 0.0); 40 private_nh.param("controller_frequency", controller_frequency_, 20.0); 41 private_nh.param("planner_patience", planner_patience_, 5.0); 42 private_nh.param("controller_patience", controller_patience_, 15.0); 43 private_nh.param("max_planning_retries", max_planning_retries_, -1); // disabled by default 44 45 private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0); 46 private_nh.param("oscillation_distance", oscillation_distance_, 0.5); 47 48 49 //set up plan triple buffer 50 planner_plan_ = new std::vector<geometry_msgs::PoseStamped>(); 51 latest_plan_ = new std::vector<geometry_msgs::PoseStamped>(); 52 controller_plan_ = new std::vector<geometry_msgs::PoseStamped>(); 53 54 //3. 設置規划器線程 55 //set up the planner's thread 56 planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this)); 57 58 //4. 創建發布者 59 //for commanding the base 60 vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1); 61 current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 ); 62 63 ros::NodeHandle action_nh("move_base"); 64 action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1); 65 66 //提供消息類型為geometry_msgs::PoseStamped的發送goals的接口，比如cb為MoveBase::goalCB，在rviz中輸入的目標點就是通過這個函數來響應的 67 ros::NodeHandle simple_nh("move_base_simple"); 68 goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1)); 69 70 //我們假設機器人的半徑與costmap的規定一致 71 private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325); 72 private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46); 73 private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_); 74 private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0); 75 76 private_nh.param("shutdown_costmaps", shutdown_costmaps_, false); 77 private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true); 78 private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true); 79 80 // 5. 設置全局路徑規划器 81 //create the ros wrapper for the planner's costmap... and initializer a pointer we'll use with the underlying map 82 planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_); 83 planner_costmap_ros_->pause(); 84 85 //initialize the global planner 86 try { 87 planner_ = bgp_loader_.createInstance(global_planner); 88 planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_); 89 } catch (const pluginlib::PluginlibException& ex) { 90 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what()); 91 exit(1); 92 } 93 94 // 6.設置局部路徑規划器 95 //create the ros wrapper for the controller's costmap... and initializer a pointer we'll use with the underlying map 96 controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_); 97 controller_costmap_ros_->pause(); 98 99 //create a local planner 100 try { 101 tc_ = blp_loader_.createInstance(local_planner); 102 ROS_INFO("Created local_planner %s", local_planner.c_str()); 103 tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_); 104 } catch (const pluginlib::PluginlibException& ex) { 105 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", local_planner.c_str(), ex.what()); 106 exit(1); 107 } 108 109 // Start actively updating costmaps based on sensor data 110 111 //7.開始更新costmap 112 planner_costmap_ros_->start(); 113 controller_costmap_ros_->start(); 114 115 //advertise a service for getting a plan 116 //8.開啟創建地圖,清除地圖服務 117 make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this); 118 119 //定義一個名為clear_costmaps的服務，cb為MoveBase::clearCostmapsService 120 clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this); 121 122 //如果不小心關閉了costmap 123 if(shutdown_costmaps_){ 124 ROS_DEBUG_NAMED("move_base","Stopping costmaps initially"); 125 planner_costmap_ros_->stop(); 126 controller_costmap_ros_->stop(); 127 } 128 129 //9.加載指定的恢復器 130 if(!loadRecoveryBehaviors(private_nh)){ 131 loadDefaultRecoveryBehaviors();//先loadRecoveryBehaviors，不行再loadDefaultRecoveryBehaviors加載默認的，這里包括了找不到路自轉360 132 } 133 134 //initially, we'll need to make a plan 135 state_ = PLANNING; 136 137 //we'll start executing recovery behaviors at the beginning of our list 138 recovery_index_ = 0; 139 140 //10.開啟move_base動作器 141 as_->start(); 142 //啟動動態參數服務器，回調函數為reconfigureCB，推薦看一下古月居https://www.guyuehome.com/1173 143 dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~")); 144 dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2); 145 dsrv_->setCallback(cb); 146 } 147 148 void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level){ 149 boost::recursive_mutex::scoped_lock l(configuration_mutex_); 150 151 //一旦被調用，我們要確保有原始配置 152 if(!setup_) 153 { 154 last_config_ = config; 155 default_config_ = config; 156 setup_ = true; 157 return; 158 } 159 160 if(config.restore_defaults) { 161 config = default_config_; 162 //如果有人在參數服務器上設置默認值，要防止循環 163 config.restore_defaults = false; 164 } 165 166 if(planner_frequency_ != config.planner_frequency) 167 { 168 planner_frequency_ = config.planner_frequency; 169 p_freq_change_ = true; 170 } 171 172 if(controller_frequency_ != config.controller_frequency) 173 { 174 controller_frequency_ = config.controller_frequency; 175 c_freq_change_ = true; 176 } 177 178 planner_patience_ = config.planner_patience; 179 controller_patience_ = config.controller_patience; 180 max_planning_retries_ = config.max_planning_retries; 181 conservative_reset_dist_ = config.conservative_reset_dist; 182 183 recovery_behavior_enabled_ = config.recovery_behavior_enabled; 184 clearing_rotation_allowed_ = config.clearing_rotation_allowed; 185 shutdown_costmaps_ = config.shutdown_costmaps; 186 187 oscillation_timeout_ = config.oscillation_timeout; 188 oscillation_distance_ = config.oscillation_distance; 189 if(config.base_global_planner != last_config_.base_global_planner) { 190 boost::shared_ptr<nav_core::BaseGlobalPlanner> old_planner = planner_; 191 //創建全局規划 192 ROS_INFO("Loading global planner %s", config.base_global_planner.c_str()); 193 try { 194 planner_ = bgp_loader_.createInstance(config.base_global_planner); 195 196 // 等待當前規划結束 197 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 198 199 // 在新規划開始前clear舊的 200 planner_plan_->clear(); 201 latest_plan_->clear(); 202 controller_plan_->clear(); 203 resetState(); 204 planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_); 205 206 lock.unlock(); 207 } catch (const pluginlib::PluginlibException& ex) { 208 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \ 209 containing library is built? Exception: %s", config.base_global_planner.c_str(), ex.what()); 210 planner_ = old_planner; 211 config.base_global_planner = last_config_.base_global_planner; 212 } 213 } 214 215 if(config.base_local_planner != last_config_.base_local_planner){ 216 boost::shared_ptr<nav_core::BaseLocalPlanner> old_planner = tc_; 217 //創建局部規划 218 try { 219 tc_ = blp_loader_.createInstance(config.base_local_planner); 220 // 清理舊的 221 planner_plan_->clear(); 222 latest_plan_->clear(); 223 controller_plan_->clear(); 224 resetState(); 225 tc_->initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_); 226 } catch (const pluginlib::PluginlibException& ex) { 227 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \ 228 containing library is built? Exception: %s", config.base_local_planner.c_str(), ex.what()); 229 tc_ = old_planner; 230 config.base_local_planner = last_config_.base_local_planner; 231 } 232 } 233 234 last_config_ = config; 235 } 236 237 //為rviz等提供調用借口，將geometry_msgs::PoseStamped形式的goal轉換成move_base_msgs::MoveBaseActionGoal，再發布到對應類型的goal話題中 238 void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){ 239 ROS_DEBUG_NAMED("move_base","In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server."); 240 move_base_msgs::MoveBaseActionGoal action_goal; 241 action_goal.header.stamp = ros::Time::now(); 242 action_goal.goal.target_pose = *goal; 243 244 action_goal_pub_.publish(action_goal); 245 } 246 247 void MoveBase::clearCostmapWindows(double size_x, double size_y){ 248 geometry_msgs::PoseStamped global_pose; 249 250 //clear the planner's costmap 251 getRobotPose(global_pose, planner_costmap_ros_); 252 253 std::vector<geometry_msgs::Point> clear_poly; 254 double x = global_pose.pose.position.x; 255 double y = global_pose.pose.position.y; 256 geometry_msgs::Point pt; 257 258 pt.x = x - size_x / 2; 259 pt.y = y - size_y / 2; 260 clear_poly.push_back(pt); 261 262 pt.x = x + size_x / 2; 263 pt.y = y - size_y / 2; 264 clear_poly.push_back(pt); 265 266 pt.x = x + size_x / 2; 267 pt.y = y + size_y / 2; 268 clear_poly.push_back(pt); 269 270 pt.x = x - size_x / 2; 271 pt.y = y + size_y / 2; 272 clear_poly.push_back(pt); 273 274 planner_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE); 275 276 //clear the controller's costmap 277 getRobotPose(global_pose, controller_costmap_ros_); 278 279 clear_poly.clear(); 280 x = global_pose.pose.position.x; 281 y = global_pose.pose.position.y; 282 283 pt.x = x - size_x / 2; 284 pt.y = y - size_y / 2; 285 clear_poly.push_back(pt); 286 287 pt.x = x + size_x / 2; 288 pt.y = y - size_y / 2; 289 clear_poly.push_back(pt); 290 291 pt.x = x + size_x / 2; 292 pt.y = y + size_y / 2; 293 clear_poly.push_back(pt); 294 295 pt.x = x - size_x / 2; 296 pt.y = y + size_y / 2; 297 clear_poly.push_back(pt); 298 299 controller_costmap_ros_->getCostmap()->setConvexPolygonCost(clear_poly, costmap_2d::FREE_SPACE); 300 } 301 302 bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){ 303 //clear the costmaps 304 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_controller(*(controller_costmap_ros_->getCostmap()->getMutex())); 305 controller_costmap_ros_->resetLayers(); 306 307 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_planner(*(planner_costmap_ros_->getCostmap()->getMutex())); 308 planner_costmap_ros_->resetLayers(); 309 return true; 310 } 311 312 313 bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp){ 314 if(as_->isActive()){ 315 ROS_ERROR("move_base must be in an inactive state to make a plan for an external user"); 316 return false; 317 } 318 //make sure we have a costmap for our planner 319 if(planner_costmap_ros_ == NULL){ 320 ROS_ERROR("move_base cannot make a plan for you because it doesn't have a costmap"); 321 return false; 322 } 323 324 //1. 獲取起始點 325 geometry_msgs::PoseStamped start; 326 //如果起始點為空，設置global_pose為起始點 327 if(req.start.header.frame_id.empty()) 328 { 329 geometry_msgs::PoseStamped global_pose; 330 if(!getRobotPose(global_pose, planner_costmap_ros_)){ 331 ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot"); 332 return false; 333 } 334 start = global_pose; 335 } 336 else 337 { 338 start = req.start; 339 } 340 341 //update the copy of the costmap the planner uses 342 clearCostmapWindows(2 * clearing_radius_, 2 * clearing_radius_); 343 344 //制定規划策略 345 std::vector<geometry_msgs::PoseStamped> global_plan; 346 if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){ 347 ROS_DEBUG_NAMED("move_base","Failed to find a plan to exact goal of (%.2f, %.2f), searching for a feasible goal within tolerance", 348 req.goal.pose.position.x, req.goal.pose.position.y); 349 350 //search outwards for a feasible goal within the specified tolerance在規定的公差范圍內向外尋找可行的goal 351 geometry_msgs::PoseStamped p; 352 p = req.goal; 353 bool found_legal = false; 354 float resolution = planner_costmap_ros_->getCostmap()->getResolution(); 355 float search_increment = resolution*3.0;//以3倍分辨率的增量向外尋找 356 if(req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance; 357 for(float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment) { 358 for(float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment) { 359 for(float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment) { 360 361 //不在本位置的外側layer查找，太近的不找 362 if(x_offset < max_offset-1e-9 && y_offset < max_offset-1e-9) continue; 363 364 //從兩個方向x、y查找精確的goal 365 for(float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0) { 366 367 //第一次遍歷如果偏移量過小,則去除這個點或者上一點 368 if(y_offset < 1e-9 && y_mult < -1.0 + 1e-9) continue; 369 370 for(float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0) { 371 if(x_offset < 1e-9 && x_mult < -1.0 + 1e-9) continue; 372 373 p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult; 374 p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult; 375 376 if(planner_->makePlan(start, p, global_plan)){ 377 if(!global_plan.empty()){ 378 379 //adding the (unreachable) original goal to the end of the global plan, in case the local planner can get you there 380 //(the reachable goal should have been added by the global planner) 381 global_plan.push_back(req.goal); 382 383 found_legal = true; 384 ROS_DEBUG_NAMED("move_base", "Found a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y); 385 break; 386 } 387 } 388 else{ 389 ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y); 390 } 391 } 392 } 393 } 394 } 395 } 396 } 397 398 //copy the plan into a message to send out 399 resp.plan.poses.resize(global_plan.size()); 400 for(unsigned int i = 0; i < global_plan.size(); ++i){ 401 resp.plan.poses[i] = global_plan[i]; 402 } 403 404 return true; 405 } 406 //析構函數 407 MoveBase::~MoveBase(){ 408 recovery_behaviors_.clear(); 409 410 delete dsrv_; 411 412 if(as_ != NULL) 413 delete as_; 414 415 if(planner_costmap_ros_ != NULL) 416 delete planner_costmap_ros_; 417 418 if(controller_costmap_ros_ != NULL) 419 delete controller_costmap_ros_; 420 421 planner_thread_->interrupt(); 422 planner_thread_->join(); 423 424 delete planner_thread_; 425 426 delete planner_plan_; 427 delete latest_plan_; 428 delete controller_plan_; 429 430 planner_.reset(); 431 tc_.reset(); 432 } 433 434 bool MoveBase::makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){ 435 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(planner_costmap_ros_->getCostmap()->getMutex())); 436 437 //規划初始化 438 plan.clear(); 439 440 //激活句柄時調用 441 if(planner_costmap_ros_ == NULL) { 442 ROS_ERROR("Planner costmap ROS is NULL, unable to create global plan"); 443 return false; 444 } 445 446 //得到機器人起始點 447 geometry_msgs::PoseStamped global_pose; 448 if(!getRobotPose(global_pose, planner_costmap_ros_)) { 449 ROS_WARN("Unable to get starting pose of robot, unable to create global plan"); 450 return false; 451 } 452 453 const geometry_msgs::PoseStamped& start = global_pose; 454 455 //如果規划失敗或者返回一個長度為0的規划，則規划失敗 456 if(!planner_->makePlan(start, goal, plan) || plan.empty()){ 457 ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", goal.pose.position.x, goal.pose.position.y); 458 return false; 459 } 460 461 return true; 462 } 463 464 void MoveBase::publishZeroVelocity(){ 465 geometry_msgs::Twist cmd_vel; 466 cmd_vel.linear.x = 0.0; 467 cmd_vel.linear.y = 0.0; 468 cmd_vel.angular.z = 0.0; 469 vel_pub_.publish(cmd_vel); 470 } 471 472 bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){ 473 //1、首先檢查四元數是否元素完整 474 if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){ 475 ROS_ERROR("Quaternion has nans or infs... discarding as a navigation goal"); 476 return false; 477 } 478 479 tf2::Quaternion tf_q(q.x, q.y, q.z, q.w); 480 481 //2、檢查四元數是否趨近於0 482 if(tf_q.length2() < 1e-6){ 483 ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal"); 484 return false; 485 } 486 487 //3、對四元數規范化，轉化為vector 488 tf_q.normalize(); 489 490 tf2::Vector3 up(0, 0, 1); 491 492 double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle())); 493 494 if(fabs(dot - 1) > 1e-3){ 495 ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical."); 496 return false; 497 } 498 499 return true; 500 } 501 502 geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg){ 503 std::string global_frame = planner_costmap_ros_->getGlobalFrameID(); 504 geometry_msgs::PoseStamped goal_pose, global_pose; 505 goal_pose = goal_pose_msg; 506 507 //tf一下 508 goal_pose.header.stamp = ros::Time(); 509 510 try{ 511 tf_.transform(goal_pose_msg, global_pose, global_frame); 512 } 513 catch(tf2::TransformException& ex){ 514 ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s", 515 goal_pose.header.frame_id.c_str(), global_frame.c_str(), ex.what()); 516 return goal_pose_msg; 517 } 518 519 return global_pose; 520 } 521 522 void MoveBase::wakePlanner(const ros::TimerEvent& event) 523 { 524 // we have slept long enough for rate 525 planner_cond_.notify_one(); 526 } 527 528 //planner線程的入口。這個函數需要等待actionlib服務器的cbMoveBase::executeCb來喚醒啟動。 529 //主要作用是調用全局路徑規划獲取路徑，同時保證規划的周期性以及規划超時清除goal 530 void MoveBase::planThread(){ 531 ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread..."); 532 ros::NodeHandle n; 533 ros::Timer timer; 534 bool wait_for_wake = false; 535 //1. 創建遞歸鎖 536 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 537 while(n.ok()){ 538 //check if we should run the planner (the mutex is locked) 539 //2.判斷是否阻塞線程 540 while(wait_for_wake || !runPlanner_){ 541 //if we should not be running the planner then suspend this thread 542 ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending"); 543 //當 std::condition_variable 對象的某個 wait 函數被調用的時候， 544 //它使用 std::unique_lock(通過 std::mutex) 來鎖住當前線程。 545 //當前線程會一直被阻塞，直到另外一個線程在相同的 std::condition_variable 對象上調用了 notification 函數來喚醒當前線程。 546 planner_cond_.wait(lock); 547 wait_for_wake = false; 548 } 549 ros::Time start_time = ros::Time::now(); 550 551 //time to plan! get a copy of the goal and unlock the mutex 552 geometry_msgs::PoseStamped temp_goal = planner_goal_; 553 lock.unlock(); 554 ROS_DEBUG_NAMED("move_base_plan_thread","Planning..."); 555 556 //run planner 557 558 //3. 獲取規划的全局路徑 559 //這里的makePlan作用是獲取機器人的位姿作為起點，然后調用全局規划器的makePlan返回規划路徑，存儲在planner_plan_ 560 planner_plan_->clear(); 561 bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_); 562 563 564 //4.如果獲得了plan,則將其賦值給latest_plan_ 565 if(gotPlan){ 566 ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size()); 567 //pointer swap the plans under mutex (the controller will pull from latest_plan_) 568 std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_; 569 570 lock.lock(); 571 planner_plan_ = latest_plan_; 572 latest_plan_ = temp_plan; 573 last_valid_plan_ = ros::Time::now(); 574 planning_retries_ = 0; 575 new_global_plan_ = true; 576 577 ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner"); 578 579 //make sure we only start the controller if we still haven't reached the goal 580 if(runPlanner_) 581 state_ = CONTROLLING; 582 if(planner_frequency_ <= 0) 583 runPlanner_ = false; 584 lock.unlock(); 585 } 586 587 //5. 達到一定條件后停止路徑規划,進入清障模式 588 //如果沒有規划出路徑，並且處於PLANNING狀態，則判斷是否超過最大規划周期或者規划次數 589 //如果是則進入自轉模式，否則應該會等待MoveBase::executeCycle的喚醒再次規划 590 else if(state_==PLANNING){ 591 //僅在MoveBase::executeCb及其調用的MoveBase::executeCycle或者重置狀態時會被設置為PLANNING 592 //一般是剛獲得新目標，或者得到路徑但計算不出下一步控制時重新進行路徑規划 593 ROS_DEBUG_NAMED("move_base_plan_thread","No Plan..."); 594 ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_); 595 596 //check if we've tried to make a plan for over our time limit or our maximum number of retries 597 //issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_ 598 //is negative (the default), it is just ignored and we have the same behavior as ever 599 lock.lock(); 600 planning_retries_++; 601 if(runPlanner_ && 602 (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){ 603 //we'll move into our obstacle clearing mode 604 state_ = CLEARING; 605 runPlanner_ = false; // proper solution for issue #523 606 publishZeroVelocity(); 607 recovery_trigger_ = PLANNING_R; 608 } 609 610 lock.unlock(); 611 } 612 613 //take the mutex for the next iteration 614 lock.lock(); 615 616 617 //6.設置睡眠模式 618 //如果還沒到規划周期則定時器睡眠，在定時器中斷中通過planner_cond_喚醒，這里規划周期為0 619 if(planner_frequency_ > 0){ 620 ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now(); 621 if (sleep_time > ros::Duration(0.0)){ 622 wait_for_wake = true; 623 timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this); 624 } 625 } 626 } 627 } 628 629 void MoveBase::executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal) 630 { 631 632 //1. 如果目標非法,返回 633 if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){ 634 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion"); 635 return; 636 } 637 638 //2. 將目標的坐標系統一轉換為全局坐標系 639 geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose); 640 641 //3. 設置目標點並喚醒路徑規划線程 642 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 643 planner_goal_ = goal; 644 runPlanner_ = true; 645 planner_cond_.notify_one(); 646 lock.unlock(); 647 648 current_goal_pub_.publish(goal); 649 std::vector<geometry_msgs::PoseStamped> global_plan; 650 651 ros::Rate r(controller_frequency_); 652 //4. 開啟costmap更新 653 if(shutdown_costmaps_){ 654 ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously"); 655 planner_costmap_ros_->start(); 656 controller_costmap_ros_->start(); 657 } 658 659 //5. 重置時間標志位 660 last_valid_control_ = ros::Time::now(); 661 last_valid_plan_ = ros::Time::now(); 662 last_oscillation_reset_ = ros::Time::now(); 663 planning_retries_ = 0; 664 665 ros::NodeHandle n; 666 667 //6. 開啟循環，循環判斷是否有新的goal搶占 668 while(n.ok()) 669 { 670 671 //7. 修改循環頻率 672 if(c_freq_change_) 673 { 674 ROS_INFO("Setting controller frequency to %.2f", controller_frequency_); 675 r = ros::Rate(controller_frequency_); 676 c_freq_change_ = false; 677 } 678 679 //8. 如果獲得一個搶占式目標 680 if(as_->isPreemptRequested()){ 681 if(as_->isNewGoalAvailable()){ 682 //if we're active and a new goal is available, we'll accept it, but we won't shut anything down 683 move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal(); 684 685 //9.如果目標無效,則返回 686 if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){ 687 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion"); 688 return; 689 } 690 //10.將目標轉換為全局坐標系 691 goal = goalToGlobalFrame(new_goal.target_pose); 692 693 //we'll make sure that we reset our state for the next execution cycle 694 695 //11.設置狀態為PLANNING 696 recovery_index_ = 0; 697 state_ = PLANNING; 698 699 //we have a new goal so make sure the planner is awake 700 701 //12. 設置目標點並喚醒路徑規划線程 702 lock.lock(); 703 planner_goal_ = goal; 704 runPlanner_ = true; 705 planner_cond_.notify_one(); 706 lock.unlock(); 707 708 //13. 把goal發布給可視化工具 709 ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y); 710 current_goal_pub_.publish(goal); 711 712 //make sure to reset our timeouts and counters 713 //14. 重置規划時間 714 last_valid_control_ = ros::Time::now(); 715 last_valid_plan_ = ros::Time::now(); 716 last_oscillation_reset_ = ros::Time::now(); 717 planning_retries_ = 0; 718 } 719 else { 720 721 //14.重置狀態,設置為搶占式任務 722 //if we've been preempted explicitly we need to shut things down 723 resetState(); 724 725 //通知ActionServer已成功搶占 726 ROS_DEBUG_NAMED("move_base","Move base preempting the current goal"); 727 as_->setPreempted(); 728 729 //we'll actually return from execute after preempting 730 return; 731 } 732 } 733 734 //we also want to check if we've changed global frames because we need to transform our goal pose 735 //15.如果目標點的坐標系和全局地圖的坐標系不相同 736 if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){ 737 //16,轉換目標點坐標系 738 goal = goalToGlobalFrame(goal); 739 740 //we want to go back to the planning state for the next execution cycle 741 recovery_index_ = 0; 742 state_ = PLANNING; 743 744 //17. 設置目標點並喚醒路徑規划線程 745 lock.lock(); 746 planner_goal_ = goal; 747 runPlanner_ = true; 748 planner_cond_.notify_one(); 749 lock.unlock(); 750 751 //publish the goal point to the visualizer 752 ROS_DEBUG_NAMED("move_base","The global frame for move_base has changed, new frame: %s, new goal position x: %.2f, y: %.2f", goal.header.frame_id.c_str(), goal.pose.position.x, goal.pose.position.y); 753 current_goal_pub_.publish(goal); 754 755 //18.重置規划器相關時間標志位 756 last_valid_control_ = ros::Time::now(); 757 last_valid_plan_ = ros::Time::now(); 758 last_oscillation_reset_ = ros::Time::now(); 759 planning_retries_ = 0; 760 } 761 762 //for timing that gives real time even in simulation 763 ros::WallTime start = ros::WallTime::now(); 764 765 //19. 到達目標點的真正工作，控制機器人進行跟隨 766 bool done = executeCycle(goal, global_plan); 767 768 //20.如果完成任務則返回 769 if(done) 770 return; 771 772 //check if execution of the goal has completed in some way 773 774 ros::WallDuration t_diff = ros::WallTime::now() - start; 775 ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec()); 776 //21. 執行休眠動作 777 r.sleep(); 778 //make sure to sleep for the remainder of our cycle time 779 if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING) 780 ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec()); 781 } 782 783 //22. 喚醒計划線程，以便它可以干凈地退出 784 lock.lock(); 785 runPlanner_ = true; 786 planner_cond_.notify_one(); 787 lock.unlock(); 788 789 //23. 如果節點結束就終止並返回 790 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed"); 791 return; 792 } 793 794 double MoveBase::distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2) 795 { 796 return hypot(p1.pose.position.x - p2.pose.position.x, p1.pose.position.y - p2.pose.position.y); 797 } 798 799 //兩個參數分別是目標點位姿以及規划出的全局路徑.通過這兩個參數，實現以下功能： 800 //利用局部路徑規划器直接輸出輪子速度，控制機器人按照路徑走到目標點，成功返回真，否則返回假。在actionlib server的回調MoveBase::executeCb中被調用。 801 bool MoveBase::executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan){ 802 803 boost::recursive_mutex::scoped_lock ecl(configuration_mutex_); 804 //we need to be able to publish velocity commands 805 geometry_msgs::Twist cmd_vel; 806 807 808 //1.獲取機器人當前位置 809 geometry_msgs::PoseStamped global_pose; 810 getRobotPose(global_pose, planner_costmap_ros_); 811 const geometry_msgs::PoseStamped& current_position = global_pose; 812 813 //push the feedback out 814 move_base_msgs::MoveBaseFeedback feedback; 815 feedback.base_position = current_position; 816 as_->publishFeedback(feedback); 817 818 //2.重置震盪標志位 819 if(distance(current_position, oscillation_pose_) >= oscillation_distance_) 820 { 821 last_oscillation_reset_ = ros::Time::now(); 822 oscillation_pose_ = current_position; 823 824 //if our last recovery was caused by oscillation, we want to reset the recovery index 825 if(recovery_trigger_ == OSCILLATION_R) 826 recovery_index_ = 0; 827 } 828 829 //3.地圖數據超時，即觀測傳感器數據不夠新,停止機器人,返回false 830 if(!controller_costmap_ros_->isCurrent()){ 831 ROS_WARN("[%s]:Sensor data is out of date, we're not going to allow commanding of the base for safety",ros::this_node::getName().c_str()); 832 publishZeroVelocity(); 833 return false; 834 } 835 836 //4. 如果獲取新的全局路徑,則將它傳輸給控制器，完成latest_plan_到controller_plan_的轉換 837 if(new_global_plan_){ 838 //make sure to set the new plan flag to false 839 new_global_plan_ = false; 840 841 ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers"); 842 843 //do a pointer swap under mutex 844 std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_; 845 846 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 847 controller_plan_ = latest_plan_; 848 latest_plan_ = temp_plan; 849 lock.unlock(); 850 ROS_DEBUG_NAMED("move_base","pointers swapped!"); 851 852 //5. 給控制器設置全局路徑 853 if(!tc_->setPlan(*controller_plan_)){ 854 //ABORT and SHUTDOWN COSTMAPS 855 ROS_ERROR("Failed to pass global plan to the controller, aborting."); 856 resetState(); 857 858 //disable the planner thread 859 lock.lock(); 860 runPlanner_ = false; 861 lock.unlock(); 862 //6.設置動作中斷,返回true 863 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller."); 864 return true; 865 } 866 867 //如果全局路徑有效，則不需要recovery 868 if(recovery_trigger_ == PLANNING_R) 869 recovery_index_ = 0; 870 } 871 872 873 //5. move_base 狀態機，處理導航的控制邏輯 874 //一般默認狀態或者接收到一個有效goal時是PLANNING，在規划出全局路徑后state_會由PLANNING->CONTROLLING 875 //如果規划失敗則由PLANNING->CLEARING。 876 switch(state_){ 877 //6. 機器人規划狀態,嘗試獲取一條全局路徑 878 case PLANNING: 879 { 880 boost::recursive_mutex::scoped_lock lock(planner_mutex_); 881 runPlanner_ = true; 882 planner_cond_.notify_one();//還在PLANNING中則喚醒規划線程讓它干活 883 } 884 ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state."); 885 break; 886 887 //7.機器人控制狀態,嘗試獲取一個有效的速度命令 888 case CONTROLLING: 889 ROS_DEBUG_NAMED("move_base","In controlling state."); 890 891 //8.如果到達目標點,重置狀態,設置動作成功,返回true 892 if(tc_->isGoalReached()){ 893 ROS_DEBUG_NAMED("move_base","Goal reached!"); 894 resetState(); 895 896 //disable the planner thread 897 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 898 runPlanner_ = false; 899 lock.unlock(); 900 901 as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached."); 902 return true; 903 } 904 905 //9. 如果超過震盪時間,停止機器人,設置清障標志位 906 if(oscillation_timeout_ > 0.0 && 907 last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now()) 908 { 909 publishZeroVelocity(); 910 state_ = CLEARING; 911 recovery_trigger_ = OSCILLATION_R; 912 } 913 914 { 915 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock(*(controller_costmap_ros_->getCostmap()->getMutex())); 916 //10. 獲取有效速度,如果獲取成功，直接發布到cmd_vel 917 if(tc_->computeVelocityCommands(cmd_vel)){ 918 ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf", 919 cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z ); 920 last_valid_control_ = ros::Time::now(); 921 //make sure that we send the velocity command to the base 922 vel_pub_.publish(cmd_vel); 923 if(recovery_trigger_ == CONTROLLING_R) 924 recovery_index_ = 0; 925 } 926 else { 927 //11.如果沒有獲取到有效速度 928 ROS_DEBUG_NAMED("move_base", "The local planner could not find a valid plan."); 929 ros::Time attempt_end = last_valid_control_ + ros::Duration(controller_patience_); 930 931 //12.判斷超過嘗試時間,如果超時，停止機器人,進入清障模式 932 if(ros::Time::now() > attempt_end){ 933 //we'll move into our obstacle clearing mode 934 publishZeroVelocity(); 935 state_ = CLEARING; 936 recovery_trigger_ = CONTROLLING_R; 937 } 938 else{ 939 //如果不超時，讓全局再規划一個路徑 940 last_valid_plan_ = ros::Time::now(); 941 planning_retries_ = 0; 942 state_ = PLANNING; 943 publishZeroVelocity(); 944 945 //enable the planner thread in case it isn't running on a clock 946 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 947 runPlanner_ = true; 948 planner_cond_.notify_one(); 949 lock.unlock(); 950 } 951 } 952 } 953 954 break; 955 956 //13. 規划或者控制失敗，恢復或者進入到清障模式 957 case CLEARING: 958 ROS_DEBUG_NAMED("move_base","In clearing/recovery state"); 959 960 //14. 有可用恢復器,執行恢復動作,設置狀態為PLANNING 961 if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){ 962 ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_, recovery_behaviors_.size()); 963 recovery_behaviors_[recovery_index_]->runBehavior(); 964 965 //we at least want to give the robot some time to stop oscillating after executing the behavior 966 last_oscillation_reset_ = ros::Time::now(); 967 968 //we'll check if the recovery behavior actually worked 969 ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state"); 970 last_valid_plan_ = ros::Time::now(); 971 planning_retries_ = 0; 972 state_ = PLANNING; 973 974 //update the index of the next recovery behavior that we'll try 975 recovery_index_++; 976 } 977 else{ 978 979 //15.沒有可用恢復器,結束動作,返回true 980 ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it."); 981 //disable the planner thread 982 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 983 runPlanner_ = false; 984 lock.unlock(); 985 986 ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this."); 987 988 if(recovery_trigger_ == CONTROLLING_R){ 989 ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors"); 990 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors."); 991 } 992 else if(recovery_trigger_ == PLANNING_R){ 993 ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors"); 994 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors."); 995 } 996 else if(recovery_trigger_ == OSCILLATION_R){ 997 ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors"); 998 as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors."); 999 } 1000 resetState(); 1001 return true; 1002 } 1003 break; 1004 default: 1005 ROS_ERROR("This case should never be reached, something is wrong, aborting"); 1006 resetState(); 1007 //disable the planner thread 1008 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 1009 runPlanner_ = false; 1010 lock.unlock(); 1011 as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it."); 1012 return true; 1013 } 1014 1015 //we aren't done yet 1016 return false; 1017 } 1018 1019 //recovery是指恢復的規划器，其跟全局規划器和局部規划器是同一個等級的。 1020 //不同的是，它是在機器人在局部代價地圖或者全局代價地圖中找不到路時才會被調用，比如rotate_recovery讓機器人原地360°旋轉，clear_costmap_recovery將代價地圖恢復到靜態地圖的樣子。 1021 bool MoveBase::loadRecoveryBehaviors(ros::NodeHandle node){ 1022 XmlRpc::XmlRpcValue behavior_list; 1023 if(node.getParam("recovery_behaviors", behavior_list)){ 1024 if(behavior_list.getType() == XmlRpc::XmlRpcValue::TypeArray){ 1025 for(int i = 0; i < behavior_list.size(); ++i){ 1026 if(behavior_list[i].getType() == XmlRpc::XmlRpcValue::TypeStruct){ 1027 if(behavior_list[i].hasMember("name") && behavior_list[i].hasMember("type")){ 1028 //check for recovery behaviors with the same name 1029 for(int j = i + 1; j < behavior_list.size(); j++){ 1030 if(behavior_list[j].getType() == XmlRpc::XmlRpcValue::TypeStruct){ 1031 if(behavior_list[j].hasMember("name") && behavior_list[j].hasMember("type")){ 1032 std::string name_i = behavior_list[i]["name"]; 1033 std::string name_j = behavior_list[j]["name"]; 1034 if(name_i == name_j){ 1035 ROS_ERROR("A recovery behavior with the name %s already exists, this is not allowed. Using the default recovery behaviors instead.", 1036 name_i.c_str()); 1037 return false; 1038 } 1039 } 1040 } 1041 } 1042 } 1043 else{ 1044 ROS_ERROR("Recovery behaviors must have a name and a type and this does not. Using the default recovery behaviors instead."); 1045 return false; 1046 } 1047 } 1048 else{ 1049 ROS_ERROR("Recovery behaviors must be specified as maps, but they are XmlRpcType %d. We'll use the default recovery behaviors instead.", 1050 behavior_list[i].getType()); 1051 return false; 1052 } 1053 } 1054 1055 //if we've made it to this point, we know that the list is legal so we'll create all the recovery behaviors 1056 for(int i = 0; i < behavior_list.size(); ++i){ 1057 try{ 1058 //check if a non fully qualified name has potentially been passed in 1059 if(!recovery_loader_.isClassAvailable(behavior_list[i]["type"])){ 1060 std::vector<std::string> classes = recovery_loader_.getDeclaredClasses(); 1061 for(unsigned int i = 0; i < classes.size(); ++i){ 1062 if(behavior_list[i]["type"] == recovery_loader_.getName(classes[i])){ 1063 //if we've found a match... we'll get the fully qualified name and break out of the loop 1064 ROS_WARN("Recovery behavior specifications should now include the package name. You are using a deprecated API. Please switch from %s to %s in your yaml file.", 1065 std::string(behavior_list[i]["type"]).c_str(), classes[i].c_str()); 1066 behavior_list[i]["type"] = classes[i]; 1067 break; 1068 } 1069 } 1070 } 1071 1072 boost::shared_ptr<nav_core::RecoveryBehavior> behavior(recovery_loader_.createInstance(behavior_list[i]["type"])); 1073 1074 //shouldn't be possible, but it won't hurt to check 1075 if(behavior.get() == NULL){ 1076 ROS_ERROR("The ClassLoader returned a null pointer without throwing an exception. This should not happen"); 1077 return false; 1078 } 1079 1080 //initialize the recovery behavior with its name 1081 behavior->initialize(behavior_list[i]["name"], &tf_, planner_costmap_ros_, controller_costmap_ros_); 1082 recovery_behaviors_.push_back(behavior); 1083 } 1084 catch(pluginlib::PluginlibException& ex){ 1085 ROS_ERROR("Failed to load a plugin. Using default recovery behaviors. Error: %s", ex.what()); 1086 return false; 1087 } 1088 } 1089 } 1090 else{ 1091 ROS_ERROR("The recovery behavior specification must be a list, but is of XmlRpcType %d. We'll use the default recovery behaviors instead.", 1092 behavior_list.getType()); 1093 return false; 1094 } 1095 } 1096 else{ 1097 //if no recovery_behaviors are specified, we'll just load the defaults 1098 return false; 1099 } 1100 1101 //if we've made it here... we've constructed a recovery behavior list successfully 1102 return true; 1103 } 1104 1105 //we'll load our default recovery behaviors here 1106 void MoveBase::loadDefaultRecoveryBehaviors(){ 1107 recovery_behaviors_.clear(); 1108 try{ 1109 //we need to set some parameters based on what's been passed in to us to maintain backwards compatibility 1110 ros::NodeHandle n("~"); 1111 n.setParam("conservative_reset/reset_distance", conservative_reset_dist_); 1112 n.setParam("aggressive_reset/reset_distance", circumscribed_radius_ * 4); 1113 1114 //1、加載recovery behavior清理costmap 1115 boost::shared_ptr<nav_core::RecoveryBehavior> cons_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery")); 1116 cons_clear->initialize("conservative_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_); 1117 recovery_behaviors_.push_back(cons_clear); 1118 1119 //2、加載recovery behavior 原地旋轉 1120 boost::shared_ptr<nav_core::RecoveryBehavior> rotate(recovery_loader_.createInstance("rotate_recovery/RotateRecovery")); 1121 if(clearing_rotation_allowed_){ 1122 rotate->initialize("rotate_recovery", &tf_, planner_costmap_ros_, controller_costmap_ros_); 1123 recovery_behaviors_.push_back(rotate); 1124 } 1125 1126 //3、加載 recovery behavior 重置 costmap 1127 boost::shared_ptr<nav_core::RecoveryBehavior> ags_clear(recovery_loader_.createInstance("clear_costmap_recovery/ClearCostmapRecovery")); 1128 ags_clear->initialize("aggressive_reset", &tf_, planner_costmap_ros_, controller_costmap_ros_); 1129 recovery_behaviors_.push_back(ags_clear); 1130 1131 //4、再一次旋轉 1132 if(clearing_rotation_allowed_) 1133 recovery_behaviors_.push_back(rotate); 1134 } 1135 catch(pluginlib::PluginlibException& ex){ 1136 ROS_FATAL("Failed to load a plugin. This should not happen on default recovery behaviors. Error: %s", ex.what()); 1137 } 1138 1139 return; 1140 } 1141 1142 void MoveBase::resetState(){ 1143 // Disable the planner thread 1144 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 1145 runPlanner_ = false; 1146 lock.unlock(); 1147 1148 // Reset statemachine 1149 state_ = PLANNING; 1150 recovery_index_ = 0; 1151 recovery_trigger_ = PLANNING_R; 1152 publishZeroVelocity(); 1153 1154 //if we shutdown our costmaps when we're deactivated... we'll do that now 1155 if(shutdown_costmaps_){ 1156 ROS_DEBUG_NAMED("move_base","Stopping costmaps"); 1157 planner_costmap_ros_->stop(); 1158 controller_costmap_ros_->stop(); 1159 } 1160 } 1161 1162 bool MoveBase::getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap) 1163 { 1164 tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose); 1165 geometry_msgs::PoseStamped robot_pose; 1166 tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose); 1167 robot_pose.header.frame_id = robot_base_frame_; 1168 robot_pose.header.stamp = ros::Time(); // latest available 1169 ros::Time current_time = ros::Time::now(); // save time for checking tf delay later 1170 1171 // 轉換到統一的全局坐標 1172 try 1173 { 1174 tf_.transform(robot_pose, global_pose, costmap->getGlobalFrameID()); 1175 } 1176 catch (tf2::LookupException& ex) 1177 { 1178 ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what()); 1179 return false; 1180 } 1181 catch (tf2::ConnectivityException& ex) 1182 { 1183 ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what()); 1184 return false; 1185 } 1186 catch (tf2::ExtrapolationException& ex) 1187 { 1188 ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what()); 1189 return false; 1190 } 1191 1192 // 全局坐標時間戳是否在costmap要求下 1193 if (current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance()) 1194 { 1195 ROS_WARN_THROTTLE(1.0, "Transform timeout for %s. " \ 1196 "Current time: %.4f, pose stamp: %.4f, tolerance: %.4f", costmap->getName().c_str(), 1197 current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance()); 1198 return false; 1199 } 1200 1201 return true; 1202 } 1203 };