From d2a7dc5926ddcd5b43fc6ee0ae2281003ae68dc3 Mon Sep 17 00:00:00 2001 From: siferati Date: Sat, 10 Apr 2021 14:45:50 +0100 Subject: [PATCH 01/10] fixes #199 --- .../spatio_temporal_voxel_grid.hpp | 7 ++++-- .../spatio_temporal_voxel_layer.hpp | 3 ++- src/spatio_temporal_voxel_grid.cpp | 24 +++++++++++++----- src/spatio_temporal_voxel_layer.cpp | 25 ++++++++++++------- 4 files changed, 41 insertions(+), 18 deletions(-) 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 #include +#include #include #include #include @@ -131,7 +132,8 @@ class SpatioTemporalVoxelGrid // Core making and clearing functions void Mark(const std::vector& marking_observations); void operator()(const observation::MeasurementReading& obs) const; - void ClearFrustums(const std::vector& clearing_observations); + void ClearFrustums(const std::vector& clearing_observations, \ + std::unordered_set& 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& frustums); + void TemporalClearAndGenerateCostmap(std::vector& frustums, \ + std::unordered_set& 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& 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& clearing_readings) +void SpatioTemporalVoxelGrid::ClearFrustums( \ + const std::vector& clearing_readings, \ + std::unordered_set& updated_cells) /*****************************************************************************/ { boost::unique_lock 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& frustums) + std::vector& frustums, \ + std::unordered_set& 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::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& updated_cells) /*****************************************************************************/ { // grabs map of occupied cells from grid and adds to costmap_ Costmap2D::resetMaps(); - std::unordered_map::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 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) From e482f7e08aeb8a760ebf280c01e9f79321ebd15e Mon Sep 17 00:00:00 2001 From: siferati Date: Wed, 14 Apr 2021 12:24:47 +0100 Subject: [PATCH 02/10] applied requested changes --- .../spatio_temporal_voxel_grid.hpp | 3 +- src/spatio_temporal_voxel_grid.cpp | 57 +++++++++++++------ src/spatio_temporal_voxel_layer.cpp | 24 ++++---- 3 files changed, 54 insertions(+), 30 deletions(-) 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) From 019f1a73dcbb599d88bf29f23c798b67129a1e18 Mon Sep 17 00:00:00 2001 From: siferati Date: Wed, 14 Apr 2021 12:35:28 +0100 Subject: [PATCH 03/10] renamed remaining updated_cells to cleared_cells --- .../spatio_temporal_voxel_grid.hpp | 4 ++-- .../spatio_temporal_voxel_layer.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) 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 2589864a..7fbd0b47 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp @@ -133,7 +133,7 @@ class SpatioTemporalVoxelGrid void Mark(const std::vector& marking_observations); void operator()(const observation::MeasurementReading& obs); void ClearFrustums(const std::vector& clearing_observations, \ - std::unordered_set& updated_cells); + std::unordered_set& cleared_cells); // Get the pointcloud of the underlying occupancy grid void GetOccupancyPointCloud(sensor_msgs::PointCloud2::Ptr& pc2); @@ -161,7 +161,7 @@ class SpatioTemporalVoxelGrid double GetFrustumAcceleration(const double& time_delta, \ const double& acceleration_factor); void TemporalClearAndGenerateCostmap(std::vector& frustums, \ - std::unordered_set& updated_cells); + std::unordered_set& cleared_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 bed1e1f5..917d858c 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp @@ -115,7 +115,7 @@ class SpatioTemporalVoxelLayer : public costmap_2d::CostmapLayer // Functions to interact with maps void UpdateROSCostmap(double* min_x, double* min_y, double* max_x, double* max_y, \ - std::unordered_set& updated_cells); + std::unordered_set& cleared_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); From cd9c081a0eaea9e70506ebba6ce0bd68c749f63b Mon Sep 17 00:00:00 2001 From: siferati Date: Wed, 14 Apr 2021 21:30:46 +0100 Subject: [PATCH 04/10] changed loop to use iterators --- src/spatio_temporal_voxel_layer.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/spatio_temporal_voxel_layer.cpp b/src/spatio_temporal_voxel_layer.cpp index 16ab2490..8f59a054 100644 --- a/src/spatio_temporal_voxel_layer.cpp +++ b/src/spatio_temporal_voxel_layer.cpp @@ -684,9 +684,10 @@ void SpatioTemporalVoxelLayer::UpdateROSCostmap( \ } } - for (const auto& cell: cleared_cells) + std::unordered_set::iterator cell; + for (cell = cleared_cells.begin(); cell != cleared_cells.end(); ++cell) { - touch(cell.x, cell.y, min_x, min_y, max_x, max_y); + touch(cell->x, cell->y, min_x, min_y, max_x, max_y); } } From 2d22cfb9a8ccb0213c4dae6689f6041b1d6221a3 Mon Sep 17 00:00:00 2001 From: siferati Date: Thu, 15 Apr 2021 13:55:30 +0100 Subject: [PATCH 05/10] fixed point cloud density when marking --- .../spatio_temporal_voxel_grid.hpp | 4 +- src/spatio_temporal_voxel_grid.cpp | 40 ++++++------------- 2 files changed, 14 insertions(+), 30 deletions(-) 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 7fbd0b47..a00f71d0 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp @@ -164,8 +164,8 @@ class SpatioTemporalVoxelGrid std::unordered_set& cleared_cells); // Populate the costmap ROS api and pointcloud with a marked point - void PopulateCostmapAndPointcloud(const openvdb::Coord& pt); - void PopulateCostmapAndPointcloud(const openvdb::Vec3d& pose_world); + void PopulateCostmapAndPointcloud(const openvdb::Coord& pt, \ + const bool& pub_every_voxel); // 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 4ef7d786..3adfe8b4 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -237,19 +237,20 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ } } // if here, we can add to costmap and PC2 - PopulateCostmapAndPointcloud(pt_index); + PopulateCostmapAndPointcloud(pt_index, true); } } /*****************************************************************************/ -void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \ - openvdb::Coord& pt) +void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud( \ + const openvdb::Coord& pt, \ + const bool& pub_every_voxel) /*****************************************************************************/ { // add pt to the pointcloud and costmap openvdb::Vec3d pose_world = this->IndexToWorld(pt); - if (_pub_voxels) + if (_pub_voxels && pub_every_voxel) { geometry_msgs::Point32 point; point.x = pose_world[0]; @@ -258,24 +259,6 @@ void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \ _grid_points->push_back(point); } - 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)); - } -} - -/*****************************************************************************/ -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()) @@ -287,7 +270,7 @@ void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \ _cost_map->insert(std::make_pair( \ occupany_cell(pose_world[0], pose_world[1]), 1)); - if (_pub_voxels) + if (_pub_voxels && !pub_every_voxel) { geometry_msgs::Point32 point; point.x = pose_world[0]; @@ -343,12 +326,13 @@ void SpatioTemporalVoxelGrid::operator()(const \ { continue; } - openvdb::Vec3d pose_world(*iter_x, *iter_y, *iter_z); - PopulateCostmapAndPointcloud(pose_world); - openvdb::Vec3d mark_grid(this->WorldToIndex(pose_world)); + openvdb::Vec3d mark_grid(this->WorldToIndex( \ + openvdb::Vec3d(*iter_x, *iter_y, *iter_z))); - if(!this->MarkGridPoint(openvdb::Coord(mark_grid[0], mark_grid[1], \ - mark_grid[2]), cur_time)) + openvdb::Coord pt_index(mark_grid[0], mark_grid[1], mark_grid[2]); + PopulateCostmapAndPointcloud(pt_index, false); + + if(!this->MarkGridPoint(pt_index, cur_time)) { std::cout << "Failed to mark point." << std::endl; } From 651fcd7762fa86f7defa6e20b18ac21933be37c3 Mon Sep 17 00:00:00 2001 From: siferati Date: Fri, 16 Apr 2021 12:58:06 +0100 Subject: [PATCH 06/10] small semantic change --- src/spatio_temporal_voxel_grid.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 3adfe8b4..46134d12 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -250,27 +250,27 @@ void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud( \ // add pt to the pointcloud and costmap openvdb::Vec3d pose_world = this->IndexToWorld(pt); - if (_pub_voxels && pub_every_voxel) - { - geometry_msgs::Point32 point; - point.x = pose_world[0]; - point.y = pose_world[1]; - point.z = pose_world[2]; - _grid_points->push_back(point); - } - 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; + + if (_pub_voxels && pub_every_voxel) + { + geometry_msgs::Point32 point; + point.x = pose_world[0]; + point.y = pose_world[1]; + point.z = pose_world[2]; + _grid_points->push_back(point); + } } else { _cost_map->insert(std::make_pair( \ occupany_cell(pose_world[0], pose_world[1]), 1)); - if (_pub_voxels && !pub_every_voxel) + if (_pub_voxels) { geometry_msgs::Point32 point; point.x = pose_world[0]; From 883a3f0b2d8ab48e28f6e7ced429b7d4bfe91ba8 Mon Sep 17 00:00:00 2001 From: siferati Date: Sat, 8 May 2021 12:08:28 +0100 Subject: [PATCH 07/10] cleared points are no longer added to the costmap --- src/spatio_temporal_voxel_grid.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 46134d12..3018ad25 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -179,6 +179,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ std::vector::iterator frustum_it = frustums.begin(); bool frustum_cycle = false; + bool cleared_point = false; const double time_since_marking = cur_time - cit_grid.getValue(); const double base_duration_to_decay = GetTemporalClearingDuration( \ @@ -203,6 +204,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ if (time_until_decay < 0.) { // expired by acceleration + cleared_point = true; cleared_cells.insert(occupany_cell(pose_world[0], pose_world[1])); if(!this->ClearGridPoint(pt_index)) { @@ -228,16 +230,19 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ if (base_duration_to_decay < 0.) { // expired by temporal clearing + cleared_point = true; cleared_cells.insert(occupany_cell(pose_world[0], pose_world[1])); if(!this->ClearGridPoint(pt_index)) { std::cout << "Failed to clear point." << std::endl; } - continue; } } // if here, we can add to costmap and PC2 - PopulateCostmapAndPointcloud(pt_index, true); + if (!cleared_point) + { + PopulateCostmapAndPointcloud(pt_index, true); + } } } From f443a2b6705b3d8026dac6d6979d9713b82e0195 Mon Sep 17 00:00:00 2001 From: siferati Date: Sat, 8 May 2021 14:07:13 +0100 Subject: [PATCH 08/10] removed duplicate code --- src/spatio_temporal_voxel_grid.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 3018ad25..6b64fc7f 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -205,7 +205,6 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ { // expired by acceleration cleared_point = true; - cleared_cells.insert(occupany_cell(pose_world[0], pose_world[1])); if(!this->ClearGridPoint(pt_index)) { std::cout << "Failed to clear point." << std::endl; @@ -231,16 +230,20 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ { // expired by temporal clearing cleared_point = true; - cleared_cells.insert(occupany_cell(pose_world[0], pose_world[1])); if(!this->ClearGridPoint(pt_index)) { std::cout << "Failed to clear point." << std::endl; } } } - // if here, we can add to costmap and PC2 - if (!cleared_point) + + if (cleared_point) { + cleared_cells.insert(occupany_cell(pose_world[0], pose_world[1])); + } + else + { + // if here, we can add to costmap and PC2 PopulateCostmapAndPointcloud(pt_index, true); } } From 40887453bdd08e6558bc52d538f96b2b14306c2e Mon Sep 17 00:00:00 2001 From: siferati Date: Tue, 11 May 2021 12:42:38 +0100 Subject: [PATCH 09/10] break early from frustum clearing. reverted fix to marking lag --- .../spatio_temporal_voxel_grid.hpp | 3 +- src/spatio_temporal_voxel_grid.cpp | 39 +++++++------------ 2 files changed, 16 insertions(+), 26 deletions(-) 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 a00f71d0..1237a0fd 100644 --- a/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp +++ b/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp @@ -164,8 +164,7 @@ class SpatioTemporalVoxelGrid std::unordered_set& cleared_cells); // Populate the costmap ROS api and pointcloud with a marked point - void PopulateCostmapAndPointcloud(const openvdb::Coord& pt, \ - const bool& pub_every_voxel); + void PopulateCostmapAndPointcloud(const openvdb::Coord& pt); // 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 6b64fc7f..7500fe81 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -209,6 +209,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ { std::cout << "Failed to clear point." << std::endl; } + break; } else { @@ -244,48 +245,38 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \ else { // if here, we can add to costmap and PC2 - PopulateCostmapAndPointcloud(pt_index, true); + PopulateCostmapAndPointcloud(pt_index); } } } /*****************************************************************************/ -void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud( \ - const openvdb::Coord& pt, \ - const bool& pub_every_voxel) +void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \ + openvdb::Coord& pt) /*****************************************************************************/ { // add pt to the pointcloud and costmap - openvdb::Vec3d pose_world = this->IndexToWorld(pt); + openvdb::Vec3d pose_world = _grid->indexToWorld(pt); + + 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); + } 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; - - if (_pub_voxels && pub_every_voxel) - { - geometry_msgs::Point32 point; - point.x = pose_world[0]; - point.y = pose_world[1]; - point.z = pose_world[2]; - _grid_points->push_back(point); - } } 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); - } } } @@ -338,7 +329,7 @@ void SpatioTemporalVoxelGrid::operator()(const \ openvdb::Vec3d(*iter_x, *iter_y, *iter_z))); openvdb::Coord pt_index(mark_grid[0], mark_grid[1], mark_grid[2]); - PopulateCostmapAndPointcloud(pt_index, false); + PopulateCostmapAndPointcloud(pt_index); if(!this->MarkGridPoint(pt_index, cur_time)) { From 5a0764589c63d5d09ee5a84b5913a57524360d32 Mon Sep 17 00:00:00 2001 From: siferati Date: Tue, 11 May 2021 12:54:50 +0100 Subject: [PATCH 10/10] revert fix to marking lag --- .../spatio_temporal_voxel_grid.hpp | 2 +- src/spatio_temporal_voxel_grid.cpp | 8 +++----- 2 files changed, 4 insertions(+), 6 deletions(-) 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 1237a0fd..4ec6ad96 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); + void operator()(const observation::MeasurementReading& obs) const; void ClearFrustums(const std::vector& clearing_observations, \ std::unordered_set& cleared_cells); diff --git a/src/spatio_temporal_voxel_grid.cpp b/src/spatio_temporal_voxel_grid.cpp index 7500fe81..032123d8 100644 --- a/src/spatio_temporal_voxel_grid.cpp +++ b/src/spatio_temporal_voxel_grid.cpp @@ -301,7 +301,7 @@ void SpatioTemporalVoxelGrid::Mark(const \ /*****************************************************************************/ void SpatioTemporalVoxelGrid::operator()(const \ - observation::MeasurementReading& obs) + observation::MeasurementReading& obs) const /*****************************************************************************/ { if (obs._marking) @@ -328,10 +328,8 @@ void SpatioTemporalVoxelGrid::operator()(const \ openvdb::Vec3d mark_grid(this->WorldToIndex( \ openvdb::Vec3d(*iter_x, *iter_y, *iter_z))); - openvdb::Coord pt_index(mark_grid[0], mark_grid[1], mark_grid[2]); - PopulateCostmapAndPointcloud(pt_index); - - if(!this->MarkGridPoint(pt_index, cur_time)) + if(!this->MarkGridPoint(openvdb::Coord(mark_grid[0], mark_grid[1], \ + mark_grid[2]), cur_time)) { std::cout << "Failed to mark point." << std::endl; }