diff --git a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp index e4deef4d..239d5c42 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp @@ -48,6 +48,7 @@ // STL #include <math.h> #include <unordered_map> +#include <unordered_set> #include <ctime> #include <iostream> #include <utility> @@ -131,7 +132,8 @@ class SpatioTemporalVoxelGrid // Core making and clearing functions void Mark(const std::vector<observation::MeasurementReading>& marking_observations); void operator()(const observation::MeasurementReading& obs) const; - void ClearFrustums(const std::vector<observation::MeasurementReading>& clearing_observations); + void ClearFrustums(const std::vector<observation::MeasurementReading>& clearing_observations, \ + std::unordered_set<occupany_cell>& updated_cells); // Get the pointcloud of the underlying occupancy grid void GetOccupancyPointCloud(sensor_msgs::PointCloud2::Ptr& pc2); @@ -158,7 +160,8 @@ class SpatioTemporalVoxelGrid double GetTemporalClearingDuration(const double& time_delta); double GetFrustumAcceleration(const double& time_delta, \ const double& acceleration_factor); - void TemporalClearAndGenerateCostmap(std::vector<frustum_model>& frustums); + void TemporalClearAndGenerateCostmap(std::vector<frustum_model>& frustums, \ + std::unordered_set<occupany_cell>& updated_cells); // Populate the costmap ROS api and pointcloud with a marked point void PopulateCostmapAndPointcloud(const openvdb::Coord& pt); diff --git a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp index 77ae8bba..bed1e1f5 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp @@ -114,7 +114,8 @@ class SpatioTemporalVoxelLayer : public costmap_2d::CostmapLayer void ObservationsResetAfterReading() const; // Functions to interact with maps - void UpdateROSCostmap(double* min_x, double* min_y, double* max_x, double* max_y); + void UpdateROSCostmap(double* min_x, double* min_y, double* max_x, double* max_y, \ + std::unordered_set<volume_grid::occupany_cell>& updated_cells); bool updateFootprint(double robot_x, double robot_y, double robot_yaw, \ double* min_x, double* min_y, double* max_x, double* max_y); void ResetGrid(void); diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 4a67d2c0..89342419 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -98,8 +98,9 @@ void SpatioTemporalVoxelGrid::InitializeGrid(void) } /*****************************************************************************/ -void SpatioTemporalVoxelGrid::ClearFrustums(const \ - std::vector<observation::MeasurementReading>& clearing_readings) +void SpatioTemporalVoxelGrid::ClearFrustums( \ + const std::vector<observation::MeasurementReading>& clearing_readings, \ + std::unordered_set<occupany_cell>& updated_cells) /*****************************************************************************/ { boost::unique_lock<boost::mutex> lock(_grid_lock); @@ -117,7 +118,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ if(clearing_readings.size() == 0) { - TemporalClearAndGenerateCostmap(obs_frustums); + TemporalClearAndGenerateCostmap(obs_frustums, updated_cells); return; } @@ -156,13 +157,14 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \ frustum->TransformModel(); obs_frustums.emplace_back(frustum, it->_decay_acceleration); } - TemporalClearAndGenerateCostmap(obs_frustums); + TemporalClearAndGenerateCostmap(obs_frustums, updated_cells); return; } /*****************************************************************************/ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ - std::vector<frustum_model>& frustums) + std::vector<frustum_model>& frustums, \ + std::unordered_set<occupany_cell>& updated_cells) /*****************************************************************************/ { // sample time once for all clearing readings @@ -173,6 +175,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ for (cit_grid; cit_grid.test(); ++cit_grid) { const openvdb::Coord pt_index(cit_grid.getCoord()); + const openvdb::Vec3d pose_world = _grid->indexToWorld(pt_index); std::vector<frustum_model>::iterator frustum_it = frustums.begin(); bool frustum_cycle = false; @@ -188,7 +191,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ continue; } - if ( frustum_it->frustum->IsInside(this->IndexToWorld(pt_index)) ) + if ( frustum_it->frustum->IsInside(pose_world) ) { frustum_cycle = true; @@ -204,6 +207,10 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ { std::cout << "Failed to clear point." << std::endl; } + else + { + updated_cells.insert(occupany_cell(pose_world[0], pose_world[1])); + } } else { @@ -228,11 +235,16 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ { std::cout << "Failed to clear point." << std::endl; } + else + { + updated_cells.insert(occupany_cell(pose_world[0], pose_world[1])); + } continue; } } // if here, we can add to costmap and PC2 PopulateCostmapAndPointcloud(pt_index); + updated_cells.insert(occupany_cell(pose_world[0], pose_world[1])); } } diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index 72d36ca7..172b2a36 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -662,24 +662,28 @@ void SpatioTemporalVoxelLayer::updateCosts( \ } /*****************************************************************************/ -void SpatioTemporalVoxelLayer::UpdateROSCostmap(double* min_x, double* min_y, \ - double* max_x, double* max_y) +void SpatioTemporalVoxelLayer::UpdateROSCostmap( \ + double* min_x, double* min_y, \ + double* max_x, double* max_y, \ + std::unordered_set<volume_grid::occupany_cell>& updated_cells) /*****************************************************************************/ { // grabs map of occupied cells from grid and adds to costmap_ Costmap2D::resetMaps(); - std::unordered_map<volume_grid::occupany_cell, uint>::iterator it; - for (it = _voxel_grid->GetFlattenedCostmap()->begin(); - it != _voxel_grid->GetFlattenedCostmap()->end(); ++it) + auto cost_map = _voxel_grid->GetFlattenedCostmap(); + + for (const auto& cell: updated_cells) { + auto it = cost_map->find(cell); uint map_x, map_y; - if ( it->second >= _mark_threshold && \ + if ( it != cost_map->end() && \ + it->second >= _mark_threshold && \ worldToMap(it->first.x, it->first.y, map_x, map_y)) { costmap_[getIndex(map_x, map_y)] = costmap_2d::LETHAL_OBSTACLE; - touch(it->first.x, it->first.y, min_x, min_y, max_x, max_y); } + touch(cell.x, cell.y, min_x, min_y, max_x, max_y); } } @@ -716,10 +720,13 @@ void SpatioTemporalVoxelLayer::updateBounds( \ ObservationsResetAfterReading(); current_ = current; + // cells that were updated + std::unordered_set<volume_grid::occupany_cell> updated_cells; + // navigation mode: clear observations, mapping mode: save maps and publish if (!_mapping_mode) { - _voxel_grid->ClearFrustums(clearing_observations); + _voxel_grid->ClearFrustums(clearing_observations, updated_cells); } else if (ros::Time::now() - _last_map_save_time > _map_save_duration) { @@ -740,7 +747,7 @@ void SpatioTemporalVoxelLayer::updateBounds( \ _voxel_grid->Mark(marking_observations); // update the ROS Layered Costmap - UpdateROSCostmap(min_x, min_y, max_x, max_y); + UpdateROSCostmap(min_x, min_y, max_x, max_y, updated_cells); // publish point cloud in navigation mode if (_publish_voxels && !_mapping_mode)