Fix deadlock when starting map with map-update-frequency set to zero (#1072)

dorezyuk · Dima Dorezyuk · web-flow · commit 39524733cafd · 2020-12-31T08:41:36.000-05:00
Co-authored-by: Dima Dorezyuk &lt;dorezyuk@magazino.eu&gt;
diff --git a/costmap_2d/src/costmap_2d_ros.cpp b/costmap_2d/src/costmap_2d_ros.cpp
@@ -326,6 +326,7 @@ void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t l
     map_update_thread_shutdown_ = true;
     map_update_thread_->join();
     delete map_update_thread_;
+    map_update_thread_ = NULL;
   }
   map_update_thread_shutdown_ = false;
   double map_update_frequency = config.update_frequency;
@@ -359,7 +360,9 @@ void Costmap2DROS::reconfigureCB(costmap_2d::Costmap2DConfig &config, uint32_t l
 
   old_config_ = config;
 
-  map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
+  // only construct the thread if the frequency is positive
+  if(map_update_frequency > 0.0)
+    map_update_thread_ = new boost::thread(boost::bind(&Costmap2DROS::mapUpdateLoop, this, map_update_frequency));
 }
 
 void Costmap2DROS::readFootprintFromConfig(const costmap_2d::Costmap2DConfig &new_config,
@@ -435,10 +438,6 @@ void Costmap2DROS::movementCB(const ros::TimerEvent &event)
 
 void Costmap2DROS::mapUpdateLoop(double frequency)
 {
-  // the user might not want to run the loop every cycle
-  if (frequency == 0.0)
-    return;
-
   ros::NodeHandle nh;
   ros::Rate r(frequency);
   while (nh.ok() && !map_update_thread_shutdown_)
@@ -522,8 +521,9 @@ void Costmap2DROS::start()
   stop_updates_ = false;
 
   // block until the costmap is re-initialized.. meaning one update cycle has run
+  // note: this does not hold, if the user has disabled map-updates allgother
   ros::Rate r(100.0);
-  while (ros::ok() && !initialized_)
+  while (ros::ok() && !initialized_ && map_update_thread_)
     r.sleep();
 }
 
