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

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