diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index fc02e33..deee0dd 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -92,19 +92,19 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name) 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()) - // { - // printf("%f: 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()) + { + printf("%f: 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(); + } // check if we want a rolling window version of the costmap bool rolling_window, track_unknown_space; diff --git a/test/static_layer_test.cpp b/test/static_layer_test.cpp index ffa8494..65d6e5b 100644 --- a/test/static_layer_test.cpp +++ b/test/static_layer_test.cpp @@ -18,7 +18,7 @@ class CostmapTester : public testing::Test { costmap_2d::Costmap2DROBOT costmap_ros_; }; -CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("costmap_global_params", tf){} +CostmapTester::CostmapTester(tf3::BufferCore& tf): costmap_ros_("costmap_params", tf){} void CostmapTester::checkConsistentCosts(){ costmap_2d::Costmap2D* costmap = costmap_ros_.getCostmap();