Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Applied Melodic fix #200 to ROS2 #203

Merged
merged 1 commit into from
May 26, 2021
Merged

Applied Melodic fix #200 to ROS2 #203

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 7 additions & 2 deletions include/spatio_temporal_voxel_layer/spatio_temporal_voxel_grid.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -43,6 +43,7 @@
// STL
#include <math.h>
#include <unordered_map>
#include <unordered_set>
#include <ctime>
#include <iostream>
#include <utility>
Expand Down Expand Up @@ -135,7 +136,9 @@ 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(std::unique_ptr<sensor_msgs::msg::PointCloud2> & pc2);
Expand All @@ -162,7 +165,9 @@ 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
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -117,7 +117,8 @@ class SpatioTemporalVoxelLayer : public nav2_costmap_2d::CostmapLayer

// Functions to interact with maps
void UpdateROSCostmap(
double * min_x, double * min_y, double * max_x, double * max_y);
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);
Expand Down
30 changes: 22 additions & 8 deletions src/spatio_temporal_voxel_grid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -94,7 +94,8 @@ void SpatioTemporalVoxelGrid::InitializeGrid(void)

/*****************************************************************************/
void SpatioTemporalVoxelGrid::ClearFrustums(
const std::vector<observation::MeasurementReading> & clearing_readings)
const std::vector<observation::MeasurementReading> & clearing_readings,
std::unordered_set<occupany_cell> & cleared_cells)
/*****************************************************************************/
{
boost::unique_lock<boost::mutex> lock(_grid_lock);
Expand All @@ -110,7 +111,7 @@ void SpatioTemporalVoxelGrid::ClearFrustums(
std::vector<frustum_model> obs_frustums;

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

Expand Down Expand Up @@ -139,12 +140,13 @@ void SpatioTemporalVoxelGrid::ClearFrustums(
frustum->TransformModel();
obs_frustums.emplace_back(frustum, it->_decay_acceleration);
}
TemporalClearAndGenerateCostmap(obs_frustums);
TemporalClearAndGenerateCostmap(obs_frustums, cleared_cells);
}

/*****************************************************************************/
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 @@ -154,9 +156,11 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(
openvdb::DoubleGrid::ValueOnCIter cit_grid = _grid->cbeginValueOn();
for (; 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 @@ -167,7 +171,7 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(
continue;
}

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

const double frustum_acceleration = GetFrustumAcceleration(
Expand All @@ -177,9 +181,11 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(
frustum_acceleration;
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 {
const double updated_mark = cit_grid.getValue() -
frustum_acceleration;
Expand All @@ -195,14 +201,22 @@ void SpatioTemporalVoxelGrid::TemporalClearAndGenerateCostmap(
if (!frustum_cycle) {
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
15 changes: 12 additions & 3 deletions src/spatio_temporal_voxel_layer.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -670,7 +670,8 @@ void SpatioTemporalVoxelLayer::updateCosts(

/*****************************************************************************/
void SpatioTemporalVoxelLayer::UpdateROSCostmap(
double * min_x, double * min_y, double * max_x, double * max_y)
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 @@ -688,6 +689,12 @@ void SpatioTemporalVoxelLayer::UpdateROSCostmap(
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 @@ -723,14 +730,16 @@ 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
bool should_save = false;
auto node = node_.lock();
if (_map_save_duration) {
should_save = node->now() - _last_map_save_time > *_map_save_duration;
}
if (!_mapping_mode) {
_voxel_grid->ClearFrustums(clearing_observations);
_voxel_grid->ClearFrustums(clearing_observations, cleared_cells);
} else if (should_save) {
_last_map_save_time = node->now();
time_t rawtime;
Expand All @@ -752,7 +761,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