Add support for dynamically changing keepout zone (#5429)

* Add support for dynamically changing keepout zone

Signed-off-by: mini-1235 <[email protected]>

* Linting

Signed-off-by: mini-1235 <[email protected]>

* Revert binary and speed changes

Signed-off-by: mini-1235 <[email protected]>

---------

Signed-off-by: mini-1235 <[email protected]>

Author: mini-1235

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

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

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(