From 53f879dc7de501b6693fc4346ffeefac9ccf4cf5 Mon Sep 17 00:00:00 2001
From: shiningjuly <33364990+shiningjuly@users.noreply.github.com>
Date: Sat, 21 Dec 2019 20:49:17 +0800
Subject: [PATCH 1/2] Update move_base.h

---
 move_base/include/move_base/move_base.h | 1 +
 1 file changed, 1 insertion(+)

diff --git a/move_base/include/move_base/move_base.h b/move_base/include/move_base/move_base.h
index 1045d77ee1..eb3d7fa703 100644
--- a/move_base/include/move_base/move_base.h
+++ b/move_base/include/move_base/move_base.h
@@ -231,6 +231,7 @@ namespace move_base {
       move_base::MoveBaseConfig default_config_;
       bool setup_, p_freq_change_, c_freq_change_;
       bool new_global_plan_;
+      bool planner_goal_updated_;
   };
 };
 #endif

From 48d4112b0ce37c0b252f39bf54174a7b00a0f076 Mon Sep 17 00:00:00 2001
From: shiningjuly <33364990+shiningjuly@users.noreply.github.com>
Date: Sat, 21 Dec 2019 20:56:23 +0800
Subject: [PATCH 2/2] Update move_base.cpp

---
 move_base/src/move_base.cpp | 18 ++++++++++++++++--
 1 file changed, 16 insertions(+), 2 deletions(-)

diff --git a/move_base/src/move_base.cpp b/move_base/src/move_base.cpp
index 299870a6ed..6f3f3ce8d1 100644
--- a/move_base/src/move_base.cpp
+++ b/move_base/src/move_base.cpp
@@ -55,7 +55,7 @@ namespace move_base {
     blp_loader_("nav_core", "nav_core::BaseLocalPlanner"), 
     recovery_loader_("nav_core", "nav_core::RecoveryBehavior"),
     planner_plan_(NULL), latest_plan_(NULL), controller_plan_(NULL),
-    runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false) {
+    runPlanner_(false), setup_(false), p_freq_change_(false), c_freq_change_(false), new_global_plan_(false), planner_goal_updated_(false) {
 
     as_ = new MoveBaseActionServer(ros::NodeHandle(), "move_base", boost::bind(&MoveBase::executeCb, this, _1), false);
 
@@ -571,13 +571,24 @@ namespace move_base {
 
       //time to plan! get a copy of the goal and unlock the mutex
       geometry_msgs::PoseStamped temp_goal = planner_goal_;
+      planner_goal_updated_ = false;
       lock.unlock();
       ROS_DEBUG_NAMED("move_base_plan_thread","Planning...");
 
       //run planner
       planner_plan_->clear();
       bool gotPlan = n.ok() && makePlan(temp_goal, *planner_plan_);
-
+      
+      //if new goal available, abandon current plan and go back to makePlan for new goal
+      if(planner_goal_updated_)
+      {
+        if(gotPlan) // still update last_valid_plan_ in case too many goals preempted
+          last_valid_plan_ = ros::Time::now();
+        //take the mutex for the next iteration
+        lock.lock();
+        continue;
+      }
+      
       if(gotPlan){
         ROS_DEBUG_NAMED("move_base_plan_thread","Got Plan with %zu points!", planner_plan_->size());
         //pointer swap the plans under mutex (the controller will pull from latest_plan_)
@@ -649,6 +660,7 @@ namespace move_base {
     boost::unique_lock<boost::recursive_mutex> lock(planner_mutex_);
     planner_goal_ = goal;
     runPlanner_ = true;
+    planner_goal_updated_ = true;
     planner_cond_.notify_one();
     lock.unlock();
 
@@ -698,6 +710,7 @@ namespace move_base {
           lock.lock();
           planner_goal_ = goal;
           runPlanner_ = true;
+          planner_goal_updated_ = true;
           planner_cond_.notify_one();
           lock.unlock();
 
@@ -736,6 +749,7 @@ namespace move_base {
         lock.lock();
         planner_goal_ = goal;
         runPlanner_ = true;
+        planner_goal_updated_ = true;
         planner_cond_.notify_one();
         lock.unlock();