diff --git a/include/robot_costmap_2d/testing_helper.h b/include/robot_costmap_2d/testing_helper.h index b864449..e3a0557 100644 --- a/include/robot_costmap_2d/testing_helper.h +++ b/include/robot_costmap_2d/testing_helper.h @@ -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"); } } diff --git a/plugins/critical_layer.cpp b/plugins/critical_layer.cpp index 8bf4806..1b1bcf2 100644 --- a/plugins/critical_layer.cpp +++ b/plugins/critical_layer.cpp @@ -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; diff --git a/plugins/directional_layer.cpp b/plugins/directional_layer.cpp index 5a29690..260af1f 100644 --- a/plugins/directional_layer.cpp +++ b/plugins/directional_layer.cpp @@ -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 diff --git a/plugins/inflation_layer.cpp b/plugins/inflation_layer.cpp index 04c3fce..37768c6 100755 --- a/plugins/inflation_layer.cpp +++ b/plugins/inflation_layer.cpp @@ -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"); } diff --git a/plugins/obstacle_layer.cpp b/plugins/obstacle_layer.cpp index cc4b2b0..f246631 100755 --- a/plugins/obstacle_layer.cpp +++ b/plugins/obstacle_layer.cpp @@ -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(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(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; diff --git a/plugins/preferred_layer.cpp b/plugins/preferred_layer.cpp index 094f83f..fcfcf4e 100644 --- a/plugins/preferred_layer.cpp +++ b/plugins/preferred_layer.cpp @@ -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_) diff --git a/plugins/static_layer.cpp b/plugins/static_layer.cpp index 13a1666..b6ec88f 100755 --- a/plugins/static_layer.cpp +++ b/plugins/static_layer.cpp @@ -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(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 diff --git a/plugins/voxel_layer.cpp b/plugins/voxel_layer.cpp index e021846..d96311d 100755 --- a/plugins/voxel_layer.cpp +++ b/plugins/voxel_layer.cpp @@ -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; diff --git a/src/costmap_2d.cpp b/src/costmap_2d.cpp index dccda0b..e5d7575 100755 --- a/src/costmap_2d.cpp +++ b/src/costmap_2d.cpp @@ -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; diff --git a/src/costmap_2d_robot.cpp b/src/costmap_2d_robot.cpp index ef87437..dbad0d1 100644 --- a/src/costmap_2d_robot.cpp +++ b/src/costmap_2d_robot.cpp @@ -262,6 +262,7 @@ void Costmap2DROBOT::getParams(const std::string& config_file_name, robot::NodeH std::cout <<"FOOTPRINT ROBOT:"< 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; } diff --git a/src/footprint.cpp b/src/footprint.cpp index dab07dd..62fec78 100755 --- a/src/footprint.cpp +++ b/src/footprint.cpp @@ -174,15 +174,15 @@ bool makeFootprintFromString(const std::string& footprint_string, std::vector 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 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: " diff --git a/src/layered_costmap.cpp b/src/layered_costmap.cpp index 3125ebb..92b0297 100755 --- a/src/layered_costmap.cpp +++ b/src/layered_costmap.cpp @@ -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; diff --git a/src/observation_buffer.cpp b/src/observation_buffer.cpp index 77d6d6c..fdc1869 100755 --- a/src/observation_buffer.cpp +++ b/src/observation_buffer.cpp @@ -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; diff --git a/test/static_layer_test.cpp b/test/static_layer_test.cpp index 20d91a2..0acc07f 100644 --- a/test/static_layer_test.cpp +++ b/test/static_layer_test.cpp @@ -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));