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

replaced printf -> robot::console

Browse Source
This commit is contained in:
HiepLM
2026-01-10 11:38:19 +07:00
parent b66bd7c751
commit b18aeb39ab
14 changed files with 65 additions and 80 deletions
Download Patch File Download Diff File Expand all files Collapse all files

23
src/costmap_2d_robot.cpp
View File

@@ -262,6 +262,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
std::cout <<"FOOTPRINT ROBOT:"<<std::endl;
new_footprint = makeFootprintFromParams(nh);
}
if (nh.hasParam("transform_tolerance"))
nh.getParam("transform_tolerance", transform_tolerance_);
@@ -383,7 +384,7 @@ void Costmap2DROBOT::mapUpdateLoop(double frequency)
r.sleep();
// make sure to sleep for the remainder of our cycle time
if (r.cycleTime() > robot::Duration(1 / frequency))
printf("Map update loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds\n", frequency,
robot::log_warning("Map update %s loop missed its desired rate of %.4fHz... the loop actually took %.4f seconds\n", name_.c_str(), frequency,
r.cycleTime().toSec());
}
}
@@ -434,11 +435,11 @@ void Costmap2DROBOT::start()
robot::Rate r(100.0);
while (!initialized_ && map_update_thread_)
{
// if (robot::Time::now() - start_time > robot::Duration(5.0))
// {
// printf("Costmap2DROBOT::start() timed out waiting for costmap to initialize or map_update_thread\n");
// break;
// }
if (robot::Time::now() - start_time > robot::Duration(5.0))
{
robot::log_warning_throttle(3.0, "Costmap2DROBOT::start() timed out waiting for costmap to initialize or map_update_thread\n");
break;
}
r.sleep();
}
}
@@ -477,7 +478,7 @@ void Costmap2DROBOT::resume()
{
if (robot::Time::now() - start_time > robot::Duration(5.0))
{
printf("Costmap2DROBOT::resume() timed out waiting for costmap to initialize\n");
robot::log_warning("Costmap2DROBOT::resume() timed out waiting for costmap to initialize\n");
break;
}
r.sleep();
@@ -552,24 +553,24 @@ bool Costmap2DROBOT::getRobotPose(robot_geometry_msgs::PoseStamped& global_pose)
}
catch (tf3::LookupException& ex)
{
printf("Cost Map No Transform available Error looking up robot pose: %s\n", ex.what());
robot::log_error("Costmap2DROBOT %s No Transform available Error looking up robot pose: %s\n", name_.c_str(), ex.what());
return false;
}
catch (tf3::ConnectivityException& ex)
{
printf("Connectivity Error looking up robot pose: %s\n", ex.what());
robot::log_error("Costmap2DROBOT %s Connectivity Error looking up robot pose: %s\n", name_.c_str(), ex.what());
return false;
}
catch (tf3::ExtrapolationException& ex)
{
printf("Extrapolation Error looking up robot pose: %s\n", ex.what());
robot::log_error("Costmap2DROBOT %s Extrapolation Error looking up robot pose: %s\n", name_.c_str(), ex.what());
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);
// check global_pose timeout
if (!global_pose.header.stamp.isZero() && current_time.toSec() - global_pose.header.stamp.toSec() > transform_tolerance_)
{
printf("Costmap2DROBOT transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f\n",
robot::log_warning("Costmap2DROBOT %s transform timeout. Current time: %.4f, global_pose stamp: %.4f, tolerance: %.4f\n", name_.c_str(),
current_time.toSec(), global_pose.header.stamp.toSec(), transform_tolerance_);
return false;
}