diff --git a/CMakeLists.txt b/CMakeLists.txt
index c6cfd45..89bf7e1 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/config/costmap_params.yaml b/config/costmap_params.yaml
index 02a0cbf..d2f5164 100644
--- a/config/costmap_params.yaml
+++ b/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
diff --git a/include/costmap_2d/utils.h b/include/costmap_2d/utils.h
index 73ecb22..7e17ea7 100644
--- a/include/costmap_2d/utils.h
+++ b/include/costmap_2d/utils.h
@@ -25,8 +25,10 @@ namespace costmap_2d
         for (const auto& p : node) {
             if (p.size() != 2)
                 throw std::runtime_error("Footprint point must be [x, y]");
-
-            fp.push_back(geometry_msgs::Point{p[0].as<double>(), p[1].as<double>()});
+            geometry_msgs::Point point;
+            point.x = p[0].as<double>();
+            point.y = p[1].as<double>();
+            fp.push_back(point);
         }
         std::cout << "Loaded footprint with " << fp.size() << " points." << std::endl;
 
diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp
index deee0dd..055ae06 100644
--- a/src/costmap_2d_robot.cpp
+++ b/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))