add file costmap_2d_robot

include/costmap_2d/costmap_2d_robot.h

@@ -40,14 +40,10 @@
 
 #include <costmap_2d/layered_costmap.h>
 #include <costmap_2d/layer.h>
-// #include <costmap_2d/costmap_2d_publisher.h>
-// #include <costmap_2d/Costmap2DConfig.h>
 #include <costmap_2d/footprint.h>
 #include <geometry_msgs/Polygon.h>
 #include <geometry_msgs/PolygonStamped.h>
 #include <geometry_msgs/PoseStamped.h>
-// #include <dynamic_reconfigure/server.h>
-// #include <pluginlib/class_loader.hpp>
 #include <tf3/LinearMath/Transform.h>
 #include <robot/rate.h>
 #include <costmap_2d/data_convert.h>

include/costmap_2d/data_convert.h

@@ -83,7 +83,7 @@ namespace costmap_2d
         return geometry_msgs::Quaternion(q.x(),q.y(),q.z(),q.w());
     }
     
-    tf3::Transform convertToTf2Transform(const geometry_msgs::Transform& msg)
+    tf3::Transform convertTotf3Transform(const geometry_msgs::Transform& msg)
     {
         tf3::Quaternion q(
             msg.rotation.x,
@@ -102,7 +102,7 @@ namespace costmap_2d
         return tf;
     }
 
-    tf3::Transform convertToTf2Transform(const tf3::TransformMsg& msg)
+    tf3::Transform convertTotf3Transform(const tf3::TransformMsg& msg)
     {
         tf3::Quaternion q(
             msg.rotation.x,

include/costmap_2d/observation_buffer.h

@@ -37,14 +37,14 @@ public:
    * @param  max_obstacle_height The minimum height of a hitpoint to be considered legal
    * @param  obstacle_range The range to which the sensor should be trusted for inserting obstacles
    * @param  raytrace_range The range to which the sensor should be trusted for raytracing to clear out space
-   * @param  tf2_buffer A reference to a tf3 Buffer
+   * @param  tf3_buffer A reference to a tf3 Buffer
    * @param  global_frame The frame to transform PointClouds into
    * @param  sensor_frame The frame of the origin of the sensor, can be left blank to be read from the messages
    * @param  tf_tolerance The amount of time to wait for a transform to be available when setting a new global frame
    */
   ObservationBuffer(std::string topic_name, double observation_keep_time, double expected_update_rate,
                     double min_obstacle_height, double max_obstacle_height, double obstacle_range,
-                    double raytrace_range, tf3::BufferCore& tf2_buffer, std::string global_frame,
+                    double raytrace_range, tf3::BufferCore& tf3_buffer, std::string global_frame,
                     std::string sensor_frame, double tf_tolerance);
 
   /**
@@ -107,7 +107,7 @@ private:
    */
   void purgeStaleObservations();
 
-  tf3::BufferCore& tf2_buffer_;
+  tf3::BufferCore& tf3_buffer_;
   // const ros::Duration observation_keep_time_;
   // const ros::Duration expected_update_rate_;
   // ros::Time last_updated_;

include/costmap_2d/obstacle_layer.h

@@ -52,7 +52,7 @@
 #include <sensor_msgs/point_cloud_conversion.h>
 
 #include <laser_geometry/laser_geometry.hpp>
-// #include <tf2_ros/message_filter.h>
+// #include <tf3_ros/message_filter.h>
 // #include <message_filters/subscriber.h>
 
 namespace costmap_2d

include/costmap_2d/testing_helper.h

@@ -49,14 +49,14 @@ unsigned int countValues(costmap_2d::Costmap2D& costmap, unsigned char value, bo
   return count;
 }
 
-void addStaticLayer(costmap_2d::LayeredCostmap& layers, tf2_ros::Buffer& tf)
+void addStaticLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
 {
   costmap_2d::StaticLayer* slayer = new costmap_2d::StaticLayer();
   layers.addPlugin(boost::shared_ptr<costmap_2d::Layer>(slayer));
   slayer->initialize(&layers, "static", &tf);
 }
 
-costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, tf2_ros::Buffer& tf)
+costmap_2d::ObstacleLayer* addObstacleLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
 {
   costmap_2d::ObstacleLayer* olayer = new costmap_2d::ObstacleLayer();
   olayer->initialize(&layers, "obstacles", &tf);
@@ -86,7 +86,7 @@ void addObservation(costmap_2d::ObstacleLayer* olayer, double x, double y, doubl
   olayer->addStaticObservation(obs, true, true);
 }
 
-costmap_2d::InflationLayer* addInflationLayer(costmap_2d::LayeredCostmap& layers, tf2_ros::Buffer& tf)
+costmap_2d::InflationLayer* addInflationLayer(costmap_2d::LayeredCostmap& layers, tf3::BufferCore& tf)
 {
   costmap_2d::InflationLayer* ilayer = new costmap_2d::InflationLayer();
   ilayer->initialize(&layers, "inflation", &tf);

include/costmap_2d/utils.h

@@ -1,6 +1,9 @@
 #ifndef COSTMAP_2D_UTILS_H
 #define COSTMAP_2D_UTILS_H
 #include <yaml-cpp/yaml.h>
+#include <filesystem>
+#include <string>
+#include <iostream>
 
 namespace costmap_2d
 {
@@ -12,6 +15,29 @@ namespace costmap_2d
         return default_value;
     }
 
+    std::string getSourceFile(const std::string& root, const std::string& config_file_name)
+    {
+        std::string path_source = " ";
+        std::string sub_folder = "config";
+        namespace fs = std::filesystem;
+
+        fs::path cfg_dir = fs::path(root) / sub_folder;
+
+        if (fs::exists(cfg_dir) && fs::is_directory(cfg_dir))
+        {
+            for (const auto& entry : fs::recursive_directory_iterator(cfg_dir))
+            {
+                if (entry.is_regular_file() && entry.path().filename() == config_file_name)
+                {
+                    path_source = entry.path().string();
+                    break;  // tìm thấy thì dừng
+                }
+            }
+        }
+        
+        return path_source;
+    }
+
 }
 
 #endif  // COSTMAP_2D_UTILS_H