proper reset

costmap_2d/include/costmap_2d/obstacle_layer.h

@@ -180,8 +180,19 @@ class ObstacleLayer : public CostmapLayer
                             double* max_x, double* max_y);
 
   void updateMapPolygon();
+
+  /**
+    * @brief  Update the cell timeout for a given index. Cells with a timeout are marked as NO_INFORMATION
+    * @param index The index of the cell to update
+   */
   void updateCellTimeout(int index);
 
+  /**
+    * @brief Setup vector with last updated time for each cell
+    * @param new_cell_clearing_timeout The new cell clearing timeout
+   */
+  void setupCellsTimeout(const double new_cell_clearing_timeout);
+
   std::vector<geometry_msgs::Point> transformed_footprint_;
   bool footprint_clearing_enabled_;
   void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
@@ -208,7 +219,7 @@ class ObstacleLayer : public CostmapLayer
   bool raytrace_outside_map_;
   Polygon map_boundary_;
 
-  double cell_clearing_timeout_;
+  double cell_clearing_timeout_ = -1;
   std::vector<ros::Time> last_updated_;
 
 private:

costmap_2d/plugins/obstacle_layer.cpp

@@ -241,18 +241,7 @@ void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint
   max_obstacle_height_ = config.max_obstacle_height;
   combination_method_ = config.combination_method;
   raytrace_outside_map_ = config.raytrace_outside_map;
-
-  // create new cell update tracking if timeout parameter has changed
-  if (config.cell_clearing_timeout < 0 || rolling_window_)
-  {
-    last_updated_.clear();
-  }
-  else if (config.cell_clearing_timeout != cell_clearing_timeout_)
-  {
-    last_updated_.clear();
-    last_updated_.resize(size_x_ * size_y_, ros::Time::now());
-  }
-  cell_clearing_timeout_ = config.cell_clearing_timeout;
+  setupCellsTimeout(config.cell_clearing_timeout);
 }
 
 void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
@@ -364,6 +353,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
     updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
   useExtraBounds(min_x, min_y, max_x, max_y);
   updateMapPolygon();
+  setupCellsTimeout(cell_clearing_timeout_);
 
   bool current = true;
   std::vector<Observation> observations, clearing_observations;
@@ -760,6 +750,44 @@ void ObstacleLayer::reset()
   activate();
 }
 
+void ObstacleLayer::setupCellsTimeout(const double new_cell_clearing_timeout)
+{
+  // if we're using rolling window, we don't clear cells based on timeout
+  if (rolling_window_)
+  {
+    return;
+  }
+
+  // new parameter is negative, clear all cells (disable timeout)
+  if (new_cell_clearing_timeout < 0)
+  {
+    last_updated_.clear();
+    return;
+  }
+
+  // check if map's size matches vector; otherwise, resize vector
+  // we need to check this first, because width and height can change in costmap_2d_ros
+  if (last_updated_.size() != size_x_ * size_y_)
+  {
+    last_updated_.clear();
+    last_updated_.resize(size_x_ * size_y_, ros::Time::now());
+    cell_clearing_timeout_ = new_cell_clearing_timeout;
+    return;
+  }
+
+  // if parameter did not change, return
+  if (new_cell_clearing_timeout == cell_clearing_timeout_)
+  {
+    return;
+  }
+
+  // parameter has changed, create new cell update tracking vector
+  last_updated_.clear();
+  last_updated_.resize(size_x_ * size_y_, ros::Time::now());
+  cell_clearing_timeout_ = new_cell_clearing_timeout;
+}
+
+
 void ObstacleLayer::updateCellTimeout(int index)
 {
   if (!rolling_window_ && !last_updated_.empty() && index < last_updated_.size())