DuongTD/costmap_2d
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Compare commits

base: DuongTD:3d621de80918df34c8e5ec67e95843bc9225e825
Branches Tags
DuongTD:main
...
compare: DuongTD:main
Branches Tags
DuongTD:main

1 Commits
3d621de809 ... main

Author SHA1 Message Date
hoangson02
2fcd211ccf update 2026-03-03 07:26:47 +00:00
1 changed files with 3 additions and 1 deletions
Expand all files Collapse all files

4
src/costmap_2d_robot.cpp
View File

@@ -129,6 +129,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name,const std::st
{ {
if (last_error + robot::Duration(5.0) < robot::Time::now()) if (last_error + robot::Duration(5.0) < robot::Time::now())
{ {
std::string all_frames_string = tf_.allFramesAsString();
robot::log_info("[%s:%d]\n INFO: tf allFramesAsString: %s", __FILE__, __LINE__, all_frames_string.c_str());
// std::cout << std::fixed << std::setprecision(6) << robot::Time::now().toSec() << std::endl; // 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", 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()); __FILE__, __LINE__, robot::Time::now().toSec(), robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
@@ -692,7 +694,7 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
} }
catch (tf3::ExtrapolationException& ex) catch (tf3::ExtrapolationException& ex)
{ {
robot::log_error("Costmap2DROBOT %s Extrapolation Error looking up robot pose: %s\n", name_.c_str(), ex.what()); // robot::log_error("Costmap2DROBOT %s Extrapolation Error looking up robot pose: %s\n", name_.c_str(), ex.what());
return false; return false;
} }
// ROS_INFO_THROTTLE(1.0, "Time Delay %f , p %f %f", current_time.toSec() - global_pose.header.stamp.toSec(), global_pose.pose.position.x, global_pose.pose.position.y); // ROS_INFO_THROTTLE(1.0, "Time Delay %f , p %f %f", current_time.toSec() - global_pose.header.stamp.toSec(), global_pose.pose.position.x, global_pose.pose.position.y);