Skip to content

Commit

Permalink
cherry-picked melodic fix #200 (#201)
Browse files Browse the repository at this point in the history
  • Loading branch information
Tiago Silva authored May 26, 2021
1 parent 14e7f5e commit 7c06d54
Show file tree
Hide file tree
Showing 4 changed files with 44 additions and 16 deletions.
7 changes: 5 additions & 2 deletions include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,6 +48,7 @@
// STL
#include <math.h>
#include <unordered_map>
#include <unordered_set>
#include <ctime>
#include <iostream>
#include <utility>
Expand Down Expand Up @@ -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>& cleared_cells);

// Get the pointcloud of the underlying occupancy grid
void GetOccupancyPointCloud(sensor_msgs::PointCloud2::Ptr& pc2);
Expand All @@ -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>& cleared_cells);

// Populate the costmap ROS api and pointcloud with a marked point
void PopulateCostmapAndPointcloud(const openvdb::Coord& pt);
Expand Down
3 changes: 2 additions & 1 deletion include/spatio_temporal_voxel_layer/spatio_temporal_voxel_layer.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>& 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);
Expand Down
32 changes: 23 additions & 9 deletions src/spatio_temporal_voxel_grid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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>& cleared_cells)
/*****************************************************************************/
{
boost::unique_lock<boost::mutex> lock(_grid_lock);
Expand All @@ -117,7 +118,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \

if(clearing_readings.size() == 0)
{
TemporalClearAndGenerateCostmap(obs_frustums);
TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells);
return;
}

Expand Down Expand Up @@ -156,13 +157,14 @@ void SpatioTemporalVoxelGrid::ClearFrustums(const \
frustum->TransformModel();
obs_frustums.emplace_back(frustum, it->_decay_acceleration);
}
TemporalClearAndGenerateCostmap(obs_frustums);
TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells);
return;
}

/*****************************************************************************/
void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \
std::vector<frustum_model>& frustums)
std::vector<frustum_model>& frustums, \
std::unordered_set<occupany_cell>& cleared_cells)
/*****************************************************************************/
{
// sample time once for all clearing readings
Expand All @@ -173,9 +175,11 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \
for (cit_grid; cit_grid.test(); ++cit_grid)
{
const openvdb::Coord pt_index(cit_grid.getCoord());
const openvdb::Vec3d pose_world = this->IndexToWorld(pt_index);

std::vector<frustum_model>::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( \
Expand All @@ -188,7 +192,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \
continue;
}

if ( frustum_it->frustum->IsInside(this->IndexToWorld(pt_index)) )
if ( frustum_it->frustum->IsInside(pose_world) )
{
frustum_cycle = true;

Expand All @@ -200,10 +204,12 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \
if (time_until_decay < 0.)
{
// expired by acceleration
cleared_point = true;
if(!this->ClearGridPoint(pt_index))
{
std::cout << "Failed to clear point." << std::endl;
}
break;
}
else
{
Expand All @@ -224,15 +230,23 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap( \
if (base_duration_to_decay < 0.)
{
// expired by temporal clearing
cleared_point = true;
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);

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);
}
}
}

Expand Down
18 changes: 14 additions & 4 deletions src/spatio_temporal_voxel_layer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -679,8 +679,10 @@ 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>& cleared_cells)
/*****************************************************************************/
{
// grabs map of occupied cells from grid and adds to costmap_
Expand All @@ -698,6 +700,12 @@ void SpatioTemporalVoxelLayer::UpdateROSCostmap(double* min_x, double* min_y, \
touch(it->first.x, it->first.y, min_x, min_y, max_x, max_y);
}
}

std::unordered_set<volume_grid::occupany_cell>::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);
}
}

/*****************************************************************************/
Expand Down Expand Up @@ -733,10 +741,12 @@ void SpatioTemporalVoxelLayer::updateBounds( \
ObservationsResetAfterReading();
current_ = current;

std::unordered_set<volume_grid::occupany_cell> cleared_cells;

// navigation mode: clear observations, mapping mode: save maps and publish
if (!_mapping_mode)
{
_voxel_grid->ClearFrustums(clearing_observations);
_voxel_grid->ClearFrustums(clearing_observations, cleared_cells);
}
else if (ros::Time::now() - _last_map_save_time > _map_save_duration)
{
Expand All @@ -757,7 +767,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, cleared_cells);

// publish point cloud in navigation mode
if (_publish_voxels && !_mapping_mode)
Expand Down

0 comments on commit 7c06d54

Please sign in to comment.