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 239d5c42..2589864a 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp @@ -131,7 +131,7 @@ class SpatioTemporalVoxelGrid // Core making and clearing functions void Mark(const std::vector& marking_observations); - void operator()(const observation::MeasurementReading& obs) const; + void operator()(const observation::MeasurementReading& obs); void ClearFrustums(const std::vector& clearing_observations, \ std::unordered_set& updated_cells); @@ -165,6 +165,7 @@ class SpatioTemporalVoxelGrid // Populate the costmap ROS api and pointcloud with a marked point void PopulateCostmapAndPointcloud(const openvdb::Coord& pt); + void PopulateCostmapAndPointcloud(const openvdb::Vec3d& pose_world); // Utilities for tranformation openvdb::Vec3d WorldToIndex(const openvdb::Vec3d& coord) const; diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 89342419..4ef7d786 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -100,7 +100,7 @@ void SpatioTemporalVoxelGrid::InitializeGrid(void) /*****************************************************************************/ void SpatioTemporalVoxelGrid::ClearFrustums( \ const std::vector& clearing_readings, \ - std::unordered_set& updated_cells) + std::unordered_set& cleared_cells) /*****************************************************************************/ { boost::unique_lock lock(_grid_lock); @@ -118,7 +118,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums( \ if(clearing_readings.size() == 0) { - TemporalClearAndGenerateCostmap(obs_frustums, updated_cells); + TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells); return; } @@ -157,14 +157,14 @@ void SpatioTemporalVoxelGrid::ClearFrustums( \ frustum->TransformModel(); obs_frustums.emplace_back(frustum, it->_decay_acceleration); } - TemporalClearAndGenerateCostmap(obs_frustums, updated_cells); + TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells); return; } /*****************************************************************************/ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ std::vector& frustums, \ - std::unordered_set& updated_cells) + std::unordered_set& cleared_cells) /*****************************************************************************/ { // sample time once for all clearing readings @@ -175,7 +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); + const openvdb::Vec3d pose_world = this->IndexToWorld(pt_index); std::vector::iterator frustum_it = frustums.begin(); bool frustum_cycle = false; @@ -203,14 +203,11 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ if (time_until_decay < 0.) { // expired by acceleration + cleared_cells.insert(occupany_cell(pose_world[0], pose_world[1])); if(!this->ClearGridPoint(pt_index)) { std::cout << "Failed to clear point." << std::endl; } - else - { - updated_cells.insert(occupany_cell(pose_world[0], pose_world[1])); - } } else { @@ -231,20 +228,16 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ if (base_duration_to_decay < 0.) { // expired by temporal clearing + cleared_cells.insert(occupany_cell(pose_world[0], pose_world[1])); if(!this->ClearGridPoint(pt_index)) { 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])); } } @@ -254,7 +247,7 @@ void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \ /*****************************************************************************/ { // add pt to the pointcloud and costmap - openvdb::Vec3d pose_world = _grid->indexToWorld(pt); + openvdb::Vec3d pose_world = this->IndexToWorld(pt); if (_pub_voxels) { @@ -278,6 +271,33 @@ void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \ } } +/*****************************************************************************/ +void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \ + openvdb::Vec3d& pose_world) +/*****************************************************************************/ +{ + std::unordered_map::iterator cell; + cell = _cost_map->find(occupany_cell(pose_world[0], pose_world[1])); + if (cell != _cost_map->end()) + { + cell->second += 1; + } + else + { + _cost_map->insert(std::make_pair( \ + occupany_cell(pose_world[0], pose_world[1]), 1)); + + if (_pub_voxels) + { + geometry_msgs::Point32 point; + point.x = pose_world[0]; + point.y = pose_world[1]; + point.z = pose_world[2]; + _grid_points->push_back(point); + } + } +} + /*****************************************************************************/ void SpatioTemporalVoxelGrid::Mark(const \ std::vector& marking_readings) @@ -299,7 +319,7 @@ void SpatioTemporalVoxelGrid::Mark(const \ /*****************************************************************************/ void SpatioTemporalVoxelGrid::operator()(const \ - observation::MeasurementReading& obs) const + observation::MeasurementReading& obs) /*****************************************************************************/ { if (obs._marking) @@ -323,8 +343,9 @@ void SpatioTemporalVoxelGrid::operator()(const \ { continue; } - openvdb::Vec3d mark_grid(this->WorldToIndex( \ - openvdb::Vec3d(*iter_x, *iter_y, *iter_z))); + openvdb::Vec3d pose_world(*iter_x, *iter_y, *iter_z); + PopulateCostmapAndPointcloud(pose_world); + openvdb::Vec3d mark_grid(this->WorldToIndex(pose_world)); if(!this->MarkGridPoint(openvdb::Coord(mark_grid[0], mark_grid[1], \ mark_grid[2]), cur_time)) diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index 172b2a36..16ab2490 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -665,24 +665,27 @@ void SpatioTemporalVoxelLayer::updateCosts( \ void SpatioTemporalVoxelLayer::UpdateROSCostmap( \ double* min_x, double* min_y, \ double* max_x, double* max_y, \ - std::unordered_set& updated_cells) + std::unordered_set& cleared_cells) /*****************************************************************************/ { // grabs map of occupied cells from grid and adds to costmap_ Costmap2D::resetMaps(); - auto cost_map = _voxel_grid->GetFlattenedCostmap(); - - for (const auto& cell: updated_cells) + std::unordered_map::iterator it; + for (it = _voxel_grid->GetFlattenedCostmap()->begin(); + it != _voxel_grid->GetFlattenedCostmap()->end(); ++it) { - auto it = cost_map->find(cell); uint map_x, map_y; - if ( it != cost_map->end() && \ - it->second >= _mark_threshold && \ + if ( 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); } + } + + for (const auto& cell: cleared_cells) + { touch(cell.x, cell.y, min_x, min_y, max_x, max_y); } } @@ -720,13 +723,12 @@ void SpatioTemporalVoxelLayer::updateBounds( \ ObservationsResetAfterReading(); current_ = current; - // cells that were updated - std::unordered_set updated_cells; + std::unordered_set cleared_cells; // navigation mode: clear observations, mapping mode: save maps and publish if (!_mapping_mode) { - _voxel_grid->ClearFrustums(clearing_observations, updated_cells); + _voxel_grid->ClearFrustums(clearing_observations, cleared_cells); } else if (ros::Time::now() - _last_map_save_time > _map_save_duration) { @@ -747,7 +749,7 @@ void SpatioTemporalVoxelLayer::updateBounds( \ _voxel_grid->Mark(marking_observations); // update the ROS Layered Costmap - UpdateROSCostmap(min_x, min_y, max_x, max_y, updated_cells); + UpdateROSCostmap(min_x, min_y, max_x, max_y, cleared_cells); // publish point cloud in navigation mode if (_publish_voxels && !_mapping_mode)