| 55 | namespace roborts_costmap { |
| 56 | |
| 57 | CostmapInterface::CostmapInterface(std::string map_name, |
| 58 | tf::TransformListener &tf, |
| 59 | std::string config_file) : |
| 60 | layered_costmap_(nullptr), |
| 61 | name_(map_name), |
| 62 | tf_(tf), |
| 63 | config_file_(config_file), |
| 64 | stop_updates_(false), |
| 65 | initialized_(true), |
| 66 | stopped_(false), |
| 67 | robot_stopped_(false), |
| 68 | map_update_thread_(NULL), |
| 69 | last_publish_(0), |
| 70 | dist_behind_robot_threshold_to_care_obstacles_(0.05), |
| 71 | is_debug_(false), |
| 72 | map_update_thread_shutdown_(false) { |
| 73 | std::string tf_error; |
| 74 | ros::NodeHandle private_nh(map_name); |
| 75 | LoadParameter(); |
| 76 | layered_costmap_ = new CostmapLayers(global_frame_, is_rolling_window_, is_track_unknown_); |
| 77 | layered_costmap_->SetFilePath(config_file_inflation_); |
| 78 | ros::Time last_error = ros::Time::now(); |
| 79 | while (ros::ok() && !tf_.waitForTransform(global_frame_, robot_base_frame_, ros::Time(), ros::Duration(0.1), \ |
| 80 | ros::Duration(0.01), &tf_error)) { |
| 81 | ros::spinOnce(); |
| 82 | if (last_error + ros::Duration(5.0) < ros::Time::now()) { |
| 83 | last_error = ros::Time::now(); |
| 84 | } |
| 85 | tf_error.clear(); |
| 86 | } |
| 87 | if (has_static_layer_) { |
| 88 | Layer *plugin_static_layer; |
| 89 | plugin_static_layer = new StaticLayer; |
| 90 | layered_costmap_->AddPlugin(plugin_static_layer); |
| 91 | plugin_static_layer->Initialize(layered_costmap_, map_name + "/" + "static_layer", &tf_); |
| 92 | } |
| 93 | if (has_obstacle_layer_) { |
| 94 | Layer *plugin_obstacle_layer = new ObstacleLayer; |
| 95 | layered_costmap_->AddPlugin(plugin_obstacle_layer); |
| 96 | plugin_obstacle_layer->Initialize(layered_costmap_, map_name + "/" + "obstacle_layer", &tf_); |
| 97 | } |
| 98 | Layer *plugin_inflation_layer = new InflationLayer; |
| 99 | layered_costmap_->AddPlugin(plugin_inflation_layer); |
| 100 | plugin_inflation_layer->Initialize(layered_costmap_, map_name + "/" + "inflation_layer", &tf_); |
| 101 | SetUnpaddedRobotFootprint(footprint_points_); |
| 102 | stop_updates_ = false; |
| 103 | initialized_ = true; |
| 104 | stopped_ = false; |
| 105 | robot_stopped_ = false; |
| 106 | map_update_thread_shutdown_ = false; |
| 107 | timer_ = private_nh.createTimer(ros::Duration(0.1), &CostmapInterface::DetectMovement, this); |
| 108 | if (is_debug_) { |
| 109 | // special values: |
| 110 | cost_translation_table_[0] = 0; // NO obstacle |
| 111 | cost_translation_table_[253] = 99; // INSCRIBED obstacle |
| 112 | cost_translation_table_[254] = 100; // LETHAL obstacle |
| 113 | cost_translation_table_[255] = -1; // UNKNOWN |
| 114 |
nothing calls this directly
no test coverage detected