| 30 | namespace elevation_mapping { |
| 31 | |
| 32 | ElevationMap::ElevationMap(ros::NodeHandle nodeHandle) |
| 33 | : nodeHandle_(nodeHandle), |
| 34 | rawMap_({"elevation", "variance", "horizontal_variance_x", "horizontal_variance_y", "horizontal_variance_xy", "color", "time", "lowest_scan_point", "sensor_x_at_lowest_scan", "sensor_y_at_lowest_scan", "sensor_z_at_lowest_scan"}), |
| 35 | fusedMap_({"elevation", "upper_bound", "lower_bound", "color"}), |
| 36 | hasUnderlyingMap_(false), |
| 37 | visibilityCleanupDuration_(0.0) |
| 38 | { |
| 39 | rawMap_.setBasicLayers({"elevation", "variance"}); |
| 40 | fusedMap_.setBasicLayers({"elevation", "upper_bound", "lower_bound"}); |
| 41 | clear(); |
| 42 | |
| 43 | elevationMapRawPublisher_ = nodeHandle_.advertise<grid_map_msgs::GridMap>("elevation_map_raw", 1); |
| 44 | elevationMapFusedPublisher_ = nodeHandle_.advertise<grid_map_msgs::GridMap>("elevation_map", 1); |
| 45 | if (!underlyingMapTopic_.empty()) underlyingMapSubscriber_ = |
| 46 | nodeHandle_.subscribe(underlyingMapTopic_, 1, &ElevationMap::underlyingMapCallback, this); |
| 47 | // TODO if (enableVisibilityCleanup_) when parameter cleanup is ready. |
| 48 | visbilityCleanupMapPublisher_ = nodeHandle_.advertise<grid_map_msgs::GridMap>("visibility_cleanup_map", 1); |
| 49 | |
| 50 | initialTime_ = ros::Time::now(); |
| 51 | } |
| 52 | |
| 53 | ElevationMap::~ElevationMap() |
| 54 | { |
nothing calls this directly
no outgoing calls
no test coverage detected