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