Skip to content

Commit

Permalink
feat: Clearing expired cells after timeout (#102)
Browse files Browse the repository at this point in the history
  • Loading branch information
renan028 authored Nov 5, 2024
1 parent bd2acbc commit dca95ff
Show file tree
Hide file tree
Showing 3 changed files with 132 additions and 1 deletion.
1 change: 1 addition & 0 deletions costmap_2d/cfg/ObstaclePlugin.cfg
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,7 @@ gen.add("enabled", bool_t, 0, "Whether to apply this plugin or not", True)
gen.add("footprint_clearing_enabled", bool_t, 0, "Whether to clear the robot's footprint of lethal obstacles", True)
gen.add("max_obstacle_height", double_t, 0, "The maximum height of any obstacle to be inserted into the costmap in meters.", 2, 0, 50)
gen.add("raytrace_outside_map", bool_t, 0, "Whether to raytrace when the sensor is outside the map", False)
gen.add("cell_clearing_timeout", double_t, 0, "The time in seconds to clear out cell information in the costmap; if negative, never clear", -1, -1, 1000)

combo_enum = gen.enum([gen.const("Overwrite", int_t, 0, "Overwrite values"),
gen.const("Maximum", int_t, 1, "Take the maximum of the values"),
Expand Down
24 changes: 24 additions & 0 deletions costmap_2d/include/costmap_2d/obstacle_layer.h
Original file line number Diff line number Diff line change
Expand Up @@ -45,6 +45,7 @@

#include <nav_msgs/OccupancyGrid.h>

#include <ros/time.h>
#include <sensor_msgs/LaserScan.h>
#include <laser_geometry/laser_geometry.h>
#include <sensor_msgs/PointCloud.h>
Expand Down Expand Up @@ -125,6 +126,14 @@ class ObstacleLayer : public CostmapLayer
void addStaticObservation(costmap_2d::Observation& obs, bool marking, bool clearing);
void clearStaticObservations(bool marking, bool clearing);

/**
* @brief See costmap_2d::setConvexPolygonCost for details; It updates the cell timeout
* @param polygon The polygon to perform the operation on
* @param cost_value The value to set costs to
* @return True if the polygon was filled... false if it could not be filled
*/
bool setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value);

protected:
virtual void setupDynamicReconfigure(ros::NodeHandle& nh);

Expand Down Expand Up @@ -171,6 +180,18 @@ class ObstacleLayer : public CostmapLayer
double* max_x, double* max_y);

void updateMapPolygon();

/**
* @brief Update the cell timeout for a given index. Cells with a timeout are marked as NO_INFORMATION
* @param index The index of the cell to update
*/
void updateCellTimeout(int index);

/**
* @brief Setup vector with last updated time for each cell
* @param new_cell_clearing_timeout The new cell clearing timeout
*/
void setupCellsTimeout(const double new_cell_clearing_timeout);

std::vector<geometry_msgs::Point> transformed_footprint_;
bool footprint_clearing_enabled_;
Expand Down Expand Up @@ -198,6 +219,9 @@ class ObstacleLayer : public CostmapLayer
bool raytrace_outside_map_;
Polygon map_boundary_;

double cell_clearing_timeout_ = -1;
std::vector<ros::Time> last_updated_;

private:
void reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint32_t level);
};
Expand Down
108 changes: 107 additions & 1 deletion costmap_2d/plugins/obstacle_layer.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -241,6 +241,7 @@ void ObstacleLayer::reconfigureCB(costmap_2d::ObstaclePluginConfig &config, uint
max_obstacle_height_ = config.max_obstacle_height;
combination_method_ = config.combination_method;
raytrace_outside_map_ = config.raytrace_outside_map;
setupCellsTimeout(config.cell_clearing_timeout);
}

void ObstacleLayer::laserScanCallback(const sensor_msgs::LaserScanConstPtr& message,
Expand Down Expand Up @@ -352,6 +353,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
updateOrigin(robot_x - getSizeInMetersX() / 2, robot_y - getSizeInMetersY() / 2);
useExtraBounds(min_x, min_y, max_x, max_y);
updateMapPolygon();
setupCellsTimeout(cell_clearing_timeout_);

bool current = true;
std::vector<Observation> observations, clearing_observations;
Expand All @@ -365,6 +367,24 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya
// update the global current status
current_ = current;

// set all cells that timeout to NO_INFORMATION
if (!rolling_window_ && !last_updated_.empty())
{
for (unsigned int i = 0; i < last_updated_.size(); i++)
{
if (last_updated_[i] + ros::Duration(cell_clearing_timeout_) < ros::Time::now())
{
costmap_[i] = NO_INFORMATION;

double wx, wy;
unsigned int mx, my;
indexToCells(i, mx, my);
mapToWorld(mx, my, wx, wy);
touch(wx, wy, min_x, min_y, max_x, max_y);
}
}
}

// raytrace freespace
for (unsigned int i = 0; i < clearing_observations.size(); ++i)
{
Expand Down Expand Up @@ -416,6 +436,7 @@ void ObstacleLayer::updateBounds(double robot_x, double robot_y, double robot_ya

unsigned int index = getIndex(mx, my);
costmap_[index] = LETHAL_OBSTACLE;
updateCellTimeout(index);
touch(px, py, min_x, min_y, max_x, max_y);
}
}
Expand Down Expand Up @@ -585,8 +606,16 @@ void ObstacleLayer::raytraceFreespace(const Observation& clearing_observation, d

unsigned int cell_raytrace_range = cellDistance(clearing_observation.raytrace_range_);
MarkCell marker(costmap_, FREE_SPACE);

// mark and update last time
const auto action = [&](unsigned int offset)
{
marker(offset);
updateCellTimeout(offset);
};

// and finally... we can execute our trace to clear obstacles along that line
raytraceLine(marker, x0, y0, x1, y1, cell_raytrace_range);
raytraceLine(action, x0, y0, x1, y1, cell_raytrace_range);

updateRaytraceBounds(ox, oy, wx, wy, clearing_observation.raytrace_range_, min_x, min_y, max_x, max_y);
}
Expand Down Expand Up @@ -722,4 +751,81 @@ void ObstacleLayer::reset()
activate();
}

void ObstacleLayer::setupCellsTimeout(const double new_cell_clearing_timeout)
{
// if we're using rolling window, we don't clear cells based on timeout
if (rolling_window_)
{
return;
}

// new parameter is negative, clear all cells (disable timeout)
if (new_cell_clearing_timeout < 0)
{
last_updated_.clear();
return;
}

// check if map's size matches vector; otherwise, resize vector
// we need to check this first, because width and height can change in costmap_2d_ros
if (last_updated_.size() != size_x_ * size_y_)
{
last_updated_.clear();
last_updated_.resize(size_x_ * size_y_, ros::Time::now());
cell_clearing_timeout_ = new_cell_clearing_timeout;
return;
}

// if parameter did not change, return
if (new_cell_clearing_timeout == cell_clearing_timeout_)
{
return;
}

// parameter has changed, create new cell update tracking vector
last_updated_.clear();
last_updated_.resize(size_x_ * size_y_, ros::Time::now());
cell_clearing_timeout_ = new_cell_clearing_timeout;
}


void ObstacleLayer::updateCellTimeout(int index)
{
if (!rolling_window_ && !last_updated_.empty() && index < last_updated_.size())
{
last_updated_[index] = ros::Time::now();
}
}

bool ObstacleLayer::setConvexPolygonCost(const std::vector<geometry_msgs::Point>& polygon, unsigned char cost_value)
{
// copied from costmap_2d/costmap_2d.cpp
// we assume the polygon is given in the global_frame... we need to transform it to map coordinates
std::vector<MapLocation> map_polygon;
for (unsigned int i = 0; i < polygon.size(); ++i)
{
MapLocation loc;
if (!worldToMap(polygon[i].x, polygon[i].y, loc.x, loc.y))
{
// ("Polygon lies outside map bounds, so we can't fill it");
return false;
}
map_polygon.push_back(loc);
}

std::vector<MapLocation> polygon_cells;

// get the cells that fill the polygon
convexFillCells(map_polygon, polygon_cells);

// set the cost of those cells
for (unsigned int i = 0; i < polygon_cells.size(); ++i)
{
unsigned int index = getIndex(polygon_cells[i].x, polygon_cells[i].y);
costmap_[index] = cost_value;
updateCellTimeout(index); // this is the only change
}
return true;
}

} // namespace costmap_2d

0 comments on commit dca95ff

Please sign in to comment.