Skip to content

Commit

Permalink
applied requested changes
Browse files Browse the repository at this point in the history
  • Loading branch information
siferati committed Apr 14, 2021
1 parent d2a7dc5 commit e482f7e
Show file tree
Hide file tree
Showing 3 changed files with 54 additions and 30 deletions.
3 changes: 2 additions & 1 deletion include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -131,7 +131,7 @@ class SpatioTemporalVoxelGrid

// Core making and clearing functions
void Mark(const std::vector<observation::MeasurementReading>& marking_observations);
void operator()(const observation::MeasurementReading& obs) const;
void operator()(const observation::MeasurementReading& obs);
void ClearFrustums(const std::vector<observation::MeasurementReading>& clearing_observations, \
std::unordered_set<occupany_cell>& updated_cells);

Expand Down Expand Up @@ -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;
Expand Down
57 changes: 39 additions & 18 deletions src/spatio_temporal_voxel_grid.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -100,7 +100,7 @@ void SpatioTemporalVoxelGrid::InitializeGrid(void)
/*****************************************************************************/
void SpatioTemporalVoxelGrid::ClearFrustums( \
const std::vector<observation::MeasurementReading>& clearing_readings, \
std::unordered_set<occupany_cell>& updated_cells)
std::unordered_set<occupany_cell>& cleared_cells)
/*****************************************************************************/
{
boost::unique_lock<boost::mutex> lock(_grid_lock);
Expand All @@ -118,7 +118,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums( \

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

Expand Down Expand Up @@ -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<frustum_model>& frustums, \
std::unordered_set<occupany_cell>& updated_cells)
std::unordered_set<occupany_cell>& cleared_cells)
/*****************************************************************************/
{
// sample time once for all clearing readings
Expand All @@ -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<frustum_model>::iterator frustum_it = frustums.begin();
bool frustum_cycle = false;
Expand Down Expand Up @@ -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
{
Expand All @@ -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]));
}
}

Expand All @@ -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)
{
Expand All @@ -278,6 +271,33 @@ void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \
}
}

/*****************************************************************************/
void SpatioTemporalVoxelGrid::PopulateCostmapAndPointcloud(const \
openvdb::Vec3d& pose_world)
/*****************************************************************************/
{
std::unordered_map<occupany_cell, uint>::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<observation::MeasurementReading>& marking_readings)
Expand All @@ -299,7 +319,7 @@ void SpatioTemporalVoxelGrid::Mark(const \

/*****************************************************************************/
void SpatioTemporalVoxelGrid::operator()(const \
observation::MeasurementReading& obs) const
observation::MeasurementReading& obs)
/*****************************************************************************/
{
if (obs._marking)
Expand All @@ -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))
Expand Down
24 changes: 13 additions & 11 deletions src/spatio_temporal_voxel_layer.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<volume_grid::occupany_cell>& updated_cells)
std::unordered_set<volume_grid::occupany_cell>& 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<volume_grid::occupany_cell, uint>::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);
}
}
Expand Down Expand Up @@ -720,13 +723,12 @@ void SpatioTemporalVoxelLayer::updateBounds( \
ObservationsResetAfterReading();
current_ = current;

// cells that were updated
std::unordered_set<volume_grid::occupany_cell> updated_cells;
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, updated_cells);
_voxel_grid->ClearFrustums(clearing_observations, cleared_cells);
}
else if (ros::Time::now() - _last_map_save_time > _map_save_duration)
{
Expand All @@ -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)
Expand Down

0 comments on commit e482f7e

Please sign in to comment.