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

Compare commits

base: DuongTD:384897b7504103468a5c0501157485c12d8289d8
Branches Tags
DuongTD:main
..
compare: DuongTD:b18aeb39abc7d8eae2991cdde94f7a45cddaab32
Branches Tags
DuongTD:main

2 Commits
384897b750 ... b18aeb39ab

Author SHA1 Message Date
HiepLM
b18aeb39ab replaced printf -> robot::console 2026-01-10 11:38:19 +07:00
HiepLM
b66bd7c751 DuongTD gui zalo 2026-01-10 10:16:51 +07:00
16 changed files with 94 additions and 108 deletions
Show Stats Expand all files Collapse all files

1
config/static_layer_params.yaml
View File

@ -1,5 +1,6 @@
static_layer:
enabled: true
map_topic: "map"
first_map_only: false
subscribe_to_updates: false
track_unknown_space: true

2
config/voxel_layer_params.yaml
View File

@ -7,5 +7,5 @@ voxel_layer:
z_voxels: 16
unknown_threshold: 15.0
mark_threshold: 0
combination_method: 3
combination_method: 1

6
include/robot_costmap_2d/testing_helper.h
View File

@ -25,12 +25,12 @@ char printableCost(unsigned char cost)
void printMap(robot_costmap_2d::Costmap2D& costmap)
{
printf("map:\n");
robot::log_info("map:\n");
for (int i = 0; i < costmap.getSizeInCellsY(); i++){
for (int j = 0; j < costmap.getSizeInCellsX(); j++){
printf("%4d", int(costmap.getCost(j, i)));
robot::log_info("%4d", int(costmap.getCost(j, i)));
}
printf("\n\n");
robot::log_info("\n\n");
}
}

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;

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;

74
src/costmap_2d_robot.cpp
View File

@ -102,6 +102,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
global_frame_ = global_frame;
robot_base_frame_ = robot_base_frame;
robot::log_error("robot_base_frame: %s", robot_base_frame.c_str());
robot::Time last_error = robot::Time::now();
std::string tf_error;
@ -137,9 +138,34 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
if (nh.hasParam("library_path"))
path_plugins = loader.findLibraryPath(name_);
robot::log_error("name: %s | rolling_window: %x", name_.c_str(), rolling_window);
layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);
// find size parameters
double map_width_meters = loadParam(layer, "width", 0.0);
double map_height_meters = loadParam(layer, "height", 0.0);
double resolution = loadParam(layer, "resolution", 0.0);
double origin_x = loadParam(layer, "origin_x", 0.0);
double origin_y = loadParam(layer, "origin_y", 0.0);
if (nh.hasParam("width"))
nh.getParam("width", map_width_meters);
if (nh.hasParam("height"))
nh.getParam("height", map_height_meters);
if (nh.hasParam("resolution"))
nh.getParam("resolution", resolution);
if (nh.hasParam("origin_x"))
nh.getParam("origin_x", origin_x);
if (nh.hasParam("origin_y"))
nh.getParam("origin_y", origin_y);
if (!layered_costmap_->isSizeLocked())
{
// robot::log_warning("ROBOT origin_x: %f | origin_y: %f", origin_x, origin_y);
layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
(unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
}
struct PluginConfig
{
std::string name;
@ -236,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_);
@ -253,31 +280,8 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH
map_update_thread_shutdown_ = false;
double map_update_frequency = loadParam(layer, "update_frequency", 0.0);
// find size parameters
double map_width_meters = loadParam(layer, "width", 0.0);
double map_height_meters = loadParam(layer, "height", 0.0);
double resolution = loadParam(layer, "resolution", 0.0);
double origin_x = loadParam(layer, "origin_x", 0.0);
double origin_y = loadParam(layer, "origin_y", 0.0);
if (nh.hasParam("update_frequency"))
nh.getParam("update_frequency", map_update_frequency);
if (nh.hasParam("width"))
nh.getParam("width", map_width_meters);
if (nh.hasParam("height"))
nh.getParam("height", map_height_meters);
if (nh.hasParam("resolution"))
nh.getParam("resolution", resolution);
if (nh.hasParam("origin_x"))
nh.getParam("origin_x", origin_x);
if (nh.hasParam("origin_y"))
nh.getParam("origin_y", origin_y);
if (!layered_costmap_->isSizeLocked())
{
layered_costmap_->resizeMap((unsigned int)(map_width_meters / resolution),
(unsigned int)(map_height_meters / resolution), resolution, origin_x, origin_y);
}
// If the padding has changed, call setUnpaddedRobotFootprint() to
// re-apply the padding.
@ -380,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());
}
}
@ -431,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();
}
}
@ -474,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();
@ -549,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: "

8
src/layered_costmap.cpp
View File

@ -111,7 +111,6 @@ namespace robot_costmap_2d
minx_ = miny_ = 1e30;
maxx_ = maxy_ = -1e30;
printf("START\n");
for (vector<boost::shared_ptr<Layer>>::iterator plugin = plugins_.begin(); plugin != plugins_.end();
++plugin)
{
@ -121,18 +120,16 @@ namespace robot_costmap_2d
double prev_miny = miny_;
double prev_maxx = maxx_;
double prev_maxy = maxy_;
std::cout << "robot x: " << robot_x << "\nrobot y: " << robot_y << "\nrobot yaw: " << robot_yaw << std::endl;
(*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_,
(*plugin)->getName().c_str());
}
}
printf("END\n");
int x0, xn, y0, yn;
costmap_.worldToMapEnforceBounds(minx_, miny_, x0, y0);
@ -143,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;

4
test/static_layer_test.cpp
View File

@ -67,9 +67,9 @@ void CostmapTester::compareCells(robot_costmap_2d::Costmap2D& costmap, unsigned
double furthest_valid_distance = 0; // ################costmap.cell_inscribed_radius_ + cell_distance + 1;
unsigned char expected_lowest_cost = 0; // ################costmap.computeCost(furthest_valid_distance);
if(neighbor_cost < expected_lowest_cost){
printf("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
robot::log_error("Cell cost (%d, %d): %d, neighbor cost (%d, %d): %d, expected lowest cost: %d, cell distance: %.2f, furthest valid distance: %.2f",
x, y, cell_cost, nx, ny, neighbor_cost, expected_lowest_cost, cell_distance, furthest_valid_distance);
printf("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
robot::log_error("Cell: (%d, %d), Neighbor: (%d, %d)", x, y, nx, ny);
costmap.saveMap("failing_costmap.pgm");
}
EXPECT_TRUE(neighbor_cost >= expected_lowest_cost || (furthest_valid_distance > 0/* costmap.cell_inflation_radius_ */&& neighbor_cost == robot_costmap_2d::FREE_SPACE));