update function dataCallBack and file test static layer

include/costmap_2d/costmap_layer.h

@@ -39,6 +39,7 @@
 #define COSTMAP_2D_COSTMAP_LAYER_H_
 #include <costmap_2d/layer.h>
 #include <costmap_2d/layered_costmap.h>
+#include <typeinfo>
 
 namespace costmap_2d
 {
@@ -70,7 +71,20 @@ public:
    */
   void addExtraBounds(double mx0, double my0, double mx1, double my1);
 
+  template<typename T>
+  void dataCallBack(const T& value) {
+      handle(value);
+  }
+
 protected:
+  // Hàm template public, dùng để gửi dữ liệu
+  template<typename T>
+  void handle(const T& data) {
+      handleImpl(static_cast<const void*>(&data), typeid(T));
+  }
+
+  // Hàm ảo duy nhất mà plugin sẽ override
+  virtual void handleImpl(const void* data, const std::type_info& type) = 0;
   /*
    * Updates the master_grid within the specified
    * bounding box using this layer's values.

include/costmap_2d/static_layer.h

@@ -30,6 +30,10 @@ public:
   virtual void incomingMap(const nav_msgs::OccupancyGrid& new_map);
   virtual void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
 protected:
+  void handleImpl(const void* data, const std::type_info& type) override;
+  void process(const nav_msgs::OccupancyGrid& new_map);
+  void process(const map_msgs::OccupancyGridUpdate& update);
+
   virtual unsigned char interpretValue(unsigned char value);
   unsigned char* threshold_;
   std::string base_frame_id_;