1726_24102025

plugins/critical_layer.cpp

@@ -0,0 +1,38 @@
+#include <costmap_2d/critical_layer.h>
+#include <boost/dll/alias.hpp>
+#include "shared.h"
+
+PLUGINLIB_EXPORT_CLASS(costmap_2d::CriticalLayer, costmap_2d::Layer)
+
+using costmap_2d::NO_INFORMATION;
+using costmap_2d::LETHAL_OBSTACLE;
+using costmap_2d::CRITICAL_SPACE;
+
+namespace costmap_2d
+{
+
+CriticalLayer::CriticalLayer(){}
+CriticalLayer::~CriticalLayer(){}
+
+unsigned char CriticalLayer::interpretValue(unsigned char value) 
+{
+  // check if the static value is above the unknown or lethal thresholds
+  if(value >= *this->threshold_)
+    return CRITICAL_SPACE;
+  else 
+    return NO_INFORMATION;
+}
+
+void CriticalLayer::updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
+{
+  if (!map_received_)
+    return;
+    
+    // if not rolling, the layered costmap (master_grid) has same coordinates as this layer
+    if (!use_maximum_)
+      updateWithOverwrite(master_grid, min_i, min_j, max_i, max_j); 
+    else
+      updateWithMax(master_grid, min_i, min_j, max_i, max_j);
+    
+}
+}