DuongTD gui zalo

config/static_layer_params.yaml

@@ -1,5 +1,6 @@
 static_layer:
   enabled: true
+  map_topic: "map"
   first_map_only: false
   subscribe_to_updates: false
   track_unknown_space: true

config/voxel_layer_params.yaml

@@ -7,5 +7,5 @@ voxel_layer:
   z_voxels: 16
   unknown_threshold: 15.0
   mark_threshold: 0
-  combination_method: 3
+  combination_method: 1
 

src/costmap_2d_robot.cpp

@@ -102,6 +102,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
 
     global_frame_ = global_frame;
     robot_base_frame_ = robot_base_frame;
+    robot::log_error("robot_base_frame: %s", robot_base_frame.c_str());
 
     robot::Time last_error = robot::Time::now();
     std::string tf_error;
@@ -137,9 +138,34 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
 
     if (nh.hasParam("library_path"))
       path_plugins = loader.findLibraryPath(name_);
-
+    robot::log_error("name: %s | rolling_window: %x", name_.c_str(), rolling_window);
     layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
 
+    // find size parameters
+    double map_width_meters = loadParam(layer, "width", 0.0);
+    double map_height_meters = loadParam(layer, "height", 0.0);
+    double resolution = loadParam(layer, "resolution", 0.0);
+    double origin_x = loadParam(layer, "origin_x", 0.0);
+    double origin_y = loadParam(layer, "origin_y", 0.0);
+
+    if (nh.hasParam("width"))
+      nh.getParam("width", map_width_meters);
+    if (nh.hasParam("height"))
+      nh.getParam("height", map_height_meters);
+    if (nh.hasParam("resolution"))
+      nh.getParam("resolution", resolution);
+    if (nh.hasParam("origin_x"))
+      nh.getParam("origin_x", origin_x);
+    if (nh.hasParam("origin_y"))
+      nh.getParam("origin_y", origin_y);
+
+    if (!layered_costmap_->isSizeLocked())
+    {
+      // robot::log_warning("ROBOT origin_x: %f | origin_y: %f", origin_x, origin_y);
+      layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
+                                  (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
+    }
+    
     struct PluginConfig 
     {
       std::string name;
@@ -253,31 +279,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
     map_update_thread_shutdown_ = false;
     double map_update_frequency = loadParam(layer, "update_frequency", 0.0);
 
-    // find size parameters
-    double map_width_meters = loadParam(layer, "width", 0.0);
-    double map_height_meters = loadParam(layer, "height", 0.0);
-    double resolution = loadParam(layer, "resolution", 0.0);
-    double origin_x = loadParam(layer, "origin_x", 0.0);
-    double origin_y = loadParam(layer, "origin_y", 0.0);
-
     if (nh.hasParam("update_frequency"))
       nh.getParam("update_frequency", map_update_frequency);
-    if (nh.hasParam("width"))
-      nh.getParam("width", map_width_meters);
-    if (nh.hasParam("height"))
-      nh.getParam("height", map_height_meters);
-    if (nh.hasParam("resolution"))
-      nh.getParam("resolution", resolution);
-    if (nh.hasParam("origin_x"))
-      nh.getParam("origin_x", origin_x);
-    if (nh.hasParam("origin_y"))
-      nh.getParam("origin_y", origin_y);
-
-    if (!layered_costmap_->isSizeLocked())
-    {
-      layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
-                                  (unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
-    }
 
     // If the padding has changed, call setUnpaddedRobotFootprint() to
     // re-apply the padding.

src/layered_costmap.cpp

@@ -111,7 +111,6 @@ 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,7 +120,6 @@ 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)
       {
@@ -132,7 +130,6 @@ namespace robot_costmap_2d
                           (*plugin)->getName().c_str());
       }
     }
-    printf("END\n");
 
     int x0, xn, y0, yn;
     costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);