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

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:
 

plugins/inflation_layer.cpp

@@ -198,7 +198,8 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
     return;
 
   // make sure the inflation list is empty at the beginning of the cycle (should always be true)
-  printf("The inflation list must be empty at the beginning of inflation\n");
+  if(!inflation_cells_.empty())
+    printf("The inflation list must be empty at the beginning of inflation\n");
 
   unsigned char* master_array = master_grid.getCharMap();
   unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();

plugins/obstacle_layer.cpp

@@ -165,6 +165,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
         }
 
         CallBackInfo info_tmp;
+        info_tmp.observation_source = source;
         info_tmp.data_type = data_type;
         info_tmp.topic = topic;
         info_tmp.inf_is_valid = inf_is_valid;
@@ -237,6 +238,8 @@ void ObstacleLayer::handleImpl(const void* data,
           topic == callback_infos_[i].topic &&
           !callback_infos_[i].inf_is_valid)
       {
+        // if(topic == "/f_scan") robot::log_error("DATA front laser! %d",i);
+        // if(topic == "/b_scan") robot::log_error("DATA back laser! %d",i);
         laserScanCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
       }
       else if (type == typeid(robot_sensor_msgs::LaserScan) &&
@@ -244,6 +247,7 @@ void ObstacleLayer::handleImpl(const void* data,
               topic == callback_infos_[i].topic &&
               callback_infos_[i].inf_is_valid)
       {
+        
         laserScanValidInfCallback(*static_cast<const robot_sensor_msgs::LaserScan*>(data), buffer);
       }
       else if (type == typeid(robot_sensor_msgs::PointCloud) &&
@@ -266,10 +270,14 @@ void ObstacleLayer::handleImpl(const void* data,
         }
         pointCloud2Callback(*static_cast<const robot_sensor_msgs::PointCloud2*>(data), buffer);
       }
-      else 
+      // else 
-      {
+      // {
-          std::cout << "obstacle_layer: Unknown type: " << type.name() << std::endl;
+      //     std::cout << "obstacle_layer: check type: " << (type == typeid(robot_sensor_msgs::LaserScan)) << std::endl 
-      }
+      //     << "obstacle_layer: inf_is_valid: " << callback_infos_[i].inf_is_valid << std::endl
+      //     << "data type: " << callback_infos_[i].data_type << std::endl
+      //     << "topic: " << topic << std::endl
+      //     << "topic check: " << callback_infos_[i].topic << std::endl << std::endl; 
+      // }
     }
   }
   else 

src/costmap_2d.cpp

@@ -255,6 +255,8 @@ void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my)
   {
     my = (int)((wy - origin_y_) / resolution_);
   }
+  // printf("CHECK FUNCTION: %f | %f\n",wx,wy);
+  // printf("CHECK FUNCTION: resolution_: %f\n",resolution_);
 }
 
 void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)

src/costmap_2d_robot.cpp

@@ -128,6 +128,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
 
     robot::PluginLoaderHelper loader;
     
+
     if (nh.hasParam("rolling_window"))
       nh.getParam("rolling_window", rolling_window);
 
@@ -395,6 +396,7 @@ void Costmap2DROBOT::updateMap()
       double x = pose.pose.position.x,
              y = pose.pose.position.y,
              yaw = data_convert::getYaw(pose.pose.orientation);
+      // robot::log_error("ROBOT POSE: %f | %f | %f",x,y,yaw);
       layered_costmap_->updateMap(x, y, yaw);
       robot_geometry_msgs::PolygonStamped footprint;
       footprint.header.frame_id = global_frame_;
@@ -498,6 +500,13 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
 {
   robot_geometry_msgs::PoseStamped robot_pose;
   robot_geometry_msgs::Pose pose_default;
+  pose_default.orientation.x = 0;
+  pose_default.orientation.y = 0;
+  pose_default.orientation.z = 0;
+  pose_default.orientation.w = 1;
+  pose_default.position.x = 0;
+  pose_default.position.y = 0;
+  pose_default.position.z = 0;
   global_pose.pose = pose_default;
   robot_pose.pose = pose_default;
 
@@ -509,10 +518,22 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
   try
   {
     // use current time if possible (makes sure it's not in the future)
-    if (tf_.canTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time)))
+    if (tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time()))
     {
-      tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_, data_convert::convertTime(current_time));
+      tf3::TransformStampedMsg transform = tf_.lookupTransform(global_frame_, robot_base_frame_,tf3::Time());
       tf3::doTransform(robot_pose, global_pose, transform);
+      // robot::log_error("%s ||| %f | %f | %f ||| %f | %f | %f | %f", transform.child_frame_id.c_str(),
+      // global_pose.pose.position.x,
+      // global_pose.pose.position.y,
+      // global_pose.pose.position.z,
+      // global_pose.pose.orientation.x,
+      // global_pose.pose.orientation.y,
+      // global_pose.pose.orientation.z,
+      // global_pose.pose.orientation.w);
+      // transform.transform.rotation.x,
+      // transform.transform.rotation.y,
+      // transform.transform.rotation.z,
+      // transform.transform.rotation.w);
     }
     // use the latest otherwise
     else
@@ -521,7 +542,7 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
       tf3::TransformStampedMsg transform = tf_.lookupTransform(
                     global_frame_,                      // frame đích
                     robot_base_frame_,                      // frame nguồn
-                    data_convert::convertTime(robot_pose.header.stamp)
+                    tf3::Time()
                 );
       tf3::doTransform(robot_pose, global_pose, transform);
     }

src/layered_costmap.cpp

@@ -111,7 +111,7 @@ namespace robot_costmap_2d
 
     minx_ = miny_ = 1e30;
     maxx_ = maxy_ = -1e30;
-
+    printf("START\n");
     for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
          ++plugin)
     {
@@ -121,6 +121,7 @@ namespace robot_costmap_2d
       double prev_miny = miny_;
       double prev_maxx = maxx_;
       double prev_maxy = maxy_;
+      std::cout << "robot x: " << robot_x << "\nrobot y: " << robot_y << "\nrobot yaw: " << robot_yaw << std::endl;
       (*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
       if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
       {
@@ -131,6 +132,7 @@ namespace robot_costmap_2d
                           (*plugin)->getName().c_str());
       }
     }
+    printf("END\n");
 
     int x0, xn, y0, yn;
     costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
@@ -141,7 +143,8 @@ namespace robot_costmap_2d
     y0 = std::max(0, y0);
     yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
 
-    printf("Updating area x: [%d, %d] y: [%d, %d]\n", x0, xn, y0, yn);
+    // printf("Updating area x: [%d, %d] y: [%d, %d]\n", x0, xn, y0, yn);
+    // printf("Updating area x: [%f, %f] y: [%f, %f]\n", minx_, miny_, maxx_, maxy_);
     
     if (xn < x0 || yn < y0)
       return;

src/observation_buffer.cpp

@@ -140,7 +140,8 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
     tf3::TransformStampedMsg tfm_1 = tf3_buffer_.lookupTransform(
                         global_frame_,                      // frame đích
                         local_origin.header.frame_id,                      // frame nguồn
-                        data_convert::convertTime(local_origin.header.stamp)
+                        tf3::Time()
+                        // data_convert::convertTime(local_origin.header.stamp)
                         );
     tf3::doTransform(local_origin, global_origin, tfm_1);
 
@@ -161,7 +162,8 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
     tf3::TransformStampedMsg tfm_2 = tf3_buffer_.lookupTransform(
                         global_frame_,                      // frame đích
                         cloud.header.frame_id,                      // frame nguồn
-                        data_convert::convertTime(cloud.header.stamp)
+                        tf3::Time()
+                        // data_convert::convertTime(cloud.header.stamp)
                         );
     tf3::doTransform(cloud, global_frame_cloud, tfm_2);
     global_frame_cloud.header.stamp = cloud.header.stamp;