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

2
plugins/critical_layer.cpp
View File

@@ -13,7 +13,6 @@ CriticalLayer::~CriticalLayer(){}
unsigned char CriticalLayer::interpretValue(unsigned char value)
{
// printf("TEST PLUGIN CRITICAL\n");
// check if the static value is above the unknown or lethal thresholds
if(value >= *this->threshold_)
return CRITICAL_SPACE;
@@ -23,7 +22,6 @@ unsigned char CriticalLayer::interpretValue(unsigned char value)
void CriticalLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_i, int min_j, int max_i, int max_j)
{
// printf("TEST PLUGIN CRITICAL\n");
if (!map_received_)
return;

17
plugins/directional_layer.cpp
View File

@@ -36,7 +36,7 @@ namespace robot_costmap_2d
{
unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
printf("Received a %d X %d map at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
robot::log_info("Received a %d X %d map at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
// resize costmap if size, resolution or origin do not match
Costmap2D *master = layered_costmap_->getCostmap();
@@ -48,7 +48,7 @@ namespace robot_costmap_2d
master->getOriginY() != new_map.info.origin.position.y))
{
// Update the size of the layered costmap (and all layers, including this one)
printf("Resizing costmap to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
robot::log_info("Resizing costmap to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
layered_costmap_->resizeMap(size_x / 2, size_y / 2, new_map.info.resolution, new_map.info.origin.position.x,
new_map.info.origin.position.y,
true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
@@ -59,7 +59,7 @@ namespace robot_costmap_2d
origin_y_ != new_map.info.origin.position.y)
{
// only update the size of the costmap stored locally in this layer
printf("Resizing static layer to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
robot::log_info("Resizing static layer to %d X %d at %f m/pix\n", size_x / 2, size_y / 2, new_map.info.resolution);
resizeMap(size_x / 2, size_y / 2, new_map.info.resolution,
new_map.info.origin.position.x, new_map.info.origin.position.y);
}
@@ -99,12 +99,12 @@ namespace robot_costmap_2d
}
else
{
printf("Stop receive new map!\n");
robot::log_info("Stop receive new map!\n");
}
// shutdown the map subscrber if firt_map_only_ flag is on
if (first_map_only_)
{
printf("Shutting down the map subscriber. first_map_only flag is on\n");
robot::log_info("Shutting down the map subscriber. first_map_only flag is on\n");
map_shutdown_ = true;
// map_sub_.shutdown();
}
@@ -129,7 +129,7 @@ namespace robot_costmap_2d
unsigned int mx, my;
if (!worldToMap(p.pose.position.x, p.pose.position.y, mx, my))
{
printf("ERROR: trying to set a start cell %f %f that is outside of map\n", p.pose.position.x, p.pose.position.y);
robot::log_error("ERROR: trying to set a start cell %f %f that is outside of map\n", p.pose.position.x, p.pose.position.y);
return false;
}
// Convert to yaw
@@ -263,8 +263,7 @@ namespace robot_costmap_2d
y_max_w = std::max(y_max_w, y[i].second);
}
// printf("%d %d %d %d", x_min, y_min, x_max, y_max);
// printf("%f %f %f %f", x_min_w, y_min_w, x_max_w, y_max_w);
for (int i = 0; i < yaw_robot.size(); i++)
{
if(inSkipErea(pose_x_, pose_y_, x[i].second, y[i].second, distance_skip_))
@@ -407,7 +406,7 @@ namespace robot_costmap_2d
}
catch (tf3::TransformException &ex)
{
printf("%s\n", ex.what());
robot::log_error("%s\n", ex.what());
return false;
}
// Copy map data given proper transformations

10
plugins/inflation_layer.cpp
View File

@@ -186,7 +186,7 @@ void InflationLayer::onFootprintChanged()
computeCaches();
need_reinflation_ = true;
printf("InflationLayer::onFootprintChanged(): num footprint points: %lu,"
robot::log_info("InflationLayer::onFootprintChanged(): num footprint points: %lu,"
" inscribed_radius_ = %.3f, inflation_radius_ = %.3f\n",
layered_costmap_->getFootprint().size(), inscribed_radius_, inflation_radius_);
}
@@ -199,19 +199,19 @@ void InflationLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int m
// make sure the inflation list is empty at the beginning of the cycle (should always be true)
if(!inflation_cells_.empty())
printf("The inflation list must be empty at the beginning of inflation\n");
robot::log_error("The inflation list must be empty at the beginning of inflation\n");
unsigned char* master_array = master_grid.getCharMap();
unsigned int size_x = master_grid.getSizeInCellsX(), size_y = master_grid.getSizeInCellsY();
if (seen_ == NULL) {
printf("InflationLayer::updateCosts(): seen_ array is NULL\n");
robot::log_error("InflationLayer::updateCosts(): seen_ array is NULL\n");
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
}
else if (seen_size_ != size_x * size_y)
{
printf("InflationLayer::updateCosts(): seen_ array size is wrong\n");
robot::log_error("InflationLayer::updateCosts(): seen_ array size is wrong\n");
delete[] seen_;
seen_size_ = size_x * size_y;
seen_ = new bool[seen_size_];
@@ -405,7 +405,7 @@ void InflationLayer::handleImpl(const void* data,
const std::type_info& info,
const std::string& source)
{
printf("This function is not available!\n");
robot::log_error("This function is not available!\n");
}

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;

1
plugins/preferred_layer.cpp
View File

@@ -14,7 +14,6 @@ PreferredLayer::~PreferredLayer(){}
unsigned char PreferredLayer::interpretValue(unsigned char value)
{
printf("TEST PLUGIN !!!\n");
// check if the static value is above the unknown or lethal thresholds
if(value == 0) return NO_INFORMATION;
else if (value >= *this->threshold_)

17
plugins/static_layer.cpp
View File

@@ -201,7 +201,7 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
std::cout << "Received new map!" << std::endl;
unsigned int size_x = new_map.info.width, size_y = new_map.info.height;
printf("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
robot::log_info("Received a %d X %d map at %f m/pix\n", size_x, size_y, new_map.info.resolution);
// resize costmap if size, resolution or origin do not match
Costmap2D* master = layered_costmap_->getCostmap();
@@ -213,7 +213,7 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
master->getOriginY() != new_map.info.origin.position.y))
{
// Update the size of the layered costmap (and all layers, including this one)
printf("Resizing costmap to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
robot::log_info("Resizing costmap to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
layered_costmap_->resizeMap(size_x, size_y, new_map.info.resolution, new_map.info.origin.position.x,
new_map.info.origin.position.y,
true /* set size_locked to true, prevents reconfigureCb from overriding map size*/);
@@ -224,7 +224,7 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
origin_y_ != new_map.info.origin.position.y)
{
// only update the size of the costmap stored locally in this layer
printf("Resizing static layer to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
robot::log_info("Resizing static layer to %d X %d at %f m/pix\n", size_x, size_y, new_map.info.resolution);
resizeMap(size_x, size_y, new_map.info.resolution,
new_map.info.origin.position.x, new_map.info.origin.position.y);
}
@@ -247,11 +247,9 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
{
unsigned char value = new_map.data[index];
costmap_[index] = interpretValue(value);
// printf("%d , ",costmap_[index]);
// 3. Ghi giá trị biến
// file << static_cast<int>(costmap_[index]) << " , ";
++index;
// printf("%d , ",costmap_[index]);
}
// file << std::endl;
}
@@ -268,13 +266,13 @@ void StaticLayer::incomingMap(const robot_nav_msgs::OccupancyGrid& new_map)
}
else
{
printf("Stop receive new map!");
robot::log_info("Stop receive new map!\n");
}
// shutdown the map subscrber if firt_map_only_ flag is on
if (first_map_only_)
{
printf("Shutting down the map subscriber. first_map_only flag is on\n");
robot::log_info("Shutting down the map subscriber. first_map_only flag is on\n");
map_shutdown_ = true;
}
}
@@ -283,7 +281,7 @@ void StaticLayer::incomingUpdate(const robot_map_msgs::OccupancyGridUpdate& upda
{
if(!map_update_shutdown_)
{
std::cout << "Update new map!" << std::endl;
robot::log_info("Update new map!\n");
unsigned int di = 0;
for (unsigned int y = 0; y < update.height ; y++)
{
@@ -328,7 +326,6 @@ void StaticLayer::reset()
{
onInitialize();
}
printf("RESET MAP");
}
void StaticLayer::updateBounds(double robot_x, double robot_y, double robot_yaw, double* min_x, double* min_y,
@@ -382,7 +379,7 @@ void StaticLayer::updateCosts(robot_costmap_2d::Costmap2D& master_grid, int min_
}
catch (tf3::TransformException ex)
{
printf("%s", ex.what());
robot::log_error("StaticLayer::updateCosts(): %s \n", ex.what());
return;
}
// Copy map data given proper transformations

2
plugins/voxel_layer.cpp
View File

@@ -316,7 +316,7 @@ void VoxelLayer::raytraceFreespace(const Observation& clearing_observation, doub
if (!worldToMap3DFloat(ox, oy, oz, sensor_x, sensor_y, sensor_z))
{
printf(
robot::log_error(
"The origin for the sensor at (%.2f, %.2f, %.2f) is out of map bounds. So, the costmap cannot raytrace for it.\n",
ox, oy, oz);
return;