update file obstacle_layer and file test plugin

include/costmap_2d/costmap_layer.h

@@ -72,19 +72,21 @@ public:
   void addExtraBounds(double mx0, double my0, double mx1, double my1);
 
   template<typename T>
-  void dataCallBack(const T& value) {
-      handle(value);
+  void dataCallBack(const T& value, const std::string& topic) {
+      handle(value, topic);
   }
 
 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));
+  void handle(const T& data, const std::string& topic) {
+      handleImpl(static_cast<const void*>(&data), typeid(T), topic);
   }
 
   // Hàm ảo duy nhất mà plugin sẽ override
-  virtual void handleImpl(const void* data, const std::type_info& type) = 0;
+  virtual void handleImpl(const void* data, 
+                          const std::type_info& type, 
+                          const std::string& topic) = 0;
   /*
    * Updates the master_grid within the specified
    * bounding box using this layer's values.

include/costmap_2d/critical_layer copy.h

@@ -1,19 +0,0 @@
-#ifndef COSTMAP_2D_CRITICAL_LAYER_H_
-#define COSTMAP_2D_CRITICAL_LAYER_H_
-
-#include <costmap_2d/static_layer.h>
-
-namespace costmap_2d
-{
-class CriticalLayer : public StaticLayer
-{
-public:
-  CriticalLayer();
-  virtual ~CriticalLayer();
-private:
-  unsigned char interpretValue(unsigned char value);
-  void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
-};
-}
-
-#endif // COSTMAP_2D_CRITICAL_LAYER_H_

include/costmap_2d/layer.h

@@ -146,6 +146,7 @@ private:
   std::vector<geometry_msgs::Point> footprint_spec_;
 };
 
+using PluginLayerPtr = std::shared_ptr<Layer>;
 
 }  // namespace costmap_2d
 

include/costmap_2d/obstacle_layer.h

@@ -51,7 +51,7 @@
 #include <sensor_msgs/PointCloud2.h>
 #include <sensor_msgs/point_cloud_conversion.h>
 
-// #include <laser_geometry/laser_geometry.h>
+#include <laser_geometry/laser_geometry.hpp>
 // #include <tf2_ros/message_filter.h>
 // #include <message_filters/subscriber.h>
 
@@ -114,6 +114,19 @@ public:
 
 protected:
 
+  void handleImpl(const void* data, 
+                  const std::type_info&, 
+                  const std::string& topic) override;
+  void laserScanCallback(const sensor_msgs::LaserScan& message, 
+               const boost::shared_ptr<ObservationBuffer>& buffer);
+  void laserScanValidInfCallback(const sensor_msgs::LaserScan& message, 
+                                 const boost::shared_ptr<ObservationBuffer>& buffer);
+  void pointCloudCallback(const sensor_msgs::PointCloud& message,
+                          const boost::shared_ptr<ObservationBuffer>& buffer);
+  void pointCloud2Callback(const sensor_msgs::PointCloud2& message,
+                           const boost::shared_ptr<ObservationBuffer>& buffer);
+  // void process(const map_msgs::OccupancyGridUpdate& update);
+
   /**
    * @brief  Get the observations used to mark space
    * @param marking_observations A reference to a vector that will be populated with the observations
@@ -150,7 +163,7 @@ protected:
   std::string global_frame_;  ///< @brief The global frame for the costmap
   double max_obstacle_height_;  ///< @brief Max Obstacle Height
 
-  // laser_geometry::LaserProjection projector_;  ///< @brief Used to project laser scans into point clouds
+  laser_geometry::LaserProjection projector_;  ///< @brief Used to project laser scans into point clouds
 
   // std::vector<boost::shared_ptr<message_filters::SubscriberBase> > observation_subscribers_;  ///< @brief Used for the observation message filters
   // std::vector<boost::shared_ptr<tf2_ros::MessageFilterBase> > observation_notifiers_;  ///< @brief Used to make sure that transforms are available for each sensor
@@ -162,6 +175,7 @@ protected:
   std::vector<costmap_2d::Observation> static_clearing_observations_, static_marking_observations_;
 
   bool rolling_window_;
+  bool stop_receiving_data_ = false;
 
   int combination_method_;
 

include/costmap_2d/static_layer.h

@@ -5,7 +5,6 @@
 #include <costmap_2d/layered_costmap.h>
 #include <nav_msgs/OccupancyGrid.h>
 #include <map_msgs/OccupancyGridUpdate.h>
-// #include <message_filters/subscriber.h>
 #include <string>
 #include <geometry_msgs/TransformStamped.h>
 
@@ -27,12 +26,12 @@ public:
   virtual void updateCosts(costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j);
 
   virtual void matchSize();
-  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);
+  void handleImpl(const void* data, 
+                  const std::type_info& type,
+                  const std::string& topic) override;
+  void incomingMap(const nav_msgs::OccupancyGrid& new_map);
+  void incomingUpdate(const map_msgs::OccupancyGridUpdate& update);
 
   virtual unsigned char interpretValue(unsigned char value);
   unsigned char* threshold_;
@@ -65,6 +64,8 @@ private:
   bool getParams();
 };
 
+using PluginStaticLayerPtr = std::shared_ptr<StaticLayer>;
+
 }  // namespace costmap_2d
 
 #endif  // COSTMAP_2D_STATIC_LAYER_H_