Add mask combination method (#96)

costmap_2d/include/costmap_2d/costmap_layer.h

@@ -115,6 +115,29 @@ class CostmapLayer : public Layer, public Costmap2D
    */
   void updateWithAddition(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
+  /*
+   * Updates the master_grid within the specified
+   * bounding box using this layer's values.
+   *
+   * Sets the new value to FREE if the layer's value is occupied.
+   * If the master value is NO_INFORMATION, keep it.
+   * If the layer's value is NO_INFORMATION,
+   * then the master value does not change.
+   *
+   */
+  void updateWithMask(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
+
+  /*
+   * Updates the master_grid with layer index
+   *
+   * Sets the new value to FREE if the layer's value is occupied.
+   * If the master value is NO_INFORMATION, keep it.
+   * If the layer's value is NO_INFORMATION,
+   * then the master value does not change.
+   *
+   */
+  void mask(costmap_2d::Costmap2D& master_grid, unsigned int master_grid_index, unsigned int costmap_index);
+
   /**
    * Updates the bounding box specified in the parameters to include
    * the location (x,y)

costmap_2d/include/costmap_2d/static_layer.h

@@ -46,10 +46,18 @@
 #include <nav_msgs/OccupancyGrid.h>
 #include <map_msgs/OccupancyGridUpdate.h>
 #include <message_filters/subscriber.h>
+#include <costmap_2d/GenericPluginConfig.h>
 
 namespace costmap_2d
 {
 
+enum class CombinationMethod
+{
+  OVERWRITE = 0,
+  MAXIMUM = 1,
+  MASK = 2
+};
+
 class StaticLayer : public CostmapLayer
 {
 public:
@@ -86,12 +94,12 @@ class StaticLayer : public CostmapLayer
   bool has_updated_data_;
   unsigned int x_, y_, width_, height_;
   bool track_unknown_space_;
-  bool use_maximum_;
   bool first_map_only_;      ///< @brief Store the first static map and reuse it on reinitializing
   bool trinary_costmap_;
   ros::Subscriber map_sub_, map_update_sub_;
 
   unsigned char lethal_threshold_, unknown_cost_value_;
+  CombinationMethod combination_method_;
 
   dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig> *dsrv_;
 };

costmap_2d/plugins/static_layer.cpp

@@ -72,13 +72,16 @@ void StaticLayer::onInitialize()
   nh.param("subscribe_to_updates", subscribe_to_updates_, false);
 
   nh.param("track_unknown_space", track_unknown_space_, true);
-  nh.param("use_maximum", use_maximum_, false);
 
   int temp_lethal_threshold, temp_unknown_cost_value;
   nh.param("lethal_cost_threshold", temp_lethal_threshold, int(100));
   nh.param("unknown_cost_value", temp_unknown_cost_value, int(-1));
   nh.param("trinary_costmap", trinary_costmap_, true);
 
+  int combination_method_int;
+  nh.param("combination_method", combination_method_int, 0);
+  combination_method_ = static_cast<CombinationMethod>(combination_method_int);
+
   lethal_threshold_ = std::max(std::min(temp_lethal_threshold, 100), 0);
   unknown_cost_value_ = temp_unknown_cost_value;
 
@@ -104,7 +107,6 @@ void StaticLayer::onInitialize()
     {
       ROS_INFO("Subscribing to updates");
       map_update_sub_ = g_nh.subscribe(map_topic + "_updates", 10, &StaticLayer::incomingUpdate, this);
-
     }
   }
   else
@@ -118,12 +120,12 @@ void StaticLayer::onInitialize()
   }
 
   dsrv_ = new dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>(nh);
-  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb = boost::bind(
-      &StaticLayer::reconfigureCB, this, _1, _2);
+  dynamic_reconfigure::Server<costmap_2d::GenericPluginConfig>::CallbackType cb =
+      boost::bind(&StaticLayer::reconfigureCB, this, _1, _2);
   dsrv_->setCallback(cb);
 }
 
-void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig &config, uint32_t level)
+void StaticLayer::reconfigureCB(costmap_2d::GenericPluginConfig& config, uint32_t level)
 {
   if (config.enabled != enabled_)
   {
@@ -314,10 +316,12 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
   if (!layered_costmap_->isRolling())
   {
     // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
-    if (!use_maximum_)
+    if (combination_method_ == CombinationMethod::OVERWRITE)
       updateWithTrueOverwrite(master_grid, min_i, min_j, max_i, max_j);
-    else
+    else if (combination_method_ == CombinationMethod::MAXIMUM)
       updateWithMax(master_grid, min_i, min_j, max_i, max_j);
+    else
+      updateWithMask(master_grid, min_i, min_j, max_i, max_j);
   }
   else
   {
@@ -346,12 +350,14 @@ void StaticLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int
         layered_costmap_->getCostmap()->mapToWorld(i, j, wx, wy);
         // Transform from global_frame_ to map_frame_
         tf2::Vector3 p(wx, wy, 0);
-        p = tf2_transform*p;
+        p = tf2_transform * p;
         // Set master_grid with cell from map
         if (worldToMap(p.x(), p.y(), mx, my))
         {
-          if (!use_maximum_)
+          if (combination_method_ == CombinationMethod::OVERWRITE)
             master_grid.setCost(i, j, getCost(mx, my));
+          else if (combination_method_ == CombinationMethod::MASK)
+            mask(master_grid, master_grid.getIndex(i, j), getIndex(mx, my));
           else
             master_grid.setCost(i, j, std::max(getCost(mx, my), master_grid.getCost(i, j)));
         }

costmap_2d/src/costmap_layer.cpp

@@ -156,4 +156,33 @@ void CostmapLayer::updateWithAddition(costmap_2d::Costmap2D& master_grid, int mi
     }
   }
 }
+
+void CostmapLayer::updateWithMask(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (!enabled_)
+    return;
+  unsigned char* master_array = master_grid.getCharMap();
+  unsigned int span = master_grid.getSizeInCellsX();
+
+  for (int j = min_j; j < max_j; j++)
+  {
+    unsigned int it = j * span + min_i;
+    for (int i = min_i; i < max_i; i++)
+    {
+      mask(master_grid, it, it);
+      it++;
+    }
+  }
+}
+
+void CostmapLayer::mask(costmap_2d::Costmap2D& master_grid, unsigned int master_grid_index, unsigned int costmap_index)
+{
+  unsigned char* master_array = master_grid.getCharMap();
+  if (costmap_[costmap_index] == costmap_2d::LETHAL_OBSTACLE)
+  {
+    master_array[master_grid_index] = costmap_2d::FREE_SPACE;
+    return;
+  }
+}
+
 }  // namespace costmap_2d