update from DuongTD

include/robot_costmap_2d/critical_layer.h

@@ -10,6 +10,12 @@ class CriticalLayer : public StaticLayer
 public:
   CriticalLayer();
   virtual ~CriticalLayer();
+
+  LayerType getType() const override
+  {
+      return LayerType::CRITICAL_LAYER;
+  }
+  
 private:
   unsigned char interpretValue(unsigned char value) override;
   void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j) override;

include/robot_costmap_2d/directional_layer.h

@@ -14,6 +14,11 @@ namespace robot_costmap_2d
     bool laneFilter(const std::vector<robot_geometry_msgs::PoseStamped> plan);
     void resetMap();
 
+    LayerType getType() const override
+    {
+        return LayerType::DIRECTIONAL_LAYER;
+    }
+    
 	private:
     void incomingMap(const robot_nav_msgs::OccupancyGrid &new_map);
 		bool laneFilter(std::vector<std::array<uint16_t, 2>> new_map, const robot_nav_msgs::Path path);

include/robot_costmap_2d/inflation_layer.h

@@ -120,6 +120,11 @@ public:
    */
   void setInflationParameters(double inflation_radius, double cost_scaling_factor);
 
+  LayerType getType() const override
+  {
+      return LayerType::INFLATION_LAYER;
+  }
+  
 protected:
   virtual void onFootprintChanged();
   boost::recursive_mutex* inflation_access_;

include/robot_costmap_2d/layer.h

@@ -45,6 +45,20 @@
 #include <robot/node_handle.h>
 namespace robot_costmap_2d
 {
+
+enum class LayerType
+{
+    UNKNOWN,
+    STATIC_LAYER,
+    OBSTACLE_LAYER,
+    INFLATION_LAYER,
+    CRITICAL_LAYER,
+    DIRECTIONAL_LAYER,
+    PREFERRED_LAYER,
+    UNPREFERRED_LAYER,
+    VOXEL_LAYER
+};
+
 class LayeredCostmap;
 
 class Layer
@@ -134,6 +148,9 @@ public:
   void dataCallBack(const T& value, const std::string& topic) {
       handle(value, topic);
   }
+
+  virtual LayerType getType() const { return LayerType::UNKNOWN; }
+  
 protected:
 
   // Hàm template public, dùng để gửi dữ liệu
@@ -157,7 +174,7 @@ protected:
   bool enabled_;
   std::string name_;
   tf3::BufferCore *tf_;
-
+  
 private:
   std::vector<robot_geometry_msgs::Point> footprint_spec_;
 };

include/robot_costmap_2d/obstacle_layer.h

@@ -57,6 +57,7 @@ namespace robot_costmap_2d
 {
 struct CallBackInfo
 {
+  std::string observation_source;
   std::string data_type;
   std::string topic;
   bool inf_is_valid;
@@ -84,6 +85,11 @@ public:
   void addStaticObservation(robot_costmap_2d::Observation& obs, bool marking, bool clearing);
   void clearStaticObservations(bool marking, bool clearing);
 
+  LayerType getType() const override
+  {
+      return LayerType::OBSTACLE_LAYER;
+  }
+  
 protected:
   void handleImpl(const void* data, 
                   const std::type_info&, 

include/robot_costmap_2d/preferred_layer.h

@@ -10,6 +10,12 @@ class PreferredLayer : public StaticLayer
 public:
   PreferredLayer();
   virtual ~PreferredLayer();
+
+  LayerType getType() const override
+  {
+      return LayerType::PREFERRED_LAYER;
+  }
+  
 private:
   unsigned char interpretValue(unsigned char value);
 };

include/robot_costmap_2d/static_layer.h

@@ -63,6 +63,12 @@ public:
   virtual void updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   virtual void matchSize();
+
+  LayerType getType() const override
+  {
+      return LayerType::STATIC_LAYER;
+  }
+  
 protected:
   void handleImpl(const void* data,
                   const std::type_info& type,

include/robot_costmap_2d/unpreferred_layer.h

@@ -11,6 +11,11 @@ public:
   UnPreferredLayer();
   virtual ~UnPreferredLayer();
 
+  LayerType getType() const override
+  {
+      return LayerType::UNPREFERRED_LAYER;
+  }
+  
 private:
   unsigned char interpretValue(unsigned char value);
 

include/robot_costmap_2d/voxel_layer.h

@@ -77,6 +77,10 @@ public:
   virtual void matchSize();
   virtual void reset();
 
+  LayerType getType() const override
+  {
+      return LayerType::VOXEL_LAYER;
+  }
 
 protected: