When working with large maps and filter masks (100m x 150m), filter mask receiving time takes a long time ~30ms for KeepoutFilter, while for other filters (SpeedFilter and BinaryFilter), it is imperceptibly small.

This appears because in the mask callback, KeepoutFilter is creating a new Costmap2D object from whole filter mask:

  // Making a new mask_costmap_
  mask_costmap_ = std::make_unique<Costmap2D>(*msg);

The constructor converts by-item each OccupancyGrid cell to Costmap2D cell, which takes long time for large masks.

Instead of it, we could just store the pointer to the OccupancyGrid message in the mask callback, and then operate directly with OccupancyGrid cells inside dynamic window when it is necessary.

Preliminary experiments show the mask receiving time will drop from 30ms down to 0.12ms, while KeepoutFilter::process() time will be increased from 0.13ms to 0.28ms which is insignificant, as follows from the table below:

So, at the expense of small map processing time increase, we may hugely optimize mask receiving callback.
It is very notable for the systems with large maps and filter masks, especially when filter masks to be updated dynamically with moving objects (allowing to use there high update rates).

Feature request

Required Info:

• Operating System:
  • Ubuntu 20.04, 22.04
• ROS2 Version:
  • ROS2 rolling built from sources (source date: 2023.01.10)
• Version or commit hash:
  • 4aaefad
• DDS implementation:
  • Cyclone DDS

Steps to reproduce measurements

• Prepare 150x100m OccupancyGrid map (3000x2000 cells with resultion = 0.05)
• Enable KeepoutFilter in the local costmap and publish this map as input mask for the filter
• Run standard TB3 simulation with enabled KeepoutFilter
• Measure KeepoutFilter::maskCallback() and KeepoutFilter::process() times, e.g. by adding auto t1 = std::chrono::steady_clock::now(); time labels in the beginning and end of each routine.

Implementation considerations

• Do not copy OccupancyGrid mask -> to Costmap2D. Directly store and use OccupancyGrid mask instead:

-  std::unique_ptr<Costmap2D> mask_costmap_;
+  nav_msgs::msg::OccupancyGrid::SharedPtr filter_mask_;

• Add new unsigned char CostmapFilter::getMaskCost(OccupancyGrid::SharedPtr filter_mask, & mx, & my) routine to CostmapFilter, which will convert OccupancyGrid values to Costmap2D ones; and will be called each time, when we need to update master costmap cell in the rolling window during KeepoutFilter::process()
• Other code to be changed to use OccupancyGrid instead of Costmap

The performance improving patch development is in progress; PR to be submitted shortly