| 55 | namespace roborts_costmap { |
| 56 | |
| 57 | void StaticLayer::OnInitialize() { |
| 58 | ros::NodeHandle nh; |
| 59 | is_current_ = true; |
| 60 | ParaStaticLayer para_static_layer; |
| 61 | |
| 62 | std::string static_map = ros::package::getPath("roborts_costmap") + \ |
| 63 | "/config/static_layer_config.prototxt"; |
| 64 | roborts_common::ReadProtoFromTextFile(static_map.c_str(), ¶_static_layer); |
| 65 | global_frame_ = layered_costmap_-> GetGlobalFrameID(); |
| 66 | first_map_only_ = para_static_layer.first_map_only(); |
| 67 | subscribe_to_updates_ = para_static_layer.subscribe_to_updates(); |
| 68 | track_unknown_space_ = para_static_layer.track_unknown_space(); |
| 69 | use_maximum_ = para_static_layer.use_maximum(); |
| 70 | int temp_threshold = para_static_layer.lethal_threshold(); |
| 71 | lethal_threshold_ = std::max(std::min(100, temp_threshold), 0); |
| 72 | trinary_costmap_ = para_static_layer.trinary_map(); |
| 73 | unknown_cost_value_ = para_static_layer.unknown_cost_value(); |
| 74 | map_received_ = false; |
| 75 | bool is_debug_ = para_static_layer.is_debug(); |
| 76 | map_topic_ = para_static_layer.topic_name(); |
| 77 | map_sub_ = nh.subscribe(map_topic_.c_str(), 1, &StaticLayer::InComingMap, this); |
| 78 | ros::Rate temp_rate(10); |
| 79 | while(!map_received_) { |
| 80 | ros::spinOnce(); |
| 81 | temp_rate.sleep(); |
| 82 | } |
| 83 | staic_layer_x_ = staic_layer_y_ = 0; |
| 84 | width_ = size_x_; |
| 85 | height_ = size_y_; |
| 86 | is_enabled_ = true; |
| 87 | has_updated_data_ = true; |
| 88 | } |
| 89 | |
| 90 | void StaticLayer::MatchSize() { |
| 91 | if (!layered_costmap_->IsRolling()) { |
nothing calls this directly
no test coverage detected