From a107e1ff8f5872cf4cd30de6649b0534a785fb5b Mon Sep 17 00:00:00 2001 From: Dima Dorezyuk Date: Sun, 11 Oct 2020 16:24:49 +0200 Subject: [PATCH 1/2] add onOriginChanged logic --- costmap_2d/include/costmap_2d/layer.h | 8 ++++++++ .../include/costmap_2d/layered_costmap.h | 14 ++++++++++++++ .../include/costmap_2d/obstacle_layer.h | 4 ++-- costmap_2d/include/costmap_2d/static_layer.h | 2 ++ costmap_2d/plugins/obstacle_layer.cpp | 9 ++++++--- costmap_2d/plugins/static_layer.cpp | 11 +++++++++++ costmap_2d/plugins/voxel_layer.cpp | 2 -- costmap_2d/src/layered_costmap.cpp | 19 ++++++++++++++++++- 8 files changed, 61 insertions(+), 8 deletions(-) diff --git a/costmap_2d/include/costmap_2d/layer.h b/costmap_2d/include/costmap_2d/layer.h index 7b4d7655e5..4f0d908bd3 100644 --- a/costmap_2d/include/costmap_2d/layer.h +++ b/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. diff --git a/costmap_2d/include/costmap_2d/layered_costmap.h b/costmap_2d/include/costmap_2d/layered_costmap.h index 495d9c3002..71bdfe8566 100644 --- a/costmap_2d/include/costmap_2d/layered_costmap.h +++ b/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_; diff --git a/costmap_2d/include/costmap_2d/obstacle_layer.h b/costmap_2d/include/costmap_2d/obstacle_layer.h index ea5a8db784..5f023b8ab2 100644 --- a/costmap_2d/include/costmap_2d/obstacle_layer.h +++ b/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 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 static_clearing_observations_, static_marking_observations_; - bool rolling_window_; dynamic_reconfigure::Server *dsrv_; int combination_method_; diff --git a/costmap_2d/include/costmap_2d/static_layer.h b/costmap_2d/include/costmap_2d/static_layer.h index 3fe9a76464..cc7412e9ff 100644 --- a/costmap_2d/include/costmap_2d/static_layer.h +++ b/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 diff --git a/costmap_2d/plugins/obstacle_layer.cpp b/costmap_2d/plugins/obstacle_layer.cpp index 6386fcaf06..c49c930568 100644 --- a/costmap_2d/plugins/obstacle_layer.cpp +++ b/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); diff --git a/costmap_2d/plugins/static_layer.cpp b/costmap_2d/plugins/static_layer.cpp index 3787de47c6..356d0df9b8 100644 --- a/costmap_2d/plugins/static_layer.cpp +++ b/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) { @@ -276,6 +282,11 @@ void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, return; } + // if the origin has not changed, skip the re-painting, since we cannot add + // any new information + if(!has_updated_data_) + return; + useExtraBounds(min_x, min_y, max_x, max_y); double wx, wy; diff --git a/costmap_2d/plugins/voxel_layer.cpp b/costmap_2d/plugins/voxel_layer.cpp index 3df9f0e6ef..a84c6159ff 100644 --- a/costmap_2d/plugins/voxel_layer.cpp +++ b/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); diff --git a/costmap_2d/src/layered_costmap.cpp b/costmap_2d/src/layered_costmap.cpp index 2ca96b8245..3da1fb0980 100644 --- a/costmap_2d/src/layered_costmap.cpp +++ b/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& 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 >::iterator iterator; + + for (iterator plugin = plugins_.begin(); plugin != plugins_.end(); ++plugin) + { + (*plugin)->onOriginChanged(); + } +} + } // namespace costmap_2d From 37cd3c1b80025287c22580c5f3b2d6b43b23c250 Mon Sep 17 00:00:00 2001 From: Dima Dorezyuk Date: Sun, 17 Jan 2021 10:28:52 +0100 Subject: [PATCH 2/2] drop return-case --- costmap_2d/plugins/static_layer.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/costmap_2d/plugins/static_layer.cpp b/costmap_2d/plugins/static_layer.cpp index 356d0df9b8..c4431646e1 100644 --- a/costmap_2d/plugins/static_layer.cpp +++ b/costmap_2d/plugins/static_layer.cpp @@ -282,11 +282,6 @@ void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, return; } - // if the origin has not changed, skip the re-painting, since we cannot add - // any new information - if(!has_updated_data_) - return; - useExtraBounds(min_x, min_y, max_x, max_y); double wx, wy;