add onOriginChanged logic

costmap_2d/include/costmap_2d/layer.h

@@ -111,6 +111,14 @@ class Layer
    * notified of changes to the robot's footprint. */
   virtual void onFootprintChanged() {}
 
+  /**
+   * @brief Called from LayeredCostmap::updateOrigin
+   *
+   * Overwrite this method in order to implement custom reactions to a changed
+   * origin of the LayeredCostmap.
+   */
+  virtual void onOriginChanged() {}
+
 protected:
   /** @brief This is called at the end of initialize().  Override to
    * implement subclass-specific initialization.

costmap_2d/include/costmap_2d/layered_costmap.h

@@ -154,6 +154,20 @@ class LayeredCostmap
    * This is updated by setFootprint(). */
   double getInscribedRadius() { return inscribed_radius_; }
 
+  /**
+   * @brief updates the origin of this class and notifies all plugins
+   *
+   * The function will update the own origin and then notify every registered
+   * plugin by calling Layer::onOriginChanged.
+   *
+   * Note: The function is a no-op, if the passed origin is equal to the current
+   * origin
+   *
+   * @param x new x value
+   * @param y new y value
+   */
+  void updateOrigin(double x, double y);
+
 private:
   Costmap2D costmap_;
   std::string global_frame_;

costmap_2d/include/costmap_2d/obstacle_layer.h

@@ -76,6 +76,7 @@ class ObstacleLayer : public CostmapLayer
   virtual void activate();
   virtual void deactivate();
   virtual void reset();
+  virtual void onOriginChanged();
 
   /**
    * @brief  A callback to handle buffering LaserScan messages
@@ -146,7 +147,7 @@ class ObstacleLayer : public CostmapLayer
 
   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, 
+  void updateFootprint(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                        double* max_x, double* max_y);
 
   std::string global_frame_;  ///< @brief The global frame for the costmap
@@ -163,7 +164,6 @@ class ObstacleLayer : public CostmapLayer
   // Used only for testing purposes
   std::vector<costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
 
-  bool rolling_window_;
   dynamic_reconfigure::Server<costmap_2d::ObstaclePluginConfig> *dsrv_;
 
   int combination_method_;

costmap_2d/include/costmap_2d/static_layer.h

@@ -66,6 +66,8 @@ class StaticLayer : public CostmapLayer
 
   virtual void matchSize();
 
+  virtual void onOriginChanged();
+
 private:
   /**
    * @brief  Callback to update the costmap's map from the map_server

costmap_2d/plugins/obstacle_layer.cpp

@@ -57,7 +57,6 @@ namespace costmap_2d
 void ObstacleLayer::onInitialize()
 {
   ros::NodeHandle nh("~/" + name_), g_nh;
-  rolling_window_ = layered_costmap_->isRolling();
 
   bool track_unknown_space;
   nh.param("track_unknown_space", track_unknown_space, layered_costmap_->isTrackingUnknown());
@@ -330,11 +329,15 @@ void ObstacleLayer::pointCloud2Callback(const sensor_msgs::PointCloud2ConstPtr&
   buffer->unlock();
 }
 
+void ObstacleLayer::onOriginChanged()
+{
+  const Costmap2D* master = layered_costmap_->getCostmap();
+  updateOrigin(master->getOriginX(), master->getOriginY());
+}
+
 void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                           double* min_y, double* max_x, double* max_y)
 {
-  if (rolling_window_)
-    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
   if (!enabled_)
     return;
   useExtraBounds(min_x, min_y, max_x, max_y);

costmap_2d/plugins/static_layer.cpp

@@ -267,6 +267,12 @@ void StaticLayer::reset()
   }
 }
 
+void StaticLayer::onOriginChanged()
+{
+  // we need to repaint everything
+  has_updated_data_ = true;
+}
+
 void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
                                double* max_x, double* max_y)
 {

costmap_2d/plugins/voxel_layer.cpp

@@ -116,8 +116,6 @@ void VoxelLayer::resetMaps()
 void VoxelLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x,
                                        double* min_y, double* max_x, double* max_y)
 {
-  if (rolling_window_)
-    updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
   if (!enabled_)
     return;
   useExtraBounds(min_x, min_y, max_x, max_y);

costmap_2d/src/layered_costmap.cpp

@@ -103,7 +103,7 @@ void LayeredCostmap::updateMap(double robot_x, double robot_y, double robot_yaw)
   {
     double new_origin_x = robot_x - costmap_.getSizeInMetersX() / 2;
     double new_origin_y = robot_y - costmap_.getSizeInMetersY() / 2;
-    costmap_.updateOrigin(new_origin_x, new_origin_y);
+    updateOrigin(new_origin_x, new_origin_y);
   }
 
   if (plugins_.size() == 0)
@@ -182,4 +182,21 @@ void LayeredCostmap::setFootprint(const std::vector<geometry_msgs::Point>& footp
   }
 }
 
+void LayeredCostmap::updateOrigin(double x, double y) {
+  // if the map's origin has not changed, skip
+  if(x == costmap_.getOriginX() && y == costmap_.getOriginY())
+    return;
+
+  // set the origin in the master layer
+  costmap_.updateOrigin(x, y);
+
+  // notify all plugins
+  typedef vector<boost::shared_ptr<Layer> >::iterator iterator;
+
+  for (iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin)
+  {
+    (*plugin)->onOriginChanged();
+  }
+}
+
 }  // namespace costmap_2d