TEST(costmap, testAdjacentToObstacleCanStillMove){
tf::TransformListener tf;
LayeredCostmap layers("frame", false, false);
layers.resizeMap(10, 10, 1, 0, 0);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 2.1, 2.3, 4.1);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
addObservation(olayer, 0, 0, MAX_Z);
layers.updateMap(0,0,0);
Costmap2D* costmap = layers.getCostmap();
//printMap(*costmap);
EXPECT_EQ( LETHAL_OBSTACLE, costmap->getCost( 0, 0 ));
EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 0 ));
EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 2, 0 ));
EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 3, 0 ));
EXPECT_TRUE( INSCRIBED_INFLATED_OBSTACLE > costmap->getCost( 2, 1 ));
EXPECT_EQ( INSCRIBED_INFLATED_OBSTACLE, costmap->getCost( 1, 1 ));
}
/**
* Test specific inflation scenario to ensure we do not set inflated obstacles to be raw obstacles.
*/
TEST(costmap, testInflation2){
tf::TransformListener tf;
LayeredCostmap layers("frame", false, false);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
addStaticLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
// Creat a small L-Shape all at once
addObservation(olayer, 1, 1, MAX_Z);
addObservation(olayer, 2, 1, MAX_Z);
addObservation(olayer, 2, 2, MAX_Z);
layers.updateMap(0,0,0);
Costmap2D* costmap = layers.getCostmap();
//printMap(*costmap);
ASSERT_EQ(costmap->getCost(2, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
ASSERT_EQ(costmap->getCost(3, 3), costmap_2d::INSCRIBED_INFLATED_OBSTACLE);
}
/**
* Test inflation for both static and dynamic obstacles
*/
TEST(costmap, testInflation){
tf::TransformListener tf;
LayeredCostmap layers("frame", false, false);
// Footprint with inscribed radius = 2.1
// circumscribed radius = 3.1
std::vector<Point> polygon = setRadii(layers, 1, 1, 1);
addStaticLayer(layers, tf);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
Costmap2D* costmap = layers.getCostmap();
layers.updateMap(0,0,0);
//printMap(*costmap);
ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE), (unsigned int)20);
ASSERT_EQ(countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)28);
/*/ Iterate over all id's and verify they are obstacles
for(std::vector<unsigned int>::const_iterator it = occupiedCells.begin(); it != occupiedCells.end(); ++it){
unsigned int ind = *it;
unsigned int x, y;
map.indexToCells(ind, x, y);
ASSERT_EQ(find(occupiedCells, map.getIndex(x, y)), true);
ASSERT_EQ(map.getCost(x, y) == costmap_2d::LETHAL_OBSTACLE || map.getCost(x, y) == costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
}*/
addObservation(olayer, 0, 0, 0.4);
layers.updateMap(0,0,0);
// It and its 2 neighbors makes 3 obstacles
ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)51);
// @todo Rewrite
// Add an obstacle at <2,0> which will inflate and refresh to of the other inflated cells
addObservation(olayer, 2, 0);
layers.updateMap(0,0,0);
// Now we expect insertions for it, and 2 more neighbors, but not all 5. Free space will propagate from
// the origin to the target, clearing the point at <0, 0>, but not over-writing the inflation of the obstacle
// at <0, 1>
ASSERT_EQ(countValues(*costmap, LETHAL_OBSTACLE) + countValues(*costmap, INSCRIBED_INFLATED_OBSTACLE), (unsigned int)54);
// Add an obstacle at <1, 9>. This will inflate obstacles around it
addObservation(olayer, 1, 9);
layers.updateMap(0,0,0);
ASSERT_EQ(costmap->getCost(1, 9), LETHAL_OBSTACLE);
ASSERT_EQ(costmap->getCost(0, 9), INSCRIBED_INFLATED_OBSTACLE);
ASSERT_EQ(costmap->getCost(2, 9), INSCRIBED_INFLATED_OBSTACLE);
// Add an obstacle and verify that it over-writes its inflated status
addObservation(olayer, 0, 9);
layers.updateMap(0,0,0);
ASSERT_EQ(costmap->getCost(0, 9), LETHAL_OBSTACLE);
}
void Costmap2D::copyCostmapWindow(const Costmap2D& map, double win_origin_x, double win_origin_y, double win_size_x, double win_size_y){
//check for self windowing
if(this == &map){
ROS_ERROR("Cannot convert this costmap into a window of itself");
return;
}
//clean up old data
deleteMaps();
deleteKernels();
//compute the bounds of our new map
unsigned int lower_left_x, lower_left_y, upper_right_x, upper_right_y;
if(!map.worldToMap(win_origin_x, win_origin_y, lower_left_x, lower_left_y)
|| ! map.worldToMap(win_origin_x + win_size_x, win_origin_y + win_size_y, upper_right_x, upper_right_y)){
ROS_ERROR("Cannot window a map that the window bounds don't fit inside of");
return;
}
size_x_ = upper_right_x - lower_left_x;
size_y_ = upper_right_y - lower_left_y;
resolution_ = map.resolution_;
origin_x_ = win_origin_x;
origin_y_ = win_origin_y;
ROS_DEBUG("ll(%d, %d), ur(%d, %d), size(%d, %d), origin(%.2f, %.2f)",
lower_left_x, lower_left_y, upper_right_x, upper_right_y, size_x_, size_y_, origin_x_, origin_y_);
//initialize our various maps and reset markers for inflation
initMaps(size_x_, size_y_);
//copy the window of the static map and the costmap that we're taking
copyMapRegion(map.costmap_, lower_left_x, lower_left_y, map.size_x_, costmap_, 0, 0, size_x_, size_x_, size_y_);
copyMapRegion(map.static_map_, lower_left_x, lower_left_y, map.size_x_, static_map_, 0, 0, size_x_, size_x_, size_y_);
max_obstacle_range_ = map.max_obstacle_range_;
max_obstacle_height_ = map.max_obstacle_height_;
max_raytrace_range_ = map.max_raytrace_range_;
inscribed_radius_ = map.inscribed_radius_;
circumscribed_radius_ = map.circumscribed_radius_;
inflation_radius_ = map.inflation_radius_;
cell_inscribed_radius_ = map.cell_inscribed_radius_;
cell_circumscribed_radius_ = map.cell_circumscribed_radius_;
cell_inflation_radius_ = map.cell_inflation_radius_;
//set the cost for the circumscribed radius of the robot
circumscribed_cost_lb_ = map.circumscribed_cost_lb_;
weight_ = map.weight_;
//copy the cost and distance kernels
copyKernels(map, cell_inflation_radius_);
}
TrajectoryPlanner::TrajectoryPlanner(WorldModel& world_model,
const Costmap2D& costmap,
std::vector<geometry_msgs::Point> footprint_spec,
double acc_lim_x, double acc_lim_y, double acc_lim_theta,
double sim_time, double sim_granularity,
int vx_samples, int vtheta_samples,
double pdist_scale, double gdist_scale, double occdist_scale,
double heading_lookahead, double oscillation_reset_dist,
double escape_reset_dist, double escape_reset_theta,
bool holonomic_robot,
double max_vel_x, double min_vel_x,
double max_vel_th, double min_vel_th, double min_in_place_vel_th,
double backup_vel,
bool dwa, bool heading_scoring, double heading_scoring_timestep, bool meter_scoring, bool simple_attractor,
vector<double> y_vels, double stop_time_buffer, double sim_period, double angular_sim_granularity)
: path_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()),
goal_map_(costmap.getSizeInCellsX(), costmap.getSizeInCellsY()),
costmap_(costmap),
world_model_(world_model), footprint_spec_(footprint_spec),
sim_time_(sim_time), sim_granularity_(sim_granularity), angular_sim_granularity_(angular_sim_granularity),
vx_samples_(vx_samples), vtheta_samples_(vtheta_samples),
pdist_scale_(pdist_scale), gdist_scale_(gdist_scale), occdist_scale_(occdist_scale),
acc_lim_x_(acc_lim_x), acc_lim_y_(acc_lim_y), acc_lim_theta_(acc_lim_theta),
prev_x_(0), prev_y_(0), escape_x_(0), escape_y_(0), escape_theta_(0), heading_lookahead_(heading_lookahead),
oscillation_reset_dist_(oscillation_reset_dist), escape_reset_dist_(escape_reset_dist),
escape_reset_theta_(escape_reset_theta), holonomic_robot_(holonomic_robot),
max_vel_x_(max_vel_x), min_vel_x_(min_vel_x),
max_vel_th_(max_vel_th), min_vel_th_(min_vel_th), min_in_place_vel_th_(min_in_place_vel_th),
backup_vel_(backup_vel),
dwa_(dwa), heading_scoring_(heading_scoring), heading_scoring_timestep_(heading_scoring_timestep),
simple_attractor_(simple_attractor), y_vels_(y_vels), stop_time_buffer_(stop_time_buffer), sim_period_(sim_period)
{
//the robot is not stuck to begin with
stuck_left = false;
stuck_right = false;
stuck_left_strafe = false;
stuck_right_strafe = false;
rotating_left = false;
rotating_right = false;
strafe_left = false;
strafe_right = false;
escaping_ = false;
final_goal_position_valid_ = false;
}
/** Test for copying a window of a costmap */
TEST(costmap, testWindowCopy){
Costmap2D map(GRID_WIDTH, GRID_HEIGHT, RESOLUTION, 0.0, 0.0, ROBOT_RADIUS, ROBOT_RADIUS, ROBOT_RADIUS,
10.0, MAX_Z, 10.0, 25, MAP_10_BY_10, THRESHOLD);
/*
for(unsigned int i = 0; i < 10; ++i){
for(unsigned int j = 0; j < 10; ++j){
printf("%3d ", map.getCost(i, j));
}
printf("\n");
}
printf("\n");
*/
Costmap2D windowCopy;
//first test that if we try to make a window that is too big, things fail
windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 12.0);
ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)0);
ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)0);
//Next, test that trying to make a map window itself fails
map.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0);
ASSERT_EQ(map.getSizeInCellsX(), (unsigned int)10);
ASSERT_EQ(map.getSizeInCellsY(), (unsigned int)10);
//Next, test that legal input generates the result that we expect
windowCopy.copyCostmapWindow(map, 2.0, 2.0, 6.0, 6.0);
ASSERT_EQ(windowCopy.getSizeInCellsX(), (unsigned int)6);
ASSERT_EQ(windowCopy.getSizeInCellsY(), (unsigned int)6);
//check that we actually get the windo that we expect
for(unsigned int i = 0; i < windowCopy.getSizeInCellsX(); ++i){
for(unsigned int j = 0; j < windowCopy.getSizeInCellsY(); ++j){
ASSERT_EQ(windowCopy.getCost(i, j), map.getCost(i + 2, j + 2));
//printf("%3d ", windowCopy.getCost(i, j));
}
//printf("\n");
}
}
void StaticLayer::incomingMap(const nav_msgs::OccupancyGridConstPtr& new_map)
{
unsigned int size_x = new_map->info.width, size_y = new_map->info.height;
ROS_DEBUG("Received a %d X %d map at %f m/pix", size_x, size_y, new_map->info.resolution);
// resize costmap if size, resolution or origin do not match
Costmap2D* master = layered_costmap_->getCostmap();
if (!layered_costmap_->isRolling() && (master->getSizeInCellsX() != size_x ||
master->getSizeInCellsY() != size_y ||
master->getResolution() != new_map->info.resolution ||
master->getOriginX() != new_map->info.origin.position.x ||
master->getOriginY() != new_map->info.origin.position.y ||
!layered_costmap_->isSizeLocked()))
{
// Update the size of the layered costmap (and all layers, including this one)
ROS_INFO("Resizing costmap to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
layered_costmap_->resizeMap(size_x, size_y, new_map->info.resolution, new_map->info.origin.position.x,
new_map->info.origin.position.y, true);
}
else if (size_x_ != size_x || size_y_ != size_y ||
resolution_ != new_map->info.resolution ||
origin_x_ != new_map->info.origin.position.x ||
origin_y_ != new_map->info.origin.position.y)
{
// only update the size of the costmap stored locally in this layer
ROS_INFO("Resizing static layer to %d X %d at %f m/pix", size_x, size_y, new_map->info.resolution);
resizeMap(size_x, size_y, new_map->info.resolution,
new_map->info.origin.position.x, new_map->info.origin.position.y);
}
unsigned int index = 0;
// initialize the costmap with static data
for (unsigned int i = 0; i < size_y; ++i)
{
for (unsigned int j = 0; j < size_x; ++j)
{
unsigned char value = new_map->data[index];
costmap_[index] = interpretValue(value);
++index;
}
}
map_frame_ = new_map->header.frame_id;
// we have a new map, update full size of map
x_ = y_ = 0;
width_ = size_x_;
height_ = size_y_;
map_received_ = true;
has_updated_data_ = true;
// shutdown the map subscrber if firt_map_only_ flag is on
if (first_map_only_)
{
ROS_INFO("Shutting down the map subscriber. first_map_only flag is on");
map_sub_.shutdown();
}
}
void StaticLayer::matchSize()
{
// If we are using rolling costmap, the static map size is
// unrelated to the size of the layered costmap
if (!layered_costmap_->isRolling())
{
Costmap2D* master = layered_costmap_->getCostmap();
resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
master->getOriginX(), master->getOriginY());
}
}
void indexToWorld(Costmap2D& map, unsigned int index, double& wx, double& wy){
unsigned int mx, my;
map.indexToCells(index, mx, my);
map.mapToWorld(mx, my, wx, wy);
}
unsigned int worldToIndex(Costmap2D& map, double wx, double wy){
unsigned int mx, my;
map.worldToMap(wx, wy, mx, my);
return map.getIndex(mx, my);
}
void CostmapLayer::matchSize()
{
Costmap2D* master = layered_costmap_->getCostmap();
resizeMap(master->getSizeInCellsX(), master->getSizeInCellsY(), master->getResolution(),
master->getOriginX(), master->getOriginY());
}
void ExploreFrontier::findFrontiers(Costmap2DROS& costmap_) {
frontiers_.clear();
Costmap2D costmap;
costmap_.getCostmapCopy(costmap);
int idx;
int w = costmap.getSizeInCellsX();
int h = costmap.getSizeInCellsY();
int size = (w * h);
int pts = 0;
map_.info.width = w;
map_.info.height = h;
map_.set_data_size(size);
map_.info.resolution = costmap.getResolution();
map_.info.origin.position.x = costmap.getOriginX();
map_.info.origin.position.y = costmap.getOriginY();
// Find all frontiers (open cells next to unknown cells).
const unsigned char* map = costmap.getCharMap();
for (idx = 0; idx < size; idx++) {
//get the world point for the index
unsigned int mx, my;
costmap.indexToCells(idx, mx, my);
geometry_msgs::Point p;
costmap.mapToWorld(mx, my, p.x, p.y);
//check if the point has valid potential and is next to unknown space
//bool valid_point = planner_->validPointPotential(p);
//bool valid_point = planner_->computePotential(p) && planner_->validPointPotential(p);
//bool valid_point = (map[idx] < LETHAL_OBSTACLE);
//bool valid_point = (map[idx] < INSCRIBED_INFLATED_OBSTACLE );
bool valid_point = (map[idx] == FREE_SPACE);
if ((valid_point && map) &&
(((idx+1 < size) && (map[idx+1] == NO_INFORMATION)) ||
((idx-1 >= 0) && (map[idx-1] == NO_INFORMATION)) ||
((idx+w < size) && (map[idx+w] == NO_INFORMATION)) ||
((idx-w >= 0) && (map[idx-w] == NO_INFORMATION))))
{
map_.data[idx] = -128;
//ROS_INFO("Candidate Point %d.", pts++ );
} else {
map_.data[idx] = -127;
}
}
// Clean up frontiers detected on separate rows of the map
// I don't know what this means -- Travis.
idx = map_.info.height - 1;
for (unsigned int y=0; y < map_.info.width; y++) {
map_.data[idx] = -127;
idx += map_.info.height;
}
// Group adjoining map_ pixels
int segment_id = 127;
std::vector< std::vector<FrontierPoint> > segments;
for (int i = 0; i < size; i++) {
if (map_.data[i] == -128) {
std::vector<int> neighbors, candidates;
std::vector<FrontierPoint> segment;
neighbors.push_back(i);
int points_in_seg = 0;
unsigned int mx, my;
geometry_msgs::Point p_init, p;
costmap.indexToCells(i, mx, my);
costmap.mapToWorld(mx, my, p_init.x, p_init.y);
// claim all neighbors
while (neighbors.size() > 0) {
int idx = neighbors.back();
neighbors.pop_back();
btVector3 tot(0,0,0);
int c = 0;
if ((idx+1 < size) && (map[idx+1] == NO_INFORMATION)) {
tot += btVector3(1,0,0);
c++;
}
if ((idx-1 >= 0) && (map[idx-1] == NO_INFORMATION)) {
tot += btVector3(-1,0,0);
c++;
}
if ((idx+w < size) && (map[idx+w] == NO_INFORMATION)) {
tot += btVector3(0,1,0);
c++;
}
if ((idx-w >= 0) && (map[idx-w] == NO_INFORMATION)) {
tot += btVector3(0,-1,0);
c++;
}
assert(c > 0);
// Only adjust the map and add idx to segment when its near enough to start point.
// This should prevent greedy approach where all cells belong to one frontier!
costmap.indexToCells(idx, mx, my);
costmap.mapToWorld(mx, my, p.x, p.y);
float dist;
dist = sqrt( pow( p.x - p_init.x, 2 ) + pow( p.y - p_init.y, 2 ));
if ( dist <= 0.8 ){
map_.data[idx] = segment_id; // This used to be up before "assert" block above.
points_in_seg += 1;
//ROS_INFO( "Dist to start cell: %3.2f", dist );
segment.push_back(FrontierPoint(idx, tot / c));
// consider 8 neighborhood
if (((idx-1)>0) && (map_.data[idx-1] == -128))
neighbors.push_back(idx-1);
if (((idx+1)<size) && (map_.data[idx+1] == -128))
neighbors.push_back(idx+1);
if (((idx-map_.info.width)>0) && (map_.data[idx-map_.info.width] == -128))
neighbors.push_back(idx-map_.info.width);
if (((idx-map_.info.width+1)>0) && (map_.data[idx-map_.info.width+1] == -128))
neighbors.push_back(idx-map_.info.width+1);
if (((idx-map_.info.width-1)>0) && (map_.data[idx-map_.info.width-1] == -128))
neighbors.push_back(idx-map_.info.width-1);
if (((idx+(int)map_.info.width)<size) && (map_.data[idx+map_.info.width] == -128))
neighbors.push_back(idx+map_.info.width);
if (((idx+(int)map_.info.width+1)<size) && (map_.data[idx+map_.info.width+1] == -128))
neighbors.push_back(idx+map_.info.width+1);
if (((idx+(int)map_.info.width-1)<size) && (map_.data[idx+map_.info.width-1] == -128))
neighbors.push_back(idx+map_.info.width-1);
}
}
segments.push_back(segment);
ROS_INFO( "Segment %d has %d costmap cells", segment_id, points_in_seg );
segment_id--;
if (segment_id < -127)
break;
}
}
int num_segments = 127 - segment_id;
ROS_INFO("Segments: %d", num_segments );
if (num_segments <= 0)
return;
for (unsigned int i=0; i < segments.size(); i++) {
Frontier frontier;
std::vector<FrontierPoint>& segment = segments[i];
uint size = segment.size();
//we want to make sure that the frontier is big enough for the robot to fit through
// This seems like a goofy heuristic rather than fact. What happens if we don't do this...?
if (size * costmap.getResolution() < costmap.getInscribedRadius()){
ROS_INFO( "Discarding segment... too small?" );
//continue;
}
float x = 0, y = 0;
btVector3 d(0,0,0);
for (uint j=0; j<size; j++) {
d += segment[j].d;
int idx = segment[j].idx;
x += (idx % map_.info.width);
y += (idx / map_.info.width);
// x = (idx % map_.info.width);
// y = (idx / map_.info.width);
}
d = d / size;
frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x / size);
frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y / size);
// frontier.pose.position.x = map_.info.origin.position.x + map_.info.resolution * (x);
// frontier.pose.position.y = map_.info.origin.position.y + map_.info.resolution * (y);
frontier.pose.position.z = 0.0;
frontier.pose.orientation = tf::createQuaternionMsgFromYaw(btAtan2(d.y(), d.x()));
frontier.size = size;
geometry_msgs::Point p;
p.x = map_.info.origin.position.x + map_.info.resolution * (x); // frontier.pose.position
p.y = map_.info.origin.position.y + map_.info.resolution * (y);
if (!planner_->validPointPotential(p)){
ROS_INFO( "Discarding segment... can't reach?" );
//continue;
}
frontiers_.push_back(frontier);
}
}
/**
* Test for the cost function correctness with a larger range and different values
*/
TEST(costmap, testCostFunctionCorrectness){
tf::TransformListener tf;
LayeredCostmap layers("frame", false, false);
layers.resizeMap(100, 100, 1, 0, 0);
// Footprint with inscribed radius = 5.0
// circumscribed radius = 8.0
std::vector<Point> polygon = setRadii(layers, 5.0, 6.25, 10.5);
ObstacleLayer* olayer = addObstacleLayer(layers, tf);
InflationLayer* ilayer = addInflationLayer(layers, tf);
layers.setFootprint(polygon);
addObservation(olayer, 50, 50, MAX_Z);
layers.updateMap(0,0,0);
Costmap2D* map = layers.getCostmap();
// Verify that the circumscribed cost lower bound is as expected: based on the cost function.
//unsigned char c = ilayer->computeCost(8.0);
//ASSERT_EQ(ilayer->getCircumscribedCost(), c);
for(unsigned int i = 0; i <= (unsigned int)ceil(5.0); i++){
// To the right
ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50 + i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
// To the left
ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50 - i, 50) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
// Down
ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50, 50 + i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
// Up
ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
ASSERT_EQ(map->getCost(50, 50 - i) >= costmap_2d::INSCRIBED_INFLATED_OBSTACLE, true);
}
// Verify the normalized cost attenuates as expected
for(unsigned int i = (unsigned int)(ceil(5.0) + 1); i <= (unsigned int)ceil(10.5); i++){
unsigned char expectedValue = ilayer->computeCost(i/1.0);
ASSERT_EQ(map->getCost(50 + i, 50), expectedValue);
}
// Update with no hits. Should clear (revert to the static map
/*map->resetMapOutsideWindow(0, 0, 0.0, 0.0);
cloud.points.resize(0);
p.x = 0.0;
p.y = 0.0;
p.z = MAX_Z;
Observation obs2(p, cloud, 100.0, 100.0);
std::vector<Observation> obsBuf2;
obsBuf2.push_back(obs2);
map->updateWorld(0, 0, obsBuf2, obsBuf2);
for(unsigned int i = 0; i < 100; i++)
for(unsigned int j = 0; j < 100; j++)
ASSERT_EQ(map->getCost(i, j), costmap_2d::FREE_SPACE);*/
}
