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

5
src/costmap_2d.cpp
View File

@@ -255,8 +255,6 @@ void Costmap2D::worldToMapEnforceBounds(double wx, double wy, int& mx, int& my)
{
my = (int)((wy - origin_y_) / resolution_);
}
// printf("CHECK FUNCTION: %f | %f\n",wx,wy);
// printf("CHECK FUNCTION: resolution_: %f\n",resolution_);
}
void Costmap2D::updateOrigin(double new_origin_x, double new_origin_y)
@@ -473,15 +471,12 @@ bool Costmap2D::saveMap(std::string file_name)
return false;
}
fprintf(fp, "P2\n%u\n%u\n%u\n", size_x_, size_y_, 0xff);
for (unsigned int iy = 0; iy < size_y_; iy++)
{
for (unsigned int ix = 0; ix < size_x_; ix++)
{
unsigned char cost = getCost(ix, iy);
fprintf(fp, "%d ", cost);
}
fprintf(fp, "\n");
}
fclose(fp);
return true;

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;
}

14
src/footprint.cpp
View File

@@ -174,15 +174,15 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ro
if (error != "")
{
printf("Error parsing footprint parameter: '%s'\n", error.c_str());
printf(" Footprint string was '%s'.\n", footprint_string.c_str());
robot::log_error("Error parsing footprint parameter: '%s'\n", error.c_str());
robot::log_error(" Footprint string was '%s'.\n", footprint_string.c_str());
return false;
}
// convert vvf into points.
if (vvf.size() < 3)
{
printf("You must specify at least three points for the robot footprint, reverting to previous footprint.\n");
robot::log_error("You must specify at least three points for the robot footprint, reverting to previous footprint.\n");
return false;
}
footprint.reserve(vvf.size());
@@ -198,7 +198,7 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector<ro
}
else
{
printf("Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.\n",
robot::log_error("Points in the footprint specification must be pairs of numbers. Found a point with %d numbers.\n",
int(vvf[ i ].size()));
return false;
}
@@ -256,7 +256,7 @@ double getNumberFromXMLRPC(robot_xmlrpcpp::XmlRpcValue& value, const std::string
value.getType() != robot_xmlrpcpp::XmlRpcValue::TypeDouble)
{
std::string& value_string = value;
printf("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
robot::log_error("Values in the footprint specification (param %s) must be numbers. Found value %s.\n",
full_param_name.c_str(), value_string.c_str());
throw std::runtime_error("Values in the footprint specification must be numbers");
}
@@ -270,7 +270,7 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot_xmlrpcpp::
if (footprint_xmlrpc.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray ||
footprint_xmlrpc.size() < 3)
{
printf("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
robot::log_error("The footprint must be specified as list of lists on the parameter server, %s was specified as %s\n",
full_param_name.c_str(), std::string(footprint_xmlrpc).c_str());
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server with at least "
"3 points eg: [[x1, y1], [x2, y2], ..., [xn, yn]]");
@@ -286,7 +286,7 @@ std::vector<robot_geometry_msgs::Point> makeFootprintFromXMLRPC(robot_xmlrpcpp::
if (point.getType() != robot_xmlrpcpp::XmlRpcValue::TypeArray ||
point.size() != 2)
{
printf("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
robot::log_error("The footprint (parameter %s) must be specified as list of lists on the parameter server eg: "
"[[x1, y1], [x2, y2], ..., [xn, yn]], but this spec is not of that form.\n",
full_param_name.c_str());
throw std::runtime_error("The footprint must be specified as list of lists on the parameter server eg: "

5
src/layered_costmap.cpp
View File

@@ -123,7 +123,7 @@ namespace robot_costmap_2d
(*plugin)->updateBounds(robot_x, robot_y, robot_yaw, &minx_, &miny_, &maxx_, &maxy_);
if (minx_ > prev_minx || miny_ > prev_miny || maxx_ < prev_maxx || maxy_ < prev_maxy)
{
printf("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
robot::log_error("Illegal bounds change, was [tl: (%f, %f), br: (%f, %f)], but "
"is now [tl: (%f, %f), br: (%f, %f)]. The offending layer is %s\n",
prev_minx, prev_miny, prev_maxx, prev_maxy,
minx_, miny_, maxx_, maxy_,
@@ -140,9 +140,6 @@ namespace robot_costmap_2d
y0 = std::max(0, y0);
yn = std::min(int(costmap_.getSizeInCellsY()), yn + 1);
// printf("Updating area x: [%d, %d] y: [%d, %d]\n", x0, xn, y0, yn);
// printf("Updating area x: [%f, %f] y: [%f, %f]\n", minx_, miny_, maxx_, maxy_);
if (xn < x0 || yn < y0)
return;

9
src/observation_buffer.cpp
View File

@@ -68,7 +68,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
robot_geometry_msgs::TransformStamped transformStamped;
if (!tf3_buffer_.canTransform(new_global_frame, global_frame_, transform_time, &tf_error))
{
printf("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
robot::log_error("Transform between %s and %s with tolerance %.2f failed: %s.\n", new_global_frame.c_str(),
global_frame_.c_str(), tf_tolerance_, tf_error.c_str());
return false;
}
@@ -106,7 +106,7 @@ bool ObservationBuffer::setGlobalFrame(const std::string new_global_frame)
}
catch (TransformException& ex)
{
printf("TF Error attempting to transform an observation from %s to %s: %s\n", global_frame_.c_str(),
robot::log_error("TF Error attempting to transform an observation from %s to %s: %s\n", global_frame_.c_str(),
new_global_frame.c_str(), ex.what());
return false;
}
@@ -207,7 +207,7 @@ void ObservationBuffer::bufferCloud(const robot_sensor_msgs::PointCloud2& cloud)
{
// if an exception occurs, we need to remove the empty observation from the list
observation_list_.pop_front();
printf("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(),
robot::log_error("TF Exception that should never happen for sensor frame: %s, cloud frame: %s, %s\n", sensor_frame_.c_str(),
cloud.header.frame_id.c_str(), ex.what());
return;
}
@@ -267,8 +267,7 @@ bool ObservationBuffer::isCurrent() const
bool current = (robot::Time::now() - last_updated_).toSec() <= expected_update_rate_.toSec();
if (!current)
{
printf(
"The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n",
robot::log_error("The %s observation buffer has not been updated for %.2f seconds, and it should be updated every %.2f seconds.\n",
topic_name_.c_str(), (robot::Time::now() - last_updated_).toSec(), expected_update_rate_.toSec());
}
return current;