update obstacle_layer

plugins/static_layer.cpp

@@ -43,6 +43,7 @@
 #include <robot_tf3_geometry_msgs/tf3_geometry_msgs.h>
 
 #include <boost/dll/alias.hpp>
+#include <fstream>
 
 
 using robot_costmap_2d::NO_INFORMATION;
@@ -82,7 +83,7 @@ bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHand
         YAML::Node layer = config["static_layer"];
 
         // ===== 1. Load default từ layer (yaml / plugin config) =====
-        bool enabled               = loadParam(layer, "enabled", true);
+        bool enabled                = loadParam(layer, "enabled", true);
         bool first_map_only         = loadParam(layer, "first_map_only", false);
         bool subscribe_to_updates   = loadParam(layer, "subscribe_to_updates", false);
         bool track_unknown_space    = loadParam(layer, "track_unknown_space", true);
@@ -90,11 +91,14 @@ bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHand
         int  lethal_cost_threshold  = loadParam(layer, "lethal_cost_threshold", 100);
         int  unknown_cost_value     = loadParam(layer, "unknown_cost_value", -1);
         bool trinary_costmap        = loadParam(layer, "trinary_costmap", true);
+        std::string map_topic       = loadParam(layer, "map_topic", std::string("map")); 
         std::string base_frame_id   = loadParam(layer, "base_frame_id", std::string("map"));
 
         // ===== 2. Override từ ROS param server (chỉ khi param tồn tại) =====
         if (nh.hasParam("enabled"))
           nh.getParam("enabled", enabled, enabled);
+        if (nh.hasParam("map_topic"))
+          nh.getParam("map_topic", map_topic); 
         if (nh.hasParam("first_map_only"))
           nh.getParam("first_map_only", first_map_only);
         if (nh.hasParam("subscribe_to_updates"))
@@ -112,6 +116,7 @@ bool StaticLayer::getParams(const std::string& config_file_name, robot::NodeHand
         if (nh.hasParam("unknown_cost_value"))
           nh.getParam("unknown_cost_value", unknown_cost_value);
 
+        map_topic_ = map_topic;
         first_map_only_ = first_map_only;
         subscribe_to_updates_ = subscribe_to_updates;
         track_unknown_space_ = track_unknown_space;
@@ -152,15 +157,25 @@ void StaticLayer::matchSize()
 
 unsigned char StaticLayer::interpretValue(unsigned char value)
 {
+  
   // check if the static value is above the unknown or lethal thresholds
   if (track_unknown_space_ && value == unknown_cost_value_)
-    return NO_INFORMATION;
+    {
+      return NO_INFORMATION;
+    }
   else if (!track_unknown_space_ && value == unknown_cost_value_)
+  {
     return FREE_SPACE;
+  }
+    
   else if (value >= lethal_threshold_)
-    return LETHAL_OBSTACLE;
+    {
+      return LETHAL_OBSTACLE;
+    }
   else if (trinary_costmap_)
-    return FREE_SPACE;
+    {
+      return FREE_SPACE;
+    }
 
   double scale = (double) value / lethal_threshold_;
   return scale * LETHAL_OBSTACLE;
@@ -170,9 +185,9 @@ void StaticLayer::handleImpl(const void* data,
                             const std::type_info& type,
                             const std::string& topic)
 {
-    if (type == typeid(robot_nav_msgs::OccupancyGrid) && topic == "map") {
+    if (type == typeid(robot_nav_msgs::OccupancyGrid) && topic == map_topic_) {
         incomingMap(*static_cast<const robot_nav_msgs::OccupancyGrid*>(data));
-    } else if (type == typeid(robot_map_msgs::OccupancyGridUpdate) && topic == "map_update") {
+    } else if (type == typeid(robot_map_msgs::OccupancyGridUpdate) && topic == map_topic_ + "_updates") {
         incomingUpdate(*static_cast<const robot_map_msgs::OccupancyGridUpdate*>(data));
     } else {
         std::cout << "[Plugin] Unknown type: " << type.name() << std::endl;
@@ -215,6 +230,15 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
     }
 
     unsigned int index = 0;
+    // // 1. Tạo file (nếu chưa có) + mở để ghi
+    // std::ofstream file("/home/duongtd/ros_test_ws/src/third_party/costmap_2d/data.txt", std::ios::app);
+
+    // // 2. Kiểm tra mở file thành công
+    // if (!file)
+    // {
+    //     std::cerr << "Cannot create file\n";
+    //     return ;
+    // }
 
     // initialize the costmap with static data
     for (unsigned int i = 0; i < size_y; ++i)
@@ -223,9 +247,16 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
       {
         unsigned char value = new_map.data[index];
         costmap_[index] = interpretValue(value);
+        // printf("%d , ",costmap_[index]);
+        // 3. Ghi giá trị biến
+        // file << static_cast<int>(costmap_[index]) << " , ";
         ++index;
+        // printf("%d , ",costmap_[index]);
       }
+      // file << std::endl;
     }
+    // // 4. Đóng file
+    // file.close();
     map_frame_ = new_map.header.frame_id;
 
     // we have a new map, update full size of map