fix file utils

CMakeLists.txt

@@ -62,6 +62,7 @@ target_link_libraries(costmap_2d
     nav_msgs
     map_msgs
     laser_geometry
+    visualization_msgs
     voxel_grid
     tf3
     tf3_geometry_msgs

config/costmap_params.yaml

@@ -27,7 +27,7 @@ costmap_2d:
   layer_config_file_name: layer_params 
 
   transform_tolerance: 0.0
-  update_frequency: 0.0
+  update_frequency: 1.0
   width: 0.0
   height: 0.0
   resolution: 0.0

include/costmap_2d/utils.h

@@ -25,7 +25,6 @@ namespace costmap_2d
         for (const auto& p : node) {
             if (p.size() != 2)
                 throw std::runtime_error("Footprint point must be [x, y]");
-
             geometry_msgs::Point point;
             point.x = p[0].as<double>();
             point.y = p[1].as<double>();

src/costmap_2d_robot.cpp

@@ -82,7 +82,6 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
     std::string folder = COSTMAP_2D_DIR;
     std::string path_source = getSourceFile(folder,config_file_name);
 
-    
     YAML::Node config = YAML::LoadFile(path_source);
     YAML::Node layer = config["costmap_2d"];
 
@@ -97,7 +96,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name)
     {
       if (last_error + robot::Duration(5.0) < robot::Time::now())
       {
-        printf("%f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
+        // std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl;
+        printf("%0.6f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n",
                 robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
         last_error = robot::Time::now();
       }
@@ -274,7 +274,7 @@ void Costmap2DROBOT::mapUpdateLoop(double frequency)
   while (!map_update_thread_shutdown_)
   {
     updateMap();
-    std::cout << "Costmap2DROBOT::mapUpdateLoop updateMap\n";
+    std::cout << robot::Time::now().toNSec()<< " :Costmap2DROBOT::mapUpdateLoop updateMap\n";
     r.sleep();
     // make sure to sleep for the remainder of our cycle time
     if (r.cycleTime() > robot::Duration(1 / frequency))