Zero. Preface
Previously wrote an article on move_ The analysis of base is called the most complete in the whole network because the whole package is annotated hhh.
However, because the article is presented in the form of source code + comments, there are many problems, such as: it is not parsed in logical order (and the correct step to read the code is to look at it step by step according to the program execution order), it does not dig into the functions inside, but more about the understanding of this framework, There is no clear explanation of the common variables between various functions (for example, the state of move_base actually runs through the whole planning process, and each flag bit changes between different functions), etc.
Based on the previous post, this blog post explains step by step according to the program execution sequence, and digs deeply to a certain extent (for example, how it is related to other packages). It also explains the meaning of class members listed in the header file during the program explanation.
I may make mistakes. I welcome friends to criticize and correct me, and contact me by private letter. Thank you here.
Don't say much, start analyzing——
1, Entrance
The entry is file move_base_node.cpp, which declares move_base::MoveBase object move_base, the incoming parameter is TF2_ ros::Buffer& tf . When declared, it enters the constructor. But before we look at constructors, let's take a look at moving_ What data is there in the namespace base.
2, Header file - namespace
Enter the namespace of the entry. The namespace is move_base. Next, let's introduce the namespace move one by one_ Content of base:
1. Declare the server side, and the message type is move_base_msgs::MoveBaseAction
1 typedef actionlib::SimpleActionServer<move_base_msgs::MoveBaseAction> MoveBaseActionServer;
2. Enumerating movebase status representations
1 enum MoveBaseState {
2 PLANNING,
3 CONTROLLING,
4 CLEARING
5 };
3. Enumeration to trigger recovery mode
1 enum RecoveryTrigger
2 {
3 PLANNING_R,
4 CONTROLLING_R,
5 OSCILLATION_R
6 };
4. The MoveBase class uses the actionlib:: ActionServer interface, which moves the robot to the target location
1 class MoveBase{ } //Expand in detail below
3, Header file - MoveBase class
In move_ In the base namespace, the most important is the MoveBase class. The entry mentioned in (1) is actually a constructor. Here, first list the members in the class, and then explain the relationship and execution order between each function member and data member in detail in (4).
1. Constructor. The parameter passed in is tf
MoveBase(tf2_ros::Buffer& tf);
2. Destructor
virtual ~MoveBase();
3. Control closed-loop, global planning, target arrival return true, and no arrival return false
bool executeCycle(geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& global_plan);
4. Clear costmap
bool clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp);
5. When action is inactive, call this function and return plan
bool planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp);
6. New global planning, target point of goal planning, plan planning
bool makePlan(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan);
7. Load navigation recovery behavior from parameter server
bool loadRecoveryBehaviors(ros::NodeHandle node);
8. Load default navigation restore behavior
void loadDefaultRecoveryBehaviors();
9. Clear the obstacles in the local planning box of the robot, size_x length of local planning box x, size_y width of local programming box y
void clearCostmapWindows(double size_x, double size_y);
10. Issue command with speed 0
void publishZeroVelocity();
11. Reset move_ Status of base action. Set the speed to 0
void resetState();
12. Wake up the planner periodically
void wakePlanner(const ros::TimerEvent& event);
13. Other functions
1 void goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal); 2 3 void planThread(); 4 5 void executeCb(const move_base_msgs::MoveBaseGoalConstPtr& move_base_goal); 6 7 bool isQuaternionValid(const geometry_msgs::Quaternion& q); 8 9 bool getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap); 10 11 double distance(const geometry_msgs::PoseStamped& p1, const geometry_msgs::PoseStamped& p2); 12 13 geometry_msgs::PoseStamped goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg);
14. Data member
1 tf2_ros::Buffer& tf_; 2 3 MoveBaseActionServer* as_; //This is the server side of actionlib 4 5 boost::shared_ptr<nav_core::BaseLocalPlanner> tc_;//Local planner, the pointer after loading and creating the instance 6 costmap_2d::Costmap2DROS* planner_costmap_ros_, *controller_costmap_ros_;//Instantiation pointer of costmap 7 8 boost::shared_ptr<nav_core::BaseGlobalPlanner> planner_;//Global planner, the pointer after the instance is loaded and created 9 std::string robot_base_frame_, global_frame_; 10 11 std::vector<boost::shared_ptr<nav_core::RecoveryBehavior> > recovery_behaviors_;//It may be recovery after error 12 unsigned int recovery_index_; 13 14 geometry_msgs::PoseStamped global_pose_; 15 double planner_frequency_, controller_frequency_, inscribed_radius_, circumscribed_radius_; 16 double planner_patience_, controller_patience_; 17 int32_t max_planning_retries_; 18 uint32_t planning_retries_; 19 double conservative_reset_dist_, clearing_radius_; 20 ros::Publisher current_goal_pub_, vel_pub_, action_goal_pub_; 21 ros::Subscriber goal_sub_; 22 ros::ServiceServer make_plan_srv_, clear_costmaps_srv_; 23 bool shutdown_costmaps_, clearing_rotation_allowed_, recovery_behavior_enabled_; 24 double oscillation_timeout_, oscillation_distance_; 25 26 MoveBaseState state_; 27 RecoveryTrigger recovery_trigger_; 28 29 ros::Time last_valid_plan_, last_valid_control_, last_oscillation_reset_; 30 geometry_msgs::PoseStamped oscillation_pose_; 31 pluginlib::ClassLoader<nav_core::BaseGlobalPlanner> bgp_loader_; 32 pluginlib::ClassLoader<nav_core::BaseLocalPlanner> blp_loader_; 33 pluginlib::ClassLoader<nav_core::RecoveryBehavior> recovery_loader_; 34 35 //What kind of planner is triggered 36 std::vector<geometry_msgs::PoseStamped>* planner_plan_;//Save the latest planned path and pass it to latest_plan_ 37 std::vector<geometry_msgs::PoseStamped>* latest_plan_;//Pass to controller in executeCycle_ plan_ 38 std::vector<geometry_msgs::PoseStamped>* controller_plan_; 39 40 //Planner thread 41 bool runPlanner_; 42 boost::recursive_mutex planner_mutex_; 43 boost::condition_variable_any planner_cond_; 44 geometry_msgs::PoseStamped planner_goal_; 45 boost::thread* planner_thread_; 46 47 48 boost::recursive_mutex configuration_mutex_; 49 dynamic_reconfigure::Server<move_base::MoveBaseConfig> *dsrv_; 50 51 void reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level); 52 53 move_base::MoveBaseConfig last_config_; 54 move_base::MoveBaseConfig default_config_; 55 bool setup_, p_freq_change_, c_freq_change_; 56 bool new_global_plan_;
4, Here comes the text
File move_base.cpp defines the functions written in the header file above. Next, go through them one by one:
The entry is the constructor MoveBase::MoveBase.
First, a bunch of parameters are initialized:
1 tf_(tf), //tf2_ ROS:: buffer & reference? Address?
2 as_(NULL), //MoveBaseActionServer * pointer
3 planner_costmap_ros_(NULL), controller_costmap_ros_(NULL),//Instantiation pointer of costmap
4 bgp_loader_("nav_core", "nav_core::BaseGlobalPlanner"), //nav_core::BaseGlobalPlanner type plug-in
5 blp_loader_("nav_core", "nav_core::BaseLocalPlanner"),//nav_core::BaseLocalPlanner type plug-in
6 recovery_loader_("nav_core", "nav_core::RecoveryBehavior"), //nav_ Plug in of type core:: recoverybehavior
7 planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),//Three planners, see which one triggers
8 runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), //configuration parameter
9 new_global_plan_(false) //configuration parameter
Create move_base action, binding Callback function. Define a named move_ SimpleActionServer for base. The Callback of this server is moveBase::executeCb
1 as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
At this time, call the callback function movebase:: executecb (const move_base_msgs:: movebasegoalconstptr & move_base_goal), as follows:
If the target is illegal, return directly:
1 if(!isQuaternionValid(move_base_goal->target_pose.pose.orientation)){
2 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
3 return;
4 }
Among them, isquaternionvalid (const geometry_msgs:: quaternion & Q) function is as follows, which is mainly used to check the legitimacy of quaternions:
1 bool MoveBase::isQuaternionValid(const geometry_msgs::Quaternion& q){
2 //1. First, check whether the quaternion element is complete
3 if(!std::isfinite(q.x) || !std::isfinite(q.y) || !std::isfinite(q.z) || !std::isfinite(q.w)){
4 ROS_ERROR("Quaternion has nans or infs... discarding as a navigation goal");
5 return false;
6 }
7 tf2::Quaternion tf_q(q.x, q.y, q.z, q.w);
8 //2. Check whether the quaternion approaches 0
9 if(tf_q.length2() < 1e-6){
10 ROS_ERROR("Quaternion has length close to zero... discarding as navigation goal");
11 return false;
12 }
13 //3. Normalize quaternions and convert them into vector s
14 tf_q.normalize();
15 tf2::Vector3 up(0, 0, 1);
16 double dot = up.dot(up.rotate(tf_q.getAxis(), tf_q.getAngle()));
17 if(fabs(dot - 1) > 1e-3){
18 ROS_ERROR("Quaternion is invalid... for navigation the z-axis of the quaternion must be close to vertical.");
19 return false;
20 }
21
22 return true;
23 }
ok, go back to the callback function and continue——
Convert the coordinate system of the target to the global coordinate system:
1 geometry_msgs::PoseStamped goal = goalToGlobalFrame(move_base_goal->target_pose);
The function movebase:: goaltoglobalframe (const geometry_msg:: posestamped & goal_pose_msg) is as follows:
1 geometry_msgs::PoseStamped MoveBase::goalToGlobalFrame(const geometry_msgs::PoseStamped& goal_pose_msg){
2 std::string global_frame = planner_costmap_ros_->getGlobalFrameID();
3 geometry_msgs::PoseStamped goal_pose, global_pose;
4 goal_pose = goal_pose_msg;
5
6 //tf wait a minute
7 goal_pose.header.stamp = ros::Time();
8
9 try{
10 tf_.transform(goal_pose_msg, global_pose, global_frame);
11 }
12 catch(tf2::TransformException& ex){
13 ROS_WARN("Failed to transform the goal pose from %s into the %s frame: %s",
14 goal_pose.header.frame_id.c_str(), global_frame.c_str(), ex.what());
15 return goal_pose_msg;
16 }
17
18 return global_pose;
19 }
ok, go back to the callback function and continue——
Set the target point and wake up the path planning thread:
1 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 2 planner_goal_ = goal; 3 runPlanner_ = true; 4 planner_cond_.notify_one(); 5 lock.unlock();
Then issue goal and set the control frequency:
1 current_goal_pub_.publish(goal); 2 std::vector<geometry_msgs::PoseStamped> global_plan; 3 ros::Rate r(controller_frequency_);
Enable costmap update:
1 if(shutdown_costmaps_){
2 ROS_DEBUG_NAMED("move_base","Starting up costmaps that were shut down previously");
3 planner_costmap_ros_->start();
4 controller_costmap_ros_->start();
5 }
Reset time flag bit:
1 last_valid_control_ = ros::Time::now(); 2 last_valid_plan_ = ros::Time::now(); 3 last_oscillation_reset_ = ros::Time::now(); 4 planning_retries_ = 0;
Start the loop to judge whether there is a new goal preemption (important!!!):
1 while(n.ok())
2 {
3
4 //7. Modify cycle frequency
5 if(c_freq_change_)
6 {
7 ROS_INFO("Setting controller frequency to %.2f", controller_frequency_);
8 r = ros::Rate(controller_frequency_);
9 c_freq_change_ = false;
10 }
11
12 //8. If you get a preemptive target
13 if(as_->isPreemptRequested()){ //Preemption function of action
14 if(as_->isNewGoalAvailable()){
15 //If there is a new goal, it will be accepted, but other processes will not be closed
16 move_base_msgs::MoveBaseGoal new_goal = *as_->acceptNewGoal();
17
18 //9. If the target is invalid, return
19 if(!isQuaternionValid(new_goal.target_pose.pose.orientation)){
20 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on goal because it was sent with an invalid quaternion");
21 return;
22 }
23 //10. Convert the target to the global coordinate system
24 goal = goalToGlobalFrame(new_goal.target_pose);
25
26 //we'll make sure that we reset our state for the next execution cycle
27 //11. Set the status to PLANNING
28 recovery_index_ = 0;
29 state_ = PLANNING;
30
31 //we have a new goal so make sure the planner is awake
32 //12. Set the target point and wake up the path planning thread
33 lock.lock();
34 planner_goal_ = goal;
35 runPlanner_ = true;
36 planner_cond_.notify_one();
37 lock.unlock();
38
39 //13. Publish goal to visualization tool
40 ROS_DEBUG_NAMED("move_base","move_base has received a goal of x: %.2f, y: %.2f", goal.pose.position.x, goal.pose.position.y);
41 current_goal_pub_.publish(goal);
42
43 //make sure to reset our timeouts and counters
44 //14. Reset planning time
45 last_valid_control_ = ros::Time::now();
46 last_valid_plan_ = ros::Time::now();
47 last_oscillation_reset_ = ros::Time::now();
48 planning_retries_ = 0;
49 }
50 else {
51
52 //14. Reset the status and set it to preemptive task
53 //if we've been preempted explicitly we need to shut things down
54 resetState();
55
56 //Notify ActionServer of successful preemption
57 ROS_DEBUG_NAMED("move_base","Move base preempting the current goal");
58 as_->setPreempted();
59
60 //we'll actually return from execute after preempting
61 return;
62 }
63 }
64
65 //we also want to check if we've changed global frames because we need to transform our goal pose
66 //15. If the coordinate system of the target point is different from that of the global map
67 if(goal.header.frame_id != planner_costmap_ros_->getGlobalFrameID()){
68 //16. Convert the target point coordinate system
69 goal = goalToGlobalFrame(goal);
70
71 //we want to go back to the planning state for the next execution cycle
72 recovery_index_ = 0;
73 state_ = PLANNING;
74
75 //17. Set the target point and wake up the path planning thread
76 lock.lock();
77 planner_goal_ = goal;
78 runPlanner_ = true;
79 planner_cond_.notify_one();
80 lock.unlock();
81
82 //publish the goal point to the visualizer
83 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);
84 current_goal_pub_.publish(goal);
85
86 //18. Reset the relevant time flag bit of the planner
87 last_valid_control_ = ros::Time::now();
88 last_valid_plan_ = ros::Time::now();
89 last_oscillation_reset_ = ros::Time::now();
90 planning_retries_ = 0;
91 }
92
93 //for timing that gives real time even in simulation
94 ros::WallTime start = ros::WallTime::now();
95
96 //19. For the real work of reaching the target point, control the robot to follow
97 bool done = executeCycle(goal, global_plan);
98
99 //20. Return if the task is completed
100 if(done)
101 return;
102
103 //check if execution of the goal has completed in some way
104
105 ros::WallDuration t_diff = ros::WallTime::now() - start;
106 ROS_DEBUG_NAMED("move_base","Full control cycle time: %.9f\n", t_diff.toSec());
107 //21. Execute sleep action
108 r.sleep();
109 //make sure to sleep for the remainder of our cycle time
110 if(r.cycleTime() > ros::Duration(1 / controller_frequency_) && state_ == CONTROLLING)
111 ROS_WARN("Control loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds", controller_frequency_, r.cycleTime().toSec());
112 }
Among them, the value of done is the return value of MoveBase::executeCycle() function. This value is very important and directly determines whether to reach the target point; MoveBase::executeCycle() function is a function that controls the robot to follow (important!!!), As follows:
Get the current position of the robot:
1 geometry_msgs::PoseStamped global_pose; 2 getRobotPose(global_pose, planner_costmap_ros_); 3 const geometry_msgs::PoseStamped& current_position = global_pose; 4 5 //push the feedback out 6 move_base_msgs::MoveBaseFeedback feedback; 7 feedback.base_position = current_position; 8 as_->publishFeedback(feedback);
The getRobotPose() function is as follows, which needs to align the coordinate system and timestamp:
1 bool MoveBase::getRobotPose(geometry_msgs::PoseStamped& global_pose, costmap_2d::Costmap2DROS* costmap)
2 {
3 tf2::toMsg(tf2::Transform::getIdentity(), global_pose.pose);
4 geometry_msgs::PoseStamped robot_pose;
5 tf2::toMsg(tf2::Transform::getIdentity(), robot_pose.pose);
6 robot_pose.header.frame_id = robot_base_frame_;
7 robot_pose.header.stamp = ros::Time(); // latest available
8 ros::Time current_time = ros::Time::now(); // save time for checking tf delay later
9
10 // Convert to unified global coordinates
11 try
12 {
13 tf_.transform(robot_pose, global_pose, costmap->getGlobalFrameID());
14 }
15 catch (tf2::LookupException& ex)
16 {
17 ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
18 return false;
19 }
20 catch (tf2::ConnectivityException& ex)
21 {
22 ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
23 return false;
24 }
25 catch (tf2::ExtrapolationException& ex)
26 {
27 ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
28 return false;
29 }
30
31 // Whether the global coordinate timestamp is under costmap requirements
32 if (current_time.toSec() - global_pose.header.stamp.toSec() > costmap->getTransformTolerance())
33 {
34 ROS_WARN_THROTTLE(1.0, "Transform timeout for %s. " \
35 "Current time: %.4f, pose stamp: %.4f, tolerance: %.4f", costmap->getName().c_str(),
36 current_time.toSec(), global_pose.header.stamp.toSec(), costmap->getTransformTolerance());
37 return false;
38 }
39
40 return true;
41 }
ok, return the MoveBase::executeCycle() function to continue——
Judge whether the current position and oscillate. The distance function returns the straight-line distance (Euclidean distance) between the two positions_ trigger_ Is an object that enumerates recoverytriggers:
1 if(distance(current_position, oscillation_pose_) >= oscillation_distance_)
2 {
3 last_oscillation_reset_ = ros::Time::now();
4 oscillation_pose_ = current_position;
5
6 //If the last recovery was caused by oscillation, reset the recovery index
7 if(recovery_trigger_ == OSCILLATION_R)
8 recovery_index_ = 0;
9 }
The map data timeout, that is, the observation sensor data is not new enough, stop the robot and return false, where the publishZeroVelocity() function returns geometry_ Msgs:: CMD of twist type_ Set vel to 0 and publish:
1 if(!controller_costmap_ros_->isCurrent()){
2 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());
3 publishZeroVelocity();
4 return false;
5 }
If a new global path is obtained, it is transferred to the controller to complete the latest_plan_ To controller_plan_ Conversion of (hint: the rules for planning conversion are mentioned in the header file):
1 if(new_global_plan_){
2 //make sure to set the new plan flag to false
3 new_global_plan_ = false;
4
5 ROS_DEBUG_NAMED("move_base","Got a new plan...swap pointers");
6
7 //do a pointer swap under mutex
8 std::vector<geometry_msgs::PoseStamped>* temp_plan = controller_plan_;
9
10 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
11 controller_plan_ = latest_plan_;
12 latest_plan_ = temp_plan;
13 lock.unlock();
14 ROS_DEBUG_NAMED("move_base","pointers swapped!");
15
16 //5. Set the global path for the controller
17 if(!tc_->setPlan(*controller_plan_)){
18 //ABORT and SHUTDOWN COSTMAPS
19 ROS_ERROR("Failed to pass global plan to the controller, aborting.");
20 resetState();
21
22 //disable the planner thread
23 lock.lock();
24 runPlanner_ = false;
25 lock.unlock();
26 //6. Set the action interrupt and return true
27 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to pass global plan to the controller.");
28 return true;
29 }
30
31 //If the global path is valid, recovery is not required
32 if(recovery_trigger_ == PLANNING_R)
33 recovery_index_ = 0;
34 }
Where, tc_ Is the pointer to the local planner, and setPlan is the function of trajectoryplanner ROS (note!!! It has something to do with external packages!!)
Then judge move_ The base state is generally the default state or PLANNING when a valid goal is received. After the global path is planned, the state is_ It will change from PLANNING to CONTROLLING. If PLANNING fails, it will change from PLANNING to cleaning. The architecture is as follows:
1 switch(state_){
2 case PLANNING:
3 case CONTROLLING:
4 case CLEARING:
5 default:
6 }
Among them, the following states are described in detail:
(1) Robot planning status, trying to obtain a global path:
1 case PLANNING:
2 {
3 boost::recursive_mutex::scoped_lock lock(planner_mutex_);
4 runPlanner_ = true;
5 planner_cond_.notify_one();//In PLANNING, wake up the PLANNING thread and let it work
6 }
7 ROS_DEBUG_NAMED("move_base","Waiting for plan, in the planning state.");
8 break;
(2) Attempt to obtain a valid speed control status of the robot:
1 case CONTROLLING:
If you reach the target point, reset the status and set the action successfully, return true, where isgoalrealed() function is the function of trajectoryplanerros (note!!! This is related to external packages!!):
1 if(tc_->isGoalReached()){
2 ROS_DEBUG_NAMED("move_base","Goal reached!");
3 resetState();
4
5 //disable the planner thread
6 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
7 runPlanner_ = false;
8 lock.unlock();
9
10 as_->setSucceeded(move_base_msgs::MoveBaseResult(), "Goal reached.");
11 return true;
12 }
Among them, the resetState() function is as follows. According to the above, the robot moves when it reaches the target point_ Base status set to PLANNING:
1 void MoveBase::resetState(){
2 // Disable the planner thread
3 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
4 runPlanner_ = false;
5 lock.unlock();
6
7 // Reset statemachine
8 state_ = PLANNING;
9 recovery_index_ = 0;
10 recovery_trigger_ = PLANNING_R;
11 publishZeroVelocity();
12
13 //if we shutdown our costmaps when we're deactivated... we'll do that now
14 if(shutdown_costmaps_){
15 ROS_DEBUG_NAMED("move_base","Stopping costmaps");
16 planner_costmap_ros_->stop();
17 controller_costmap_ros_->stop();
18 }
19 }
Go back and continue to look. If the oscillation time is exceeded, stop the robot and set the obstacle removal flag:
1 if(oscillation_timeout_ > 0.0 &&
2 last_oscillation_reset_ + ros::Duration(oscillation_timeout_) < ros::Time::now())
3 {
4 publishZeroVelocity();
5 state_ = CLEARING;
6 recovery_trigger_ = OSCILLATION_R;
7 }
Get the effective speed. If you get it successfully, publish it directly to cmd_vel:
1 if(tc_->computeVelocityCommands(cmd_vel)){
2 ROS_DEBUG_NAMED( "move_base", "Got a valid command from the local planner: %.3lf, %.3lf, %.3lf",
3 cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z );
4 last_valid_control_ = ros::Time::now();
5 //make sure that we send the velocity command to the base
6 vel_pub_.publish(cmd_vel);
7 if(recovery_trigger_ == CONTROLLING_R)
8 recovery_index_ = 0;
9 }
Among them, computeVelocityCommands function is the function of trajectoryplanerros (note!!! This is related to external packages!!).
If the effective speed is not obtained, judge whether the attempt time is exceeded. If it times out, stop the robot and enter the obstacle removal mode:
1 if(ros::Time::now() > attempt_end){
2 //we'll move into our obstacle clearing mode
3 publishZeroVelocity();
4 state_ = CLEARING;
5 recovery_trigger_ = CONTROLLING_R;
6 }
If there is no timeout, plan a new path globally:
1 last_valid_plan_ = ros::Time::now(); 2 planning_retries_ = 0; 3 state_ = PLANNING; 4 publishZeroVelocity(); 5 6 //enable the planner thread in case it isn't running on a clock 7 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_); 8 runPlanner_ = true; 9 planner_cond_.notify_one(); 10 lock.unlock();
(3) The robot is in the obstacle clearing state, planning or control fails, and it recovers or enters the obstacle clearing mode:
1 case CLEARING:
If a restorer is available, perform the recovery action and set the status to PLANNING:
1 if(recovery_behavior_enabled_ && recovery_index_ < recovery_behaviors_.size()){
2 ROS_DEBUG_NAMED("move_base_recovery","Executing behavior %u of %zu", recovery_index_, recovery_behaviors_.size());
3 recovery_behaviors_[recovery_index_]->runBehavior();
4
5 //we at least want to give the robot some time to stop oscillating after executing the behavior
6 last_oscillation_reset_ = ros::Time::now();
7
8 //we'll check if the recovery behavior actually worked
9 ROS_DEBUG_NAMED("move_base_recovery","Going back to planning state");
10 last_valid_plan_ = ros::Time::now();
11 planning_retries_ = 0;
12 state_ = PLANNING;
13
14 //update the index of the next recovery behavior that we'll try
15 recovery_index_++;
16 }
Among them, recovery_behaviors_ The type is STD:: vector < boost:: shared_ ptr<nav_ Core:: recoverybehavior > >, runBehavior() function is move_ slow_ and_ Functions in the clear package. (note!!! Link to external package!!!).
If no restorer is available, end the action and return true:
1 ROS_DEBUG_NAMED("move_base_recovery","All recovery behaviors have failed, locking the planner and disabling it.");
2 //disable the planner thread
3 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
4 runPlanner_ = false;
5 lock.unlock();
6
7 ROS_DEBUG_NAMED("move_base_recovery","Something should abort after this.");
8
9 if(recovery_trigger_ == CONTROLLING_R){
10 ROS_ERROR("Aborting because a valid control could not be found. Even after executing all recovery behaviors");
11 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid control. Even after executing recovery behaviors.");
12 }
13 else if(recovery_trigger_ == PLANNING_R){
14 ROS_ERROR("Aborting because a valid plan could not be found. Even after executing all recovery behaviors");
15 as_->setAborted(move_base_msgs::MoveBaseResult(), "Failed to find a valid plan. Even after executing recovery behaviors.");
16 }
17 else if(recovery_trigger_ == OSCILLATION_R){
18 ROS_ERROR("Aborting because the robot appears to be oscillating over and over. Even after executing all recovery behaviors");
19 as_->setAborted(move_base_msgs::MoveBaseResult(), "Robot is oscillating. Even after executing recovery behaviors.");
20 }
21 resetState();
22 return true;
(4) In addition to the above states:
1 default:
2 ROS_ERROR("This case should never be reached, something is wrong, aborting");
3 resetState();
4 //disable the planner thread
5 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
6 runPlanner_ = false;
7 lock.unlock();
8 as_->setAborted(move_base_msgs::MoveBaseResult(), "Reached a case that should not be hit in move_base. This is a bug, please report it.");
9 return true;
ok, the MoveBase::executeCycle() function is finally introduced. Go back to the callback function and continue——
The rest is to liberate the thread and feed back to the action result:
1 //22. Wake up the scheduling thread so that it can exit cleanly 2 lock.lock(); 3 runPlanner_ = true; 4 planner_cond_.notify_one(); 5 lock.unlock(); 6 7 //23. Terminate and return if the node ends 8 as_->setAborted(move_base_msgs::MoveBaseResult(), "Aborting on the goal because the node has been killed"); 9 return;
ok, after reading the MoveBase::executeCb() function, return to the constructor MoveBase::MoveBase——
The trigger mode (three modes: planning, control and oscillation) is set to "planning":
1 recovery_trigger_ = PLANNING_R;
Load parameters and obtain some parameters from the parameter server, including two planner names, cost map coordinate system, planning frequency, control cycle, etc
1 std::string global_planner, local_planner;
2 private_nh.param("base_global_planner", global_planner, std::string("navfn/NavfnROS"));
3 private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS"));
4 private_nh.param("global_costmap/robot_base_frame", robot_base_frame_, std::string("base_link"));
5 private_nh.param("global_costmap/global_frame", global_frame_, std::string("map"));
6 private_nh.param("planner_frequency", planner_frequency_, 0.0);
7 private_nh.param("controller_frequency", controller_frequency_, 20.0);
8 private_nh.param("planner_patience", planner_patience_, 5.0);
9 private_nh.param("controller_patience", controller_patience_, 15.0);
10 private_nh.param("max_planning_retries", max_planning_retries_, -1); // disabled by default
11 private_nh.param("oscillation_timeout", oscillation_timeout_, 0.0);
12 private_nh.param("oscillation_distance", oscillation_distance_, 0.5);
Save the latest planned path for the three planners (the planner_plan), pass it to the latest plan, and then pass it to the controller plan through the executeCycle) Set memory buffer:
1 planner_plan_ = new std::vector<geometry_msgs::PoseStamped>(); 2 latest_plan_ = new std::vector<geometry_msgs::PoseStamped>(); 3 controller_plan_ = new std::vector<geometry_msgs::PoseStamped>();
Set planner thread, planner_thread_ Is a pointer of type boost::thread *:
1 planner_thread_ = new boost::thread(boost::bind(&MoveBase::planThread, this));
The MoveBase::planThread() function is the entry of the planner thread. This function needs to wait for cbMoveBase::executeCb of actionlib server to wake up and start. The main function is to call the global path planning to obtain the path, ensure the periodicity of the planning and clear the goal when the planning times out, as follows:
1 void MoveBase::planThread(){
2 ROS_DEBUG_NAMED("move_base_plan_thread","Starting planner thread...");
3 ros::NodeHandle n;
4 ros::Timer timer;
5 bool wait_for_wake = false;
6 //1. Create recursive lock
7 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
8 while(n.ok()){
9 //check if we should run the planner (the mutex is locked)
10 //2. Judge whether the thread is blocked
11 while(wait_for_wake || !runPlanner_){
12 //if we should not be running the planner then suspend this thread
13 ROS_DEBUG_NAMED("move_base_plan_thread","Planner thread is suspending");
14 //When STD:: condition_ When a wait function of the variable object is called,
15 //It uses STD:: unique_ Lock (through std::mutex) to lock the current thread.
16 //The current thread will be blocked until another thread is in the same STD:: condition_ The variable object calls the notification function to wake up the current thread.
17 planner_cond_.wait(lock);
18 wait_for_wake = false;
19 }
20 ros::Time start_time = ros::Time::now();
21
22 //time to plan! get a copy of the goal and unlock the mutex
23 geometry_msgs::PoseStamped temp_goal = planner_goal_;
24 lock.unlock();
25 ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");
26
27 //run planner
28
29 //3. Get the global path of the plan
30 //The makePlan function here is to get the pose of the robot as the starting point, then call the makePlan of the global planner to return the planning path, and store it in planner_. plan_
31 planner_plan_->clear();
32 bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
33
34
35 //4. If the plan is obtained, assign it to latest_plan_
36 if(gotPlan){
37 ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
38 //pointer swap the plans under mutex (the controller will pull from latest_plan_)
39 std::vector<geometry_msgs::PoseStamped>* temp_plan = planner_plan_;
40
41 lock.lock();
42 planner_plan_ = latest_plan_;
43 latest_plan_ = temp_plan;
44 last_valid_plan_ = ros::Time::now();
45 planning_retries_ = 0;
46 new_global_plan_ = true;
47
48 ROS_DEBUG_NAMED("move_base_plan_thread","Generated a plan from the base_global_planner");
49
50 //make sure we only start the controller if we still haven't reached the goal
51 if(runPlanner_)
52 state_ = CONTROLLING;
53 if(planner_frequency_ <= 0)
54 runPlanner_ = false;
55 lock.unlock();
56 }
57
58 //5. When certain conditions are met, stop path planning and enter the obstacle removal mode
59 //If the path is not planned and is in the PLANNING state, judge whether the maximum PLANNING cycle or PLANNING times are exceeded
60 //If yes, enter the rotation mode, otherwise it should wait for the wake-up of MoveBase::executeCycle to plan again
61 else if(state_==PLANNING){
62 //It is set to PLANNING only when MoveBase::executeCb and its invoked MoveBase::executeCycle or reset state
63 //Generally, when a new target is just obtained or the path is obtained but the next control cannot be calculated, the path planning shall be carried out again
64 ROS_DEBUG_NAMED("move_base_plan_thread","No Plan...");
65 ros::Time attempt_end = last_valid_plan_ + ros::Duration(planner_patience_);
66
67 //check if we've tried to make a plan for over our time limit or our maximum number of retries
68 //issue #496: we stop planning when one of the conditions is true, but if max_planning_retries_
69 //is negative (the default), it is just ignored and we have the same behavior as ever
70 lock.lock();
71 planning_retries_++;
72 if(runPlanner_ &&
73 (ros::Time::now() > attempt_end || planning_retries_ > uint32_t(max_planning_retries_))){
74 //we'll move into our obstacle clearing mode
75 state_ = CLEARING;
76 runPlanner_ = false; // proper solution for issue #523
77 publishZeroVelocity();
78 recovery_trigger_ = PLANNING_R;
79 }
80
81 lock.unlock();
82 }
83
84 //take the mutex for the next iteration
85 lock.lock();
86
87
88 //6. Set sleep mode
89 //If the planning cycle is not reached, the timer will sleep and pass the planner in the timer interrupt_ cond_ Wake up, the planning cycle here is 0
90 if(planner_frequency_ > 0){
91 ros::Duration sleep_time = (start_time + ros::Duration(1.0/planner_frequency_)) - ros::Time::now();
92 if (sleep_time > ros::Duration(0.0)){
93 wait_for_wake = true;
94 timer = n.createTimer(sleep_time, &MoveBase::wakePlanner, this);
95 }
96 }
97 }
98 }
Among them, planner_cond_ Threads specially used to start path planning are often encountered in other places; In this step, from latest_plan_ To planner_plan_ The leap of; Planer in MoveBase::wakePlanner() function_ cond_ The thread of path planning is turned on.
ok, after watching MoveBase::planThread(), go back to the constructor MoveBase::MoveBase and continue——
Create a publisher. The topic name is cmd_vel, one is cunrrent_goal, one is goal:
1 vel_pub_ = nh.advertise<geometry_msgs::Twist>("cmd_vel", 1);
2 current_goal_pub_ = private_nh.advertise<geometry_msgs::PoseStamped>("current_goal", 0 );
3
4 ros::NodeHandle action_nh("move_base");
5 action_goal_pub_ = action_nh.advertise<move_base_msgs::MoveBaseActionGoal>("goal", 1);
The provided message type is geometry_msgs::PoseStamped's interface for sending goals, for example, cb is MoveBase::goalCB. The target point entered in rviz responds through this function:
1 ros::NodeHandle simple_nh("move_base_simple");
2 goal_sub_ = simple_nh.subscribe<geometry_msgs::PoseStamped>("goal", 1, boost::bind(&MoveBase::goalCB, this, _1));
Among them, let's look at the MoveBase::goalCB() function, which passes in goal. Its main function is to provide an excuse for rviz and others to call geometry_ Msgs:: convert goal in posestamped form to move_base_msgs::MoveBaseActionGoal, and then publish to the corresponding type of goal topic:
1 void MoveBase::goalCB(const geometry_msgs::PoseStamped::ConstPtr& goal){
2 ROS_DEBUG_NAMED("move_base","In ROS goal callback, wrapping the PoseStamped in the action message and re-sending to the server.");
3 move_base_msgs::MoveBaseActionGoal action_goal;
4 action_goal.header.stamp = ros::Time::now();
5 action_goal.goal.target_pose = *goal;
6
7 action_goal_pub_.publish(action_goal);
8 }
ok, after reading the MoveBase::goalCB() function, go back to the constructor MoveBase::MoveBase and continue——
To set the costmap parameter, the trick is to set the expansion layer to be greater than the radius of the robot:
1 private_nh.param("local_costmap/inscribed_radius", inscribed_radius_, 0.325);
2 private_nh.param("local_costmap/circumscribed_radius", circumscribed_radius_, 0.46);
3 private_nh.param("clearing_radius", clearing_radius_, circumscribed_radius_);
4 private_nh.param("conservative_reset_dist", conservative_reset_dist_, 3.0);
5
6 private_nh.param("shutdown_costmaps", shutdown_costmaps_, false);
7 private_nh.param("clearing_rotation_allowed", clearing_rotation_allowed_, true);
8 private_nh.param("recovery_behavior_enabled", recovery_behavior_enabled_, true);
Set global path planner_costmap_ros_ It's costmap_2d::Costmap2DROS * type instantiation pointer, planner_ Is boost:: shared_ ptr<nav_ Core:: baseglobalplanner > type:
1 planner_costmap_ros_ = new costmap_2d::Costmap2DROS("global_costmap", tf_);
2 planner_costmap_ros_->pause();
3 try {
4 planner_ = bgp_loader_.createInstance(global_planner);
5 planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_);
6 } catch (const pluginlib::PluginlibException& ex) {
7 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());
8 exit(1);
9 }
Set local path planner:
1 controller_costmap_ros_ = new costmap_2d::Costmap2DROS("local_costmap", tf_);
2 controller_costmap_ros_->pause();
3 try {
4 tc_ = blp_loader_.createInstance(local_planner);
5 ROS_INFO("Created local_planner %s", local_planner.c_str());
6 tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_costmap_ros_);
7 } catch (const pluginlib::PluginlibException& ex) {
8 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());
9 exit(1);
10 }
At this time, tc_ It becomes an instance object of the local planner.
Start updating costmap:
1 planner_costmap_ros_->start(); 2 controller_costmap_ros_->start();
Global planning:
1 make_plan_srv_ = private_nh.advertiseService("make_plan", &MoveBase::planService, this);
Among them, the MoveBase::planService() function writes the strategy of global planning, and the distance to search the path outward:
1 bool MoveBase::planService(nav_msgs::GetPlan::Request &req, nav_msgs::GetPlan::Response &resp)
Get the starting point. If there is no starting point, get the current global position as the starting point:
1 geometry_msgs::PoseStamped start;
2 //If the starting point is empty, set global_pose is the starting point
3 if(req.start.header.frame_id.empty())
4 {
5 geometry_msgs::PoseStamped global_pose;
6 if(!getRobotPose(global_pose, planner_costmap_ros_)){
7 ROS_ERROR("move_base cannot make a plan for you because it could not get the start pose of the robot");
8 return false;
9 }
10 start = global_pose;
11 }
12 else
13 {
14 start = req.start;
15 }
Develop planning strategy:
1 std::vector<geometry_msgs::PoseStamped> global_plan;
2 if(!planner_->makePlan(start, req.goal, global_plan) || global_plan.empty()){
3 ROS_DEBUG_NAMED("move_base","Failed to find a plan to exact goal of (%.2f, %.2f), searching for a feasible goal within tolerance",
4 req.goal.pose.position.x, req.goal.pose.position.y);
5
6 //Look for feasible goal s outside the specified tolerance range
7 geometry_msgs::PoseStamped p;
8 p = req.goal;
9 bool found_legal = false;
10 float resolution = planner_costmap_ros_->getCostmap()->getResolution();
11 float search_increment = resolution*3.0;//Look outward in 3x resolution increments
12 if(req.tolerance > 0.0 && req.tolerance < search_increment) search_increment = req.tolerance;
13 for(float max_offset = search_increment; max_offset <= req.tolerance && !found_legal; max_offset += search_increment) {
14 for(float y_offset = 0; y_offset <= max_offset && !found_legal; y_offset += search_increment) {
15 for(float x_offset = 0; x_offset <= max_offset && !found_legal; x_offset += search_increment) {
16
17 //Not in the outer layer of this position, not too close
18 if(x_offset < max_offset-1e-9 && y_offset < max_offset-1e-9) continue;
19
20 //Find the exact goal from two directions x and y
21 for(float y_mult = -1.0; y_mult <= 1.0 + 1e-9 && !found_legal; y_mult += 2.0) {
22
23 //In the first traversal, if the offset is too small, remove this point or the previous point
24 if(y_offset < 1e-9 && y_mult < -1.0 + 1e-9) continue;
25
26 for(float x_mult = -1.0; x_mult <= 1.0 + 1e-9 && !found_legal; x_mult += 2.0) {
27 if(x_offset < 1e-9 && x_mult < -1.0 + 1e-9) continue;
28
29 p.pose.position.y = req.goal.pose.position.y + y_offset * y_mult;
30 p.pose.position.x = req.goal.pose.position.x + x_offset * x_mult;
31
32 if(planner_->makePlan(start, p, global_plan)){
33 if(!global_plan.empty()){
34
35 //adding the (unreachable) original goal to the end of the global plan, in case the local planner can get you there
36 //(the reachable goal should have been added by the global planner)
37 global_plan.push_back(req.goal);
38
39 found_legal = true;
40 ROS_DEBUG_NAMED("move_base", "Found a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
41 break;
42 }
43 }
44 else{
45 ROS_DEBUG_NAMED("move_base","Failed to find a plan to point (%.2f, %.2f)", p.pose.position.x, p.pose.position.y);
46 }
47 }
48 }
49 }
50 }
51 }
52 }
Then put the planned global_plan attached to resp and passed out:
1 resp.plan.poses.resize(global_plan.size());
2 for(unsigned int i = 0; i < global_plan.size(); ++i){
3 resp.plan.poses[i] = global_plan[i];
4 }
ok, after reading the MoveBase::planService() function, go back to the constructor MoveBase::MoveBase and continue——
Start clearing map service:
1 clear_costmaps_srv_ = private_nh.advertiseService("clear_costmaps", &MoveBase::clearCostmapsService, this);
Among them, the MoveBase::clearCostmapsService () function is invoked to provide the function of clearing costmap once:
1 bool MoveBase::clearCostmapsService(std_srvs::Empty::Request &req, std_srvs::Empty::Response &resp){
2 //clear the costmaps
3 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_controller(*(controller_costmap_ros_->getCostmap()->getMutex()));
4 controller_costmap_ros_->resetLayers();
5
6 boost::unique_lock<costmap_2d::Costmap2D::mutex_t> lock_planner(*(planner_costmap_ros_->getCostmap()->getMutex()));
7 planner_costmap_ros_->resetLayers();
8 return true;
9 }
It is worth noting that one of the functions resetLayers() calls the costmap package (note!! external links!!!), The function of this function is to reset the map, including resetting the general map and resetting each layer of the map:
1 void Costmap2DROS::resetLayers()
2 {
3 Costmap2D* top = layered_costmap_->getCostmap();
4 top->resetMap(0, 0, top->getSizeInCellsX(), top->getSizeInCellsY());
5 std::vector < boost::shared_ptr<Layer> > *plugins = layered_costmap_->getPlugins();
6 for (vector<boost::shared_ptr<Layer> >::iterator plugin = plugins->begin(); plugin != plugins->end();
7 ++plugin)
8 {
9 (*plugin)->reset();
10 }
11 }
In this function, first reset the total map information to the default value. Then the reset function of each layer is called to reset each layer (Ninth rows). For different reset functions, the functions are as follows:
For the static layer, in the reset function, the onInitialize function will be called to re initialize, but no special operation will be carried out. Only the map update flag bit is set to true, resulting in the update of the boundary (maximum map boundary), resulting in the update of the whole map when updating the map later:
1 void StaticLayer::reset()
2 {
3 if (first_map_only_)
4 {
5 has_updated_data_ = true;
6 }
7 else
8 {
9 onInitialize();
10 }
11 }
For the obstacle layer, in the reset function, the sensor topic will be unsubscribed first, then the map will be reset, and then the sensor topic will be re subscribed, so as to ensure that the whole layer starts again:
1 void ObstacleLayer::reset()
2 {
3 deactivate();
4 resetMaps();
5 current_ = true;
6 activate();
7 }
ok, after reading the MoveBase::clearCostmapsService() function, go back to the constructor MoveBase::MoveBase and continue——
If you accidentally close costmap, deactivate:
1 if(shutdown_costmaps_){
2 ROS_DEBUG_NAMED("move_base","Stopping costmaps initially");
3 planner_costmap_ros_->stop();
4 controller_costmap_ros_->stop();
5 }
Load the specified restorer. If the restorer cannot be loaded, the default restorer will be used. This includes 360 rotation when the road cannot be found:
1 if(!loadRecoveryBehaviors(private_nh)){
2 loadDefaultRecoveryBehaviors();
3 }
After the navigation process is basically completed, initialize the status:
1 //initially, we'll need to make a plan 2 state_ = PLANNING; 3 //we'll start executing recovery behaviors at the beginning of our list 4 recovery_index_ = 0; 5 //10. Start move_base actuator 6 as_->start();
Start dynamic parameter server:
1 dsrv_ = new dynamic_reconfigure::Server<move_base::MoveBaseConfig>(ros::NodeHandle("~"));
2 dynamic_reconfigure::Server<move_base::MoveBaseConfig>::CallbackType cb = boost::bind(&MoveBase::reconfigureCB, this, _1, _2);
3 dsrv_->setCallback(cb);
Among them, the callback function MoveBase::reconfigureCB() is configured with various parameters. Those interested can see the following code:
1 void MoveBase::reconfigureCB(move_base::MoveBaseConfig &config, uint32_t level){
2 boost::recursive_mutex::scoped_lock l(configuration_mutex_);
3
4 //Once called, we need to ensure that there is the original configuration
5 if(!setup_)
6 {
7 last_config_ = config;
8 default_config_ = config;
9 setup_ = true;
10 return;
11 }
12
13 if(config.restore_defaults) {
14 config = default_config_;
15 //If someone sets the default value on the parameter server, prevent looping
16 config.restore_defaults = false;
17 }
18
19 if(planner_frequency_ != config.planner_frequency)
20 {
21 planner_frequency_ = config.planner_frequency;
22 p_freq_change_ = true;
23 }
24
25 if(controller_frequency_ != config.controller_frequency)
26 {
27 controller_frequency_ = config.controller_frequency;
28 c_freq_change_ = true;
29 }
30
31 planner_patience_ = config.planner_patience;
32 controller_patience_ = config.controller_patience;
33 max_planning_retries_ = config.max_planning_retries;
34 conservative_reset_dist_ = config.conservative_reset_dist;
35
36 recovery_behavior_enabled_ = config.recovery_behavior_enabled;
37 clearing_rotation_allowed_ = config.clearing_rotation_allowed;
38 shutdown_costmaps_ = config.shutdown_costmaps;
39
40 oscillation_timeout_ = config.oscillation_timeout;
41 oscillation_distance_ = config.oscillation_distance;
42 if(config.base_global_planner != last_config_.base_global_planner) {
43 boost::shared_ptr<nav_core::BaseGlobalPlanner> old_planner = planner_;
44 //Create global plan
45 ROS_INFO("Loading global planner %s", config.base_global_planner.c_str());
46 try {
47 planner_ = bgp_loader_.createInstance(config.base_global_planner);
48
49 // Wait for the end of the current planning
50 boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
51
52 // clear the old before the new plan starts
53 planner_plan_->clear();
54 latest_plan_->clear();
55 controller_plan_->clear();
56 resetState();
57 planner_->initialize(bgp_loader_.getName(config.base_global_planner), planner_costmap_ros_);
58
59 lock.unlock();
60 } catch (const pluginlib::PluginlibException& ex) {
61 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
62 containing library is built? Exception: %s", config.base_global_planner.c_str(), ex.what());
63 planner_ = old_planner;
64 config.base_global_planner = last_config_.base_global_planner;
65 }
66 }
67
68 if(config.base_local_planner != last_config_.base_local_planner){
69 boost::shared_ptr<nav_core::BaseLocalPlanner> old_planner = tc_;
70 //Create local plan
71 try {
72 tc_ = blp_loader_.createInstance(config.base_local_planner);
73 // Clean up old
74 planner_plan_->clear();
75 latest_plan_->clear();
76 controller_plan_->clear();
77 resetState();
78 tc_->initialize(blp_loader_.getName(config.base_local_planner), &tf_, controller_costmap_ros_);
79 } catch (const pluginlib::PluginlibException& ex) {
80 ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the \
81 containing library is built? Exception: %s", config.base_local_planner.c_str(), ex.what());
82 tc_ = old_planner;
83 config.base_local_planner = last_config_.base_local_planner;
84 }
85 }
86
87 last_config_ = config;
88 }
ok, the constructor MoveBase::MoveBase is finished. Then it goes to the destructor to free up memory——
1 MoveBase::~MoveBase(){
2 recovery_behaviors_.clear();
3
4 delete dsrv_;
5
6 if(as_ != NULL)
7 delete as_;
8
9 if(planner_costmap_ros_ != NULL)
10 delete planner_costmap_ros_;
11
12 if(controller_costmap_ros_ != NULL)
13 delete controller_costmap_ros_;
14
15 planner_thread_->interrupt();
16 planner_thread_->join();
17
18 delete planner_thread_;
19
20 delete planner_plan_;
21 delete latest_plan_;
22 delete controller_plan_;
23
24 planner_.reset();
25 tc_.reset();
26 }
So far, move_ The base has been parsed. Congratulations on your patience. If there are mistakes, you are welcome to point them out. Thank you here!
5, Points needing attention
1,move_base and costmap_2d, global planner, local planner and restorer are closely connected, and the functions of these packages are called in many places.
2. We need to focus on those enumeration constants written in the header file and various flag bits, many of which are judgment conditions in the body.
3. Multithreading and smart pointers need to be studied in depth, which is very helpful for memory and task management.