diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp index e51809df696..25238cf8a35 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/costmap_filter.hpp @@ -99,7 +99,7 @@ class CostmapFilter : public Layer */ void updateBounds( double robot_x, double robot_y, double robot_yaw, - double * min_x, double * min_y, double * max_x, double * max_y) final; + double * min_x, double * min_y, double * max_x, double * max_y) override; /** * @brief Update the costs in the master costmap in the window diff --git a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp index e615ba465a0..b23e03a5e49 100644 --- a/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp +++ b/nav2_costmap_2d/include/nav2_costmap_2d/costmap_filters/keepout_filter.hpp @@ -68,6 +68,20 @@ class KeepoutFilter : public CostmapFilter void initializeFilter( const std::string & filter_info_topic); + /** + * @brief Update the bounds of the master costmap by this layer's update dimensions + * @param robot_x X pose of robot + * @param robot_y Y pose of robot + * @param robot_yaw Robot orientation + * @param min_x X min map coord of the window to update + * @param min_y Y min map coord of the window to update + * @param max_x X max map coord of the window to update + * @param max_y Y max map coord of the window to update + */ + void updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, double * max_x, double * max_y) override; + /** * @brief Process the keepout layer at the current pose / bounds / grid */ @@ -108,6 +122,12 @@ class KeepoutFilter : public CostmapFilter bool last_pose_lethal_{false}; // If true, last pose was lethal unsigned int lethal_state_update_min_x_{999999u}, lethal_state_update_min_y_{999999u}; unsigned int lethal_state_update_max_x_{0u}, lethal_state_update_max_y_{0u}; + + unsigned int x_{0}; + unsigned int y_{0}; + unsigned int width_{0}; + unsigned int height_{0}; + bool has_updated_data_{false}; }; } // namespace nav2_costmap_2d diff --git a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp index e727d00634e..5fa544dec1c 100644 --- a/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp +++ b/nav2_costmap_2d/plugins/costmap_filters/keepout_filter.cpp @@ -155,6 +155,37 @@ void KeepoutFilter::maskCallback( // Store filter_mask_ filter_mask_ = msg; + has_updated_data_ = true; + x_ = y_ = 0; + width_ = msg->info.width; + height_ = msg->info.height; +} + +void KeepoutFilter::updateBounds( + double robot_x, double robot_y, double robot_yaw, + double * min_x, double * min_y, double * max_x, double * max_y) +{ + if (!enabled_) { + return; + } + + CostmapFilter::updateBounds(robot_x, robot_y, robot_yaw, min_x, min_y, max_x, max_y); + + if(!has_updated_data_) { + return; + } + + double wx, wy; + + layered_costmap_->getCostmap()->mapToWorld(x_, y_, wx, wy); + *min_x = std::min(wx, *min_x); + *min_y = std::min(wy, *min_y); + + layered_costmap_->getCostmap()->mapToWorld(x_ + width_, y_ + height_, wx, wy); + *max_x = std::max(wx, *max_x); + *max_y = std::max(wy, *max_y); + + has_updated_data_ = false; } void KeepoutFilter::process(