Stvl distance decay (#324)

* Fix sign

Signed-off-by: Angsa Deployment Team <[email protected]>

* Merge branch 'stvl-distance-clearer-take-2' into jazzy

* Fix order

Signed-off-by: Tony Najjar <[email protected]>

---------

Signed-off-by: Angsa Deployment Team <[email protected]>
Signed-off-by: Tony Najjar <[email protected]>
Co-authored-by: Angsa Deployment Team <[email protected]>

Author: tonynajjar

spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp

@@ -130,15 +130,16 @@ class SpatioTemporalVoxelGrid
     rclcpp::Clock::SharedPtr clock,
     const float & voxel_size, const double & background_value,
     const int & decay_model, const double & voxel_decay,
-    const bool & pub_voxels);
+    const double & voxel_distance_decay, const bool & pub_voxels);
   ~SpatioTemporalVoxelGrid(void);
 
   // 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,
-    std::unordered_set<occupany_cell> & cleared_cells);
+    std::unordered_set<occupany_cell> & cleared_cells,
+    openvdb::Vec3d & robot_pose_world);
 
   // Get the pointcloud of the underlying occupancy grid
   void GetOccupancyPointCloud(std::unique_ptr<sensor_msgs::msg::PointCloud2> & pc2);
@@ -168,7 +169,8 @@ class SpatioTemporalVoxelGrid
     const double & time_delta, const double & acceleration_factor);
   void TemporalClearAndGenerateCostmap(
     std::vector<frustum_model> & frustums,
-    std::unordered_set<occupany_cell> & cleared_cells);
+    std::unordered_set<occupany_cell> & cleared_cells,
+    openvdb::Vec3d & robot_pose_world);
 
   // Populate the costmap ROS api and pointcloud with a marked point
   void PopulateCostmapAndPointcloud(const openvdb::Coord & pt);
@@ -181,7 +183,7 @@ class SpatioTemporalVoxelGrid
 
   mutable openvdb::DoubleGrid::Ptr _grid;
   int _decay_model;
-  double _background_value, _voxel_size, _voxel_decay;
+  double _background_value, _voxel_size, _voxel_decay, _voxel_distance_decay;
   bool _pub_voxels;
   std::unique_ptr<std::vector<geometry_msgs::msg::Point32>> _grid_points;
   std::unordered_map<occupany_cell, uint> * _cost_map;

spatio_temporal_voxel_layer/include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp

@@ -181,7 +181,7 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer
   std::unique_ptr<rclcpp::Duration> _map_save_duration;
   rclcpp::Time _last_map_save_time;
   std::string _global_frame;
-  double _voxel_size, _voxel_decay;
+  double _voxel_size, _voxel_decay, _voxel_distance_decay;
   int _combination_method, _mark_threshold;
   volume_grid::GlobalDecayModel _decay_model;
   bool _update_footprint_enabled, _enabled;

spatio_temporal_voxel_layer/src/spatio_temporal_voxel_grid.cpp

@@ -49,9 +49,11 @@ namespace volume_grid
 SpatioTemporalVoxelGrid::SpatioTemporalVoxelGrid(
   rclcpp::Clock::SharedPtr clock,
   const float & voxel_size, const double & background_value,
-  const int & decay_model, const double & voxel_decay, const bool & pub_voxels)
+  const int & decay_model, const double & voxel_decay, const double & voxel_distance_decay,
+  const bool & pub_voxels)
 : _clock(clock), _decay_model(decay_model), _background_value(background_value),
-  _voxel_size(voxel_size), _voxel_decay(voxel_decay), _pub_voxels(pub_voxels),
+  _voxel_size(voxel_size), _voxel_decay(voxel_decay), _voxel_distance_decay(voxel_distance_decay),
+  _pub_voxels(pub_voxels),
   _grid_points(std::make_unique<std::vector<geometry_msgs::msg::Point32>>()),
   _cost_map(new std::unordered_map<occupany_cell, uint>)
 /*****************************************************************************/
@@ -95,7 +97,7 @@ void SpatioTemporalVoxelGrid::InitializeGrid(void)
 /*****************************************************************************/
 void SpatioTemporalVoxelGrid::ClearFrustums(
   const std::vector<observation::MeasurementReading> & clearing_readings,
-  std::unordered_set<occupany_cell> & cleared_cells)
+  std::unordered_set<occupany_cell> & cleared_cells, openvdb::Vec3d & robot_pose_world)
 /*****************************************************************************/
 {
   boost::unique_lock<boost::mutex> lock(_grid_lock);
@@ -113,7 +115,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums(
   std::vector<frustum_model> obs_frustums;
 
   if (clearing_readings.size() == 0) {
-    TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells);
+    TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells, robot_pose_world);
     return;
   }
 
@@ -142,20 +144,22 @@ void SpatioTemporalVoxelGrid::ClearFrustums(
     frustum->TransformModel();
     obs_frustums.emplace_back(frustum, it->_decay_acceleration);
   }
-  TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells);
+  TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells, robot_pose_world);
 }
 
 /*****************************************************************************/
 void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(
   std::vector<frustum_model> & frustums,
-  std::unordered_set<occupany_cell> & cleared_cells)
+  std::unordered_set<occupany_cell> & cleared_cells,
+  openvdb::Vec3d & robot_pose_world)
 /*****************************************************************************/
 {
   // sample time once for all clearing readings
   const double cur_time = _clock->now().seconds();
 
   // check each point in the grid for inclusion in a frustum
   openvdb::DoubleGrid::ValueOnCIter cit_grid = _grid->cbeginValueOn();
+  double voxel_distance_decay_squared = _voxel_distance_decay * _voxel_distance_decay;
   for (; cit_grid.test(); ++cit_grid) {
     const openvdb::Coord pt_index(cit_grid.getCoord());
     const openvdb::Vec3d pose_world = this->IndexToWorld(pt_index);
@@ -164,6 +168,23 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(
     bool frustum_cycle = false;
     bool cleared_point = false;
 
+    // spatial filtering
+    if (_voxel_distance_decay > 0.0) {
+      // check squared distance from robot to voxel
+      double distance_2d_squared =
+        (pose_world[0] - robot_pose_world[0]) * (pose_world[0] - robot_pose_world[0]) +
+        (pose_world[1] - robot_pose_world[1]) * (pose_world[1] - robot_pose_world[1]);
+
+      if (distance_2d_squared > voxel_distance_decay_squared) {
+        cleared_point = true;
+        if (!this->ClearGridPoint(pt_index)) {
+          std::cout << "Failed to clear point." << std::endl;
+        }
+        cleared_cells.insert(occupany_cell(pose_world[0], pose_world[1]));
+        continue;
+      }
+    }
+
     const double time_since_marking = cur_time - cit_grid.getValue();
     const double base_duration_to_decay = GetTemporalClearingDuration(
       time_since_marking);

spatio_temporal_voxel_layer/src/spatio_temporal_voxel_layer.cpp

@@ -119,6 +119,9 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
   // decay param
   declareParameter("voxel_decay", rclcpp::ParameterValue(-1.0));
   node->get_parameter(name_ + ".voxel_decay", _voxel_decay);
+  // distance decay param
+  declareParameter("voxel_distance_decay", rclcpp::ParameterValue(-1.0));
+  node->get_parameter(name_ + ".voxel_distance_decay", _voxel_distance_decay);
   // whether to map or navigate
   declareParameter("mapping_mode", rclcpp::ParameterValue(false));
   node->get_parameter(name_ + ".mapping_mode", _mapping_mode);
@@ -159,7 +162,7 @@ void SpatioTemporalVoxelLayer::onInitialize(void)
 
   _voxel_grid = std::make_unique<volume_grid::SpatioTemporalVoxelGrid>(
     node->get_clock(), _voxel_size, static_cast<double>(default_value_), _decay_model,
-    _voxel_decay, _publish_voxels);
+    _voxel_decay, _voxel_distance_decay, _publish_voxels);
 
   matchSize();
 
@@ -777,7 +780,10 @@ void SpatioTemporalVoxelLayer::updateBounds(
     should_save = node->now() - _last_map_save_time > *_map_save_duration;
   }
   if (!_mapping_mode) {
-    _voxel_grid->ClearFrustums(clearing_observations, cleared_cells);
+    openvdb::Vec3d robot_pose_world;
+    robot_pose_world[0] = robot_x;
+    robot_pose_world[1] = robot_y;
+    _voxel_grid->ClearFrustums(clearing_observations, cleared_cells, robot_pose_world);
   } else if (should_save) {
     _last_map_save_time = node->now();
     time_t rawtime;