From fdfba18bde0662ab5f042b938ecd011c5382ca7a Mon Sep 17 00:00:00 2001 From: HiepLM Date: Tue, 16 Dec 2025 15:13:07 +0700 Subject: [PATCH] update --- src/costmap_2d_robot.cpp | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index c641209..99eddf7 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -35,6 +35,7 @@ * Author: Eitan Marder-Eppstein * David V. Lu!! *********************************************************************/ +#include #include #include #include @@ -105,20 +106,20 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH robot::Time last_error = robot::Time::now(); std::string tf_error; - // // we need to make sure that the transform between the robot base frame and the global frame is available - // while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error)) - // { - // if (last_error + robot::Duration(5.0) < robot::Time::now()) - // { - // // 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(); - // } - // // The error string will accumulate and errors will typically be the same, so the last - // // will do for the warning above. Reset the string here to avoid accumulation. - // tf_error.clear(); - // } + // we need to make sure that the transform between the robot base frame and the global frame is available + while (!tf_.canTransform(global_frame_, robot_base_frame_, tf3::Time::now(), &tf_error)) + { + if (last_error + robot::Duration(5.0) < robot::Time::now()) + { + // std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl; + robot::log_warning("[%s:%d] %0.6f: Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s\n", + __FILE__, __LINE__, robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str()); + last_error = robot::Time::now(); + } + // The error string will accumulate and errors will typically be the same, so the last + // will do for the warning above. Reset the string here to avoid accumulation. + tf_error.clear(); + } // check if we want a rolling window version of the costmap bool rolling_window = loadParam(layer, "rolling_window", false);