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

30
plugins/obstacle_layer.cpp
View File

@@ -187,7 +187,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
// enabled_ = enabled;
printf("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
robot::log_info("Creating an observation buffer for topic %s, frame %s\n", topic.c_str(),
sensor_frame.c_str());
// create an observation buffer
@@ -203,7 +203,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
if (clearing)
clearing_buffers_.push_back(observation_buffers_.back());
printf(
robot::log_info(
"Created an observation buffer for topic %s, global frame: %s, "
"expected update rate: %.2f, observation persistence: %.2f\n",
topic.c_str(), global_frame_.c_str(), expected_update_rate, observation_keep_time);
@@ -211,7 +211,7 @@ bool ObstacleLayer::getParams(const std::string& config_file_name, robot::NodeHa
}
}
catch (const YAML::BadFile& e) {
std::cerr << "Cannot open YAML file: " << e.what() << std::endl;
robot::log_error("Cannot open YAML file: %s\n", e.what());
return false;
}
@@ -256,7 +256,7 @@ void ObstacleLayer::handleImpl(const void* data,
{
if (callback_infos_[i].inf_is_valid)
{
printf("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
robot::log_error("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.\n");
}
pointCloudCallback(*static_cast<const robot_sensor_msgs::PointCloud*>(data), buffer);
}
@@ -266,7 +266,7 @@ void ObstacleLayer::handleImpl(const void* data,
{
if (callback_infos_[i].inf_is_valid)
{
printf("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.");
robot::log_error("obstacle_layer: inf_is_valid option is not applicable to PointCloud observations.\n");
}
pointCloud2Callback(*static_cast<const robot_sensor_msgs::PointCloud2*>(data), buffer);
}
@@ -282,7 +282,7 @@ void ObstacleLayer::handleImpl(const void* data,
}
else
{
std::cout << "Stop receiving data!" << std::endl;
robot::log_info("Stop receiving data!\n");
return;
}
}
@@ -301,13 +301,13 @@ void ObstacleLayer::laserScanCallback(const robot_sensor_msgs::LaserScan& messag
}
catch (tf3::TransformException &ex)
{
printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(),
robot::log_error("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n", global_frame_.c_str(),
ex.what());
projector_.projectLaser(message, cloud);
}
catch (std::runtime_error &ex)
{
printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
robot::log_error("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
return; //ignore this message
}
@@ -343,13 +343,13 @@ void ObstacleLayer::laserScanValidInfCallback(const robot_sensor_msgs::LaserScan
}
catch (tf3::TransformException &ex)
{
printf("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n",
robot::log_error("High fidelity enabled, but TF returned a transform exception to frame %s: %s\n",
global_frame_.c_str(), ex.what());
projector_.projectLaser(message, cloud);
}
catch (std::runtime_error &ex)
{
printf("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
robot::log_error("transformLaserScanToPointCloud error, it seems the message from laser sensor is malformed. Ignore this laser scan. what(): %s\n", ex.what());
return; //ignore this message
}
@@ -366,7 +366,7 @@ void ObstacleLayer::pointCloudCallback(const robot_sensor_msgs::PointCloud& mess
if (!robot_sensor_msgs::convertPointCloudToPointCloud2(message, cloud2))
{
printf("Failed to convert a PointCloud to a PointCloud2, dropping message\n");
robot::log_error("Failed to convert a PointCloud to a PointCloud2, dropping message\n");
return;
}
@@ -430,7 +430,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
// if the obstacle is too high or too far away from the robot we won't add it
if (pz > max_obstacle_height_)
{
printf("The point is too high\n");
robot::log_error("The point is too high\n");
continue;
}
@@ -441,7 +441,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
// if the point is far enough away... we won't consider it
if (sq_dist >= sq_obstacle_range)
{
printf("The point is too far away\n");
robot::log_error("The point is too far away\n");
continue;
}
@@ -449,7 +449,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
unsigned int mx, my;
if (!worldToMap(px, py, mx, my))
{
printf("Computing map coords failed\n");
robot::log_error("Computing map coords failed\n");
continue;
}
@@ -554,7 +554,7 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d
unsigned int x0, y0;
if (!worldToMap(ox, oy, x0, y0))
{
printf(
robot::log_error(
"The origin for the sensor at (%.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
ox, oy);
return;