git commit -m "first commit"
This commit is contained in:
145
navigations/base_local_planner/test/footprint_helper_test.cpp
Executable file
145
navigations/base_local_planner/test/footprint_helper_test.cpp
Executable file
@@ -0,0 +1,145 @@
|
||||
/*
|
||||
* footprint_helper_test.cpp
|
||||
*
|
||||
* Created on: May 2, 2012
|
||||
* Author: tkruse
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <base_local_planner/footprint_helper.h>
|
||||
|
||||
#include <base_local_planner/map_grid.h>
|
||||
#include <base_local_planner/costmap_model.h>
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
|
||||
#include "wavefront_map_accessor.h"
|
||||
|
||||
namespace base_local_planner {
|
||||
|
||||
|
||||
class FootprintHelperTest : public testing::Test {
|
||||
public:
|
||||
FootprintHelper fh;
|
||||
|
||||
FootprintHelperTest() {
|
||||
}
|
||||
|
||||
virtual void TestBody(){}
|
||||
|
||||
void correctLineCells() {
|
||||
std::vector<base_local_planner::Position2DInt> footprint;
|
||||
fh.getLineCells(0, 10, 0, 10, footprint);
|
||||
EXPECT_EQ(11, footprint.size());
|
||||
EXPECT_EQ(footprint[0].x, 0);
|
||||
EXPECT_EQ(footprint[0].y, 0);
|
||||
EXPECT_EQ(footprint[5].x, 5);
|
||||
EXPECT_EQ(footprint[5].y, 5);
|
||||
EXPECT_EQ(footprint[10].x, 10);
|
||||
EXPECT_EQ(footprint[10].y, 10);
|
||||
}
|
||||
|
||||
void correctFootprint(){
|
||||
MapGrid* mg = new MapGrid (10, 10);
|
||||
WavefrontMapAccessor* wa = new WavefrontMapAccessor(mg, .25);
|
||||
const costmap_2d::Costmap2D& map = *wa;
|
||||
|
||||
std::vector<geometry_msgs::Point> footprint_spec;
|
||||
geometry_msgs::Point pt;
|
||||
//create a square footprint
|
||||
pt.x = 2;
|
||||
pt.y = 2;
|
||||
footprint_spec.push_back(pt);
|
||||
pt.x = 2;
|
||||
pt.y = -2;
|
||||
footprint_spec.push_back(pt);
|
||||
pt.x = -2;
|
||||
pt.y = -2;
|
||||
footprint_spec.push_back(pt);
|
||||
pt.x = -2;
|
||||
pt.y = 2;
|
||||
footprint_spec.push_back(pt);
|
||||
|
||||
Eigen::Vector3f pos(4.5, 4.5, 0);
|
||||
//just create a basic footprint
|
||||
std::vector<base_local_planner::Position2DInt> footprint = fh.getFootprintCells(pos, footprint_spec, map, false);
|
||||
|
||||
EXPECT_EQ(20, footprint.size());
|
||||
//we expect the front line to be first
|
||||
EXPECT_EQ(footprint[0].x, 6); EXPECT_EQ(footprint[0].y, 6);
|
||||
EXPECT_EQ(footprint[1].x, 6); EXPECT_EQ(footprint[1].y, 5);
|
||||
EXPECT_EQ(footprint[2].x, 6); EXPECT_EQ(footprint[2].y, 4);
|
||||
EXPECT_EQ(footprint[3].x, 6); EXPECT_EQ(footprint[3].y, 3);
|
||||
EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 2);
|
||||
|
||||
//next the right line
|
||||
EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 2);
|
||||
EXPECT_EQ(footprint[6].x, 5); EXPECT_EQ(footprint[6].y, 2);
|
||||
EXPECT_EQ(footprint[7].x, 4); EXPECT_EQ(footprint[7].y, 2);
|
||||
EXPECT_EQ(footprint[8].x, 3); EXPECT_EQ(footprint[8].y, 2);
|
||||
EXPECT_EQ(footprint[9].x, 2); EXPECT_EQ(footprint[9].y, 2);
|
||||
|
||||
//next the back line
|
||||
EXPECT_EQ(footprint[10].x, 2); EXPECT_EQ(footprint[10].y, 2);
|
||||
EXPECT_EQ(footprint[11].x, 2); EXPECT_EQ(footprint[11].y, 3);
|
||||
EXPECT_EQ(footprint[12].x, 2); EXPECT_EQ(footprint[12].y, 4);
|
||||
EXPECT_EQ(footprint[13].x, 2); EXPECT_EQ(footprint[13].y, 5);
|
||||
EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 6);
|
||||
|
||||
//finally the left line
|
||||
EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 6);
|
||||
EXPECT_EQ(footprint[16].x, 3); EXPECT_EQ(footprint[16].y, 6);
|
||||
EXPECT_EQ(footprint[17].x, 4); EXPECT_EQ(footprint[17].y, 6);
|
||||
EXPECT_EQ(footprint[18].x, 5); EXPECT_EQ(footprint[18].y, 6);
|
||||
EXPECT_EQ(footprint[19].x, 6); EXPECT_EQ(footprint[19].y, 6);
|
||||
|
||||
|
||||
pos = Eigen::Vector3f(4.5, 4.5, M_PI_2);
|
||||
//check that rotation of the footprint works
|
||||
footprint = fh.getFootprintCells(pos, footprint_spec, map, false);
|
||||
|
||||
//first the left line
|
||||
EXPECT_EQ(footprint[0].x, 2); EXPECT_EQ(footprint[0].y, 6);
|
||||
EXPECT_EQ(footprint[1].x, 3); EXPECT_EQ(footprint[1].y, 6);
|
||||
EXPECT_EQ(footprint[2].x, 4); EXPECT_EQ(footprint[2].y, 6);
|
||||
EXPECT_EQ(footprint[3].x, 5); EXPECT_EQ(footprint[3].y, 6);
|
||||
EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 6);
|
||||
|
||||
//next the front line
|
||||
EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 6);
|
||||
EXPECT_EQ(footprint[6].x, 6); EXPECT_EQ(footprint[6].y, 5);
|
||||
EXPECT_EQ(footprint[7].x, 6); EXPECT_EQ(footprint[7].y, 4);
|
||||
EXPECT_EQ(footprint[8].x, 6); EXPECT_EQ(footprint[8].y, 3);
|
||||
EXPECT_EQ(footprint[9].x, 6); EXPECT_EQ(footprint[9].y, 2);
|
||||
|
||||
//next the right line
|
||||
EXPECT_EQ(footprint[10].x, 6); EXPECT_EQ(footprint[10].y, 2);
|
||||
EXPECT_EQ(footprint[11].x, 5); EXPECT_EQ(footprint[11].y, 2);
|
||||
EXPECT_EQ(footprint[12].x, 4); EXPECT_EQ(footprint[12].y, 2);
|
||||
EXPECT_EQ(footprint[13].x, 3); EXPECT_EQ(footprint[13].y, 2);
|
||||
EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 2);
|
||||
|
||||
//next the back line
|
||||
EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 2);
|
||||
EXPECT_EQ(footprint[16].x, 2); EXPECT_EQ(footprint[16].y, 3);
|
||||
EXPECT_EQ(footprint[17].x, 2); EXPECT_EQ(footprint[17].y, 4);
|
||||
EXPECT_EQ(footprint[18].x, 2); EXPECT_EQ(footprint[18].y, 5);
|
||||
EXPECT_EQ(footprint[19].x, 2); EXPECT_EQ(footprint[19].y, 6);
|
||||
}
|
||||
|
||||
};
|
||||
|
||||
|
||||
TEST(FootprintHelperTest, correctFootprint){
|
||||
FootprintHelperTest tct;
|
||||
tct.correctFootprint();
|
||||
}
|
||||
|
||||
TEST(FootprintHelperTest, correctLineCells){
|
||||
FootprintHelperTest tct;
|
||||
tct.correctLineCells();
|
||||
}
|
||||
|
||||
}
|
||||
18
navigations/base_local_planner/test/gtest_main.cpp
Executable file
18
navigations/base_local_planner/test/gtest_main.cpp
Executable file
@@ -0,0 +1,18 @@
|
||||
/*
|
||||
* gtest_main.cpp
|
||||
*
|
||||
* Created on: Apr 6, 2012
|
||||
* Author: tkruse
|
||||
*/
|
||||
|
||||
#include <iostream>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
int main(int argc, char **argv) {
|
||||
std::cout << "Running main() from gtest_main.cc\n";
|
||||
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
|
||||
81
navigations/base_local_planner/test/line_iterator_test.cpp
Executable file
81
navigations/base_local_planner/test/line_iterator_test.cpp
Executable file
@@ -0,0 +1,81 @@
|
||||
/*
|
||||
* Copyright (c) 2012, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
* * Neither the name of the Willow Garage, Inc. nor the names of its
|
||||
* contributors may be used to endorse or promote products derived from
|
||||
* this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
|
||||
* AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
|
||||
* IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
|
||||
* ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
|
||||
* LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
|
||||
* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
|
||||
* SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
|
||||
* INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
|
||||
* CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
|
||||
* ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include "base_local_planner/line_iterator.h"
|
||||
|
||||
TEST( LineIterator, south )
|
||||
{
|
||||
base_local_planner::LineIterator line( 1, 2, 1, 4 );
|
||||
EXPECT_TRUE( line.isValid() );
|
||||
EXPECT_EQ( 1, line.getX() );
|
||||
EXPECT_EQ( 2, line.getY() );
|
||||
line.advance();
|
||||
EXPECT_TRUE( line.isValid() );
|
||||
EXPECT_EQ( 1, line.getX() );
|
||||
EXPECT_EQ( 3, line.getY() );
|
||||
line.advance();
|
||||
EXPECT_TRUE( line.isValid() );
|
||||
EXPECT_EQ( 1, line.getX() );
|
||||
EXPECT_EQ( 4, line.getY() );
|
||||
line.advance();
|
||||
EXPECT_FALSE( line.isValid() );
|
||||
}
|
||||
|
||||
TEST( LineIterator, north_north_west )
|
||||
{
|
||||
base_local_planner::LineIterator line( 0, 0, -2, -4 );
|
||||
EXPECT_TRUE( line.isValid() );
|
||||
EXPECT_EQ( 0, line.getX() );
|
||||
EXPECT_EQ( 0, line.getY() );
|
||||
line.advance();
|
||||
EXPECT_TRUE( line.isValid() );
|
||||
EXPECT_EQ( -1, line.getX() );
|
||||
EXPECT_EQ( -1, line.getY() );
|
||||
line.advance();
|
||||
EXPECT_TRUE( line.isValid() );
|
||||
EXPECT_EQ( -1, line.getX() );
|
||||
EXPECT_EQ( -2, line.getY() );
|
||||
line.advance();
|
||||
EXPECT_TRUE( line.isValid() );
|
||||
EXPECT_EQ( -2, line.getX() );
|
||||
EXPECT_EQ( -3, line.getY() );
|
||||
line.advance();
|
||||
EXPECT_TRUE( line.isValid() );
|
||||
EXPECT_EQ( -2, line.getX() );
|
||||
EXPECT_EQ( -4, line.getY() );
|
||||
line.advance();
|
||||
EXPECT_FALSE( line.isValid() );
|
||||
}
|
||||
|
||||
int main( int argc, char **argv ) {
|
||||
testing::InitGoogleTest( &argc, argv );
|
||||
return RUN_ALL_TESTS();
|
||||
}
|
||||
206
navigations/base_local_planner/test/map_grid_test.cpp
Executable file
206
navigations/base_local_planner/test/map_grid_test.cpp
Executable file
@@ -0,0 +1,206 @@
|
||||
/*
|
||||
* map_grid_test.cpp
|
||||
*
|
||||
* Created on: May 2, 2012
|
||||
* Author: tkruse
|
||||
*/
|
||||
#include <queue>
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <base_local_planner/map_grid.h>
|
||||
#include <base_local_planner/map_cell.h>
|
||||
|
||||
#include "wavefront_map_accessor.h"
|
||||
|
||||
namespace base_local_planner {
|
||||
|
||||
TEST(MapGridTest, initNull){
|
||||
MapGrid map_grid;
|
||||
EXPECT_EQ(0, map_grid.size_x_);
|
||||
EXPECT_EQ(0, map_grid.size_y_);
|
||||
}
|
||||
|
||||
TEST(MapGridTest, operatorBrackets){
|
||||
MapGrid map_grid(10, 10);
|
||||
map_grid(3, 5).target_dist = 5;
|
||||
EXPECT_EQ(5, map_grid.getCell(3, 5).target_dist);
|
||||
}
|
||||
|
||||
TEST(MapGridTest, copyConstructor){
|
||||
MapGrid map_grid(10, 10);
|
||||
map_grid(3, 5).target_dist = 5;
|
||||
MapGrid map_grid2;
|
||||
map_grid2 = map_grid;
|
||||
EXPECT_EQ(5, map_grid(3, 5).target_dist);
|
||||
}
|
||||
|
||||
TEST(MapGridTest, getIndex){
|
||||
MapGrid map_grid(10, 10);
|
||||
EXPECT_EQ(53, map_grid.getIndex(3, 5));
|
||||
}
|
||||
|
||||
TEST(MapGridTest, reset){
|
||||
MapGrid map_grid(10, 10);
|
||||
map_grid(0, 0).target_dist = 1;
|
||||
map_grid(0, 0).target_mark = true;
|
||||
map_grid(0, 0).within_robot = true;
|
||||
map_grid(3, 5).target_dist = 1;
|
||||
map_grid(3, 5).target_mark = true;
|
||||
map_grid(3, 5).within_robot = true;
|
||||
map_grid(9, 9).target_dist = 1;
|
||||
map_grid(9, 9).target_mark = true;
|
||||
map_grid(9, 9).within_robot = true;
|
||||
EXPECT_EQ(1, map_grid(0, 0).target_dist);
|
||||
EXPECT_EQ(true, map_grid(0, 0).target_mark);
|
||||
EXPECT_EQ(true, map_grid(0, 0).within_robot);
|
||||
EXPECT_EQ(1, map_grid(3, 5).target_dist);
|
||||
EXPECT_EQ(true, map_grid(3, 5).target_mark);
|
||||
EXPECT_EQ(true, map_grid(3, 5).within_robot);
|
||||
EXPECT_EQ(1, map_grid(9, 9).target_dist);
|
||||
EXPECT_EQ(true, map_grid(9, 9).target_mark);
|
||||
EXPECT_EQ(true, map_grid(9, 9).within_robot);
|
||||
|
||||
map_grid.resetPathDist();
|
||||
|
||||
EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(9, 9).target_dist);
|
||||
EXPECT_EQ(false, map_grid(9, 9).target_mark);
|
||||
EXPECT_EQ(false, map_grid(9, 9).within_robot);
|
||||
EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(3, 5).target_dist);
|
||||
EXPECT_EQ(false, map_grid(3, 5).target_mark);
|
||||
EXPECT_EQ(false, map_grid(3, 5).within_robot);
|
||||
EXPECT_EQ(map_grid.unreachableCellCosts(), map_grid(0, 0).target_dist);
|
||||
EXPECT_EQ(false, map_grid(0, 0).target_mark);
|
||||
EXPECT_EQ(false, map_grid(0, 0).within_robot);
|
||||
}
|
||||
|
||||
|
||||
TEST(MapGridTest, properGridConstruction){
|
||||
MapGrid mg(10, 10);
|
||||
MapCell mc;
|
||||
|
||||
for(int i = 0; i < 10; ++i){
|
||||
for(int j = 0; j < 10; ++j){
|
||||
EXPECT_FLOAT_EQ(mg(i, j).cx, i);
|
||||
EXPECT_FLOAT_EQ(mg(i, j).cy, j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(MapGridTest, sizeCheck){
|
||||
MapGrid mg(10, 10);
|
||||
MapCell mc;
|
||||
|
||||
mg.sizeCheck(20, 25);
|
||||
|
||||
for(int i = 0; i < 20; ++i){
|
||||
for(int j = 0; j < 25; ++j){
|
||||
EXPECT_FLOAT_EQ(mg(i, j).cx, i);
|
||||
EXPECT_FLOAT_EQ(mg(i, j).cy, j);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
TEST(MapGridTest, adjustPlanEmpty){
|
||||
MapGrid mg(10, 10);
|
||||
const std::vector<geometry_msgs::PoseStamped> global_plan_in;
|
||||
std::vector<geometry_msgs::PoseStamped> global_plan_out;
|
||||
double resolution = 0;
|
||||
mg.adjustPlanResolution(global_plan_in, global_plan_out, resolution);
|
||||
EXPECT_EQ(0, global_plan_out.size());
|
||||
}
|
||||
|
||||
TEST(MapGridTest, adjustPlan){
|
||||
MapGrid mg(10, 10);
|
||||
std::vector<geometry_msgs::PoseStamped> global_plan_in;
|
||||
std::vector<geometry_msgs::PoseStamped> global_plan_out;
|
||||
double resolution = 1;
|
||||
geometry_msgs::PoseStamped start;
|
||||
start.pose.position.x = 1;
|
||||
start.pose.position.y = 1;
|
||||
geometry_msgs::PoseStamped end;
|
||||
end.pose.position.x = 5;
|
||||
end.pose.position.y = 5;
|
||||
global_plan_in.push_back(start);
|
||||
global_plan_in.push_back(end);
|
||||
mg.adjustPlanResolution(global_plan_in, global_plan_out, resolution);
|
||||
|
||||
EXPECT_EQ(1, global_plan_out[0].pose.position.x);
|
||||
EXPECT_EQ(1, global_plan_out[0].pose.position.y);
|
||||
EXPECT_EQ(5, global_plan_out.back().pose.position.x);
|
||||
EXPECT_EQ(5, global_plan_out.back().pose.position.y);
|
||||
|
||||
for (unsigned int i = 1; i < global_plan_out.size(); ++i)
|
||||
{
|
||||
geometry_msgs::Point& p0 = global_plan_out[i - 1].pose.position;
|
||||
geometry_msgs::Point& p1 = global_plan_out[i].pose.position;
|
||||
double d = hypot(p0.x - p1.x, p0.y - p1.y);
|
||||
EXPECT_LT(d, resolution);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(MapGridTest, adjustPlan2){
|
||||
std::vector<geometry_msgs::PoseStamped> base_plan, result;
|
||||
|
||||
// Push two points, at (0,0) and (0,1). Gap is 1 meter
|
||||
base_plan.push_back(geometry_msgs::PoseStamped());
|
||||
base_plan.push_back(geometry_msgs::PoseStamped());
|
||||
base_plan.back().pose.position.y = 1.0;
|
||||
|
||||
// resolution >= 1, path won't change
|
||||
MapGrid::adjustPlanResolution(base_plan, result, 2.0);
|
||||
EXPECT_EQ(2, result.size());
|
||||
result.clear();
|
||||
MapGrid::adjustPlanResolution(base_plan, result, 1.0);
|
||||
EXPECT_EQ(2, result.size());
|
||||
result.clear();
|
||||
|
||||
// 0.5 <= resolution < 1.0, one point should be added in the middle
|
||||
MapGrid::adjustPlanResolution(base_plan, result, 0.8);
|
||||
EXPECT_EQ(3, result.size());
|
||||
result.clear();
|
||||
MapGrid::adjustPlanResolution(base_plan, result, 0.5);
|
||||
EXPECT_EQ(3, result.size());
|
||||
result.clear();
|
||||
|
||||
// 0.333 <= resolution < 0.5, two points should be added in the middle
|
||||
MapGrid::adjustPlanResolution(base_plan, result, 0.34);
|
||||
EXPECT_EQ(4, result.size());
|
||||
result.clear();
|
||||
|
||||
// 0.25 <= resolution < 0.333, three points should be added in the middle
|
||||
MapGrid::adjustPlanResolution(base_plan, result, 0.32);
|
||||
EXPECT_EQ(5, result.size());
|
||||
result.clear();
|
||||
|
||||
MapGrid::adjustPlanResolution(base_plan, result, 0.1);
|
||||
EXPECT_EQ(11, result.size());
|
||||
result.clear();
|
||||
}
|
||||
|
||||
TEST(MapGridTest, distancePropagation){
|
||||
MapGrid mg(10, 10);
|
||||
|
||||
WavefrontMapAccessor* wa = new WavefrontMapAccessor(&mg, .25);
|
||||
std::queue<MapCell*> dist_queue;
|
||||
mg.computeTargetDistance(dist_queue, *wa);
|
||||
EXPECT_EQ(false, mg(0, 0).target_mark);
|
||||
|
||||
MapCell& mc = mg.getCell(0, 0);
|
||||
mc.target_dist = 0.0;
|
||||
mc.target_mark = true;
|
||||
dist_queue.push(&mc);
|
||||
mg.computeTargetDistance(dist_queue, *wa);
|
||||
EXPECT_EQ(true, mg(0, 0).target_mark);
|
||||
EXPECT_EQ(0.0, mg(0, 0).target_dist);
|
||||
EXPECT_EQ(true, mg(1, 1).target_mark);
|
||||
EXPECT_EQ(2.0, mg(1, 1).target_dist);
|
||||
EXPECT_EQ(true, mg(0, 4).target_mark);
|
||||
EXPECT_EQ(4.0, mg(0, 4).target_dist);
|
||||
EXPECT_EQ(true, mg(4, 0).target_mark);
|
||||
EXPECT_EQ(4.0, mg(4, 0).target_dist);
|
||||
EXPECT_EQ(true, mg(9, 9).target_mark);
|
||||
EXPECT_EQ(18.0, mg(9, 9).target_dist);
|
||||
}
|
||||
|
||||
}
|
||||
26
navigations/base_local_planner/test/trajectory_generator_test.cpp
Executable file
26
navigations/base_local_planner/test/trajectory_generator_test.cpp
Executable file
@@ -0,0 +1,26 @@
|
||||
/*
|
||||
* footprint_helper_test.cpp
|
||||
*
|
||||
* Created on: May 2, 2012
|
||||
* Author: tkruse
|
||||
*/
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <vector>
|
||||
|
||||
#include <base_local_planner/simple_trajectory_generator.h>
|
||||
|
||||
namespace base_local_planner {
|
||||
|
||||
class TrajectoryGeneratorTest : public testing::Test {
|
||||
public:
|
||||
SimpleTrajectoryGenerator tg;
|
||||
|
||||
TrajectoryGeneratorTest() {
|
||||
}
|
||||
|
||||
virtual void TestBody(){}
|
||||
};
|
||||
|
||||
}
|
||||
215
navigations/base_local_planner/test/utest.cpp
Executable file
215
navigations/base_local_planner/test/utest.cpp
Executable file
@@ -0,0 +1,215 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Willow Garage nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
#include <gtest/gtest.h>
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
|
||||
#include <base_local_planner/map_cell.h>
|
||||
#include <base_local_planner/map_grid.h>
|
||||
#include <base_local_planner/trajectory.h>
|
||||
#include <base_local_planner/trajectory_planner.h>
|
||||
#include <base_local_planner/costmap_model.h>
|
||||
#include <costmap_2d/costmap_2d.h>
|
||||
#include <math.h>
|
||||
|
||||
#include <geometry_msgs/Point.h>
|
||||
#include <base_local_planner/Position2DInt.h>
|
||||
|
||||
#include "wavefront_map_accessor.h"
|
||||
|
||||
using namespace std;
|
||||
|
||||
namespace base_local_planner {
|
||||
|
||||
class TrajectoryPlannerTest : public testing::Test {
|
||||
public:
|
||||
TrajectoryPlannerTest(MapGrid* g, WavefrontMapAccessor* wave, const costmap_2d::Costmap2D& map, std::vector<geometry_msgs::Point> footprint_spec);
|
||||
void correctFootprint();
|
||||
void footprintObstacles();
|
||||
void checkGoalDistance();
|
||||
void checkPathDistance();
|
||||
virtual void TestBody(){}
|
||||
|
||||
MapGrid* map_;
|
||||
WavefrontMapAccessor* wa;
|
||||
CostmapModel cm;
|
||||
TrajectoryPlanner tc;
|
||||
};
|
||||
|
||||
TrajectoryPlannerTest::TrajectoryPlannerTest(MapGrid* g, WavefrontMapAccessor* wave, const costmap_2d::Costmap2D& map, std::vector<geometry_msgs::Point> footprint_spec)
|
||||
: map_(g), wa(wave), cm(map), tc(cm, map, footprint_spec, 0.0, 1.0, 1.0, 1.0, 1.0, 2.0)
|
||||
{}
|
||||
|
||||
|
||||
|
||||
void TrajectoryPlannerTest::footprintObstacles(){
|
||||
//place an obstacle
|
||||
map_->operator ()(4, 6).target_dist = 1;
|
||||
wa->synchronize();
|
||||
EXPECT_EQ(wa->getCost(4,6), costmap_2d::LETHAL_OBSTACLE);
|
||||
Trajectory traj(0, 0, 0, 0.1, 30);
|
||||
//tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj, 2, 30);
|
||||
tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj);
|
||||
//we expect this path to hit the obstacle
|
||||
EXPECT_FLOAT_EQ(traj.cost_, -1.0);
|
||||
|
||||
//place a wall next to the footprint of the robot
|
||||
tc.path_map_(7, 1).target_dist = 1;
|
||||
tc.path_map_(7, 3).target_dist = 1;
|
||||
tc.path_map_(7, 4).target_dist = 1;
|
||||
tc.path_map_(7, 5).target_dist = 1;
|
||||
tc.path_map_(7, 6).target_dist = 1;
|
||||
tc.path_map_(7, 7).target_dist = 1;
|
||||
wa->synchronize();
|
||||
|
||||
//try to rotate into it
|
||||
//tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj, 2, 30);
|
||||
tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj);
|
||||
//we expect this path to hit the obstacle
|
||||
EXPECT_FLOAT_EQ(traj.cost_, -1.0);
|
||||
}
|
||||
|
||||
void TrajectoryPlannerTest::checkGoalDistance(){
|
||||
//let's box a cell in and make sure that its distance gets set to max
|
||||
map_->operator ()(1, 2).target_dist = 1;
|
||||
map_->operator ()(1, 1).target_dist = 1;
|
||||
map_->operator ()(1, 0).target_dist = 1;
|
||||
map_->operator ()(2, 0).target_dist = 1;
|
||||
map_->operator ()(3, 0).target_dist = 1;
|
||||
map_->operator ()(3, 1).target_dist = 1;
|
||||
map_->operator ()(3, 2).target_dist = 1;
|
||||
map_->operator ()(2, 2).target_dist = 1;
|
||||
wa->synchronize();
|
||||
|
||||
//set a goal
|
||||
tc.path_map_.resetPathDist();
|
||||
queue<MapCell*> target_dist_queue;
|
||||
MapCell& current = tc.path_map_(4, 9);
|
||||
current.target_dist = 0.0;
|
||||
current.target_mark = true;
|
||||
target_dist_queue.push(¤t);
|
||||
tc.path_map_.computeTargetDistance(target_dist_queue, tc.costmap_);
|
||||
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 8).target_dist, 1.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 7).target_dist, 2.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 6).target_dist, 100.0); //there's an obstacle here placed above
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 5).target_dist, 6.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 4).target_dist, 7.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 3).target_dist, 8.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 2).target_dist, 9.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 1).target_dist, 10.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 0).target_dist, 11.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(5, 8).target_dist, 2.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(9, 4).target_dist, 10.0);
|
||||
|
||||
//check the boxed in cell
|
||||
EXPECT_FLOAT_EQ(100.0, tc.path_map_(2, 2).target_dist);
|
||||
|
||||
}
|
||||
|
||||
void TrajectoryPlannerTest::checkPathDistance(){
|
||||
tc.path_map_.resetPathDist();
|
||||
queue<MapCell*> target_dist_queue;
|
||||
MapCell& current = tc.path_map_(4, 9);
|
||||
current.target_dist = 0.0;
|
||||
current.target_mark = true;
|
||||
target_dist_queue.push(¤t);
|
||||
tc.path_map_.computeTargetDistance(target_dist_queue, tc.costmap_);
|
||||
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 8).target_dist, 1.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 7).target_dist, 2.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 6).target_dist, 100.0); //there's an obstacle here placed above
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 5).target_dist, 6.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 4).target_dist, 7.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 3).target_dist, 8.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 2).target_dist, 9.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 1).target_dist, 10.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(4, 0).target_dist, 11.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(5, 8).target_dist, 2.0);
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(9, 4).target_dist, 10.0);
|
||||
|
||||
//check the boxed in cell
|
||||
EXPECT_FLOAT_EQ(tc.path_map_(2, 2).target_dist, 100.0);
|
||||
|
||||
}
|
||||
|
||||
|
||||
TrajectoryPlannerTest* tct = NULL;
|
||||
|
||||
TrajectoryPlannerTest* setup_testclass_singleton() {
|
||||
if (tct == NULL) {
|
||||
MapGrid* mg = new MapGrid (10, 10);
|
||||
WavefrontMapAccessor* wa = new WavefrontMapAccessor(mg, .25);
|
||||
const costmap_2d::Costmap2D& map = *wa;
|
||||
std::vector<geometry_msgs::Point> footprint_spec;
|
||||
geometry_msgs::Point pt;
|
||||
//create a square footprint
|
||||
pt.x = 2;
|
||||
pt.y = 2;
|
||||
footprint_spec.push_back(pt);
|
||||
pt.x = 2;
|
||||
pt.y = -2;
|
||||
footprint_spec.push_back(pt);
|
||||
pt.x = -2;
|
||||
pt.y = -2;
|
||||
footprint_spec.push_back(pt);
|
||||
pt.x = -2;
|
||||
pt.y = 2;
|
||||
footprint_spec.push_back(pt);
|
||||
|
||||
tct = new base_local_planner::TrajectoryPlannerTest(mg, wa, map, footprint_spec);
|
||||
}
|
||||
return tct;
|
||||
}
|
||||
|
||||
//make sure that trajectories that intersect obstacles are invalidated
|
||||
TEST(TrajectoryPlannerTest, footprintObstacles){
|
||||
TrajectoryPlannerTest* tct = setup_testclass_singleton();
|
||||
tct->footprintObstacles();
|
||||
}
|
||||
|
||||
//make sure that goal distance is being computed as expected
|
||||
TEST(TrajectoryPlannerTest, checkGoalDistance){
|
||||
TrajectoryPlannerTest* tct = setup_testclass_singleton();
|
||||
tct->checkGoalDistance();
|
||||
}
|
||||
|
||||
//make sure that path distance is being computed as expected
|
||||
TEST(TrajectoryPlannerTest, checkPathDistance){
|
||||
TrajectoryPlannerTest* tct = setup_testclass_singleton();
|
||||
tct->checkPathDistance();
|
||||
}
|
||||
|
||||
}; //namespace
|
||||
178
navigations/base_local_planner/test/velocity_iterator_test.cpp
Executable file
178
navigations/base_local_planner/test/velocity_iterator_test.cpp
Executable file
@@ -0,0 +1,178 @@
|
||||
/*********************************************************************
|
||||
* Software License Agreement (BSD License)
|
||||
*
|
||||
* Copyright (c) 2008, Willow Garage, Inc.
|
||||
* All rights reserved.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above
|
||||
* copyright notice, this list of conditions and the following
|
||||
* disclaimer in the documentation and/or other materials provided
|
||||
* with the distribution.
|
||||
* * Neither the name of the Willow Garage nor the names of its
|
||||
* contributors may be used to endorse or promote products derived
|
||||
* from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
|
||||
* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*********************************************************************/
|
||||
|
||||
|
||||
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#include <base_local_planner/velocity_iterator.h>
|
||||
|
||||
|
||||
namespace base_local_planner {
|
||||
|
||||
TEST(VelocityIteratorTest, testsingle) {
|
||||
double result[5];
|
||||
int i = 0;
|
||||
for(base_local_planner::VelocityIterator x_it(0.0, 0.0, 1); !x_it.isFinished(); x_it++) {
|
||||
result[i] = x_it.getVelocity();
|
||||
i++;
|
||||
}
|
||||
EXPECT_EQ(1, i);
|
||||
EXPECT_EQ(0, result[0]);
|
||||
}
|
||||
|
||||
TEST(VelocityIteratorTest, testsingle_pos) {
|
||||
double result[5];
|
||||
int i = 0;
|
||||
for(base_local_planner::VelocityIterator x_it(2.2, 2.2, 1); !x_it.isFinished(); x_it++) {
|
||||
result[i] = x_it.getVelocity();
|
||||
i++;
|
||||
}
|
||||
EXPECT_EQ(1, i);
|
||||
EXPECT_EQ(2.2, result[0]);
|
||||
}
|
||||
|
||||
TEST(VelocityIteratorTest, testsingle_neg) {
|
||||
double result[5];
|
||||
int i = 0;
|
||||
for(base_local_planner::VelocityIterator x_it(-3.3, -3.3, 1); !x_it.isFinished(); x_it++) {
|
||||
result[i] = x_it.getVelocity();
|
||||
i++;
|
||||
}
|
||||
EXPECT_EQ(1, i);
|
||||
EXPECT_EQ(-3.3, result[0]);
|
||||
}
|
||||
|
||||
TEST(VelocityIteratorTest, test1) {
|
||||
double result[5];
|
||||
int i = 0;
|
||||
for(base_local_planner::VelocityIterator x_it(-30, 30, 1); !x_it.isFinished(); x_it++) {
|
||||
result[i] = x_it.getVelocity();
|
||||
i++;
|
||||
}
|
||||
EXPECT_EQ(3, i);
|
||||
double expected [3]= {-30.0, 0.0, 30.0};
|
||||
for (int j = 0; j < 3; ++j) {
|
||||
EXPECT_EQ(expected[j], result[j]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(VelocityIteratorTest, test1_pos) {
|
||||
double result[5];
|
||||
int i = 0;
|
||||
for(base_local_planner::VelocityIterator x_it(10, 30, 1); !x_it.isFinished(); x_it++) {
|
||||
result[i] = x_it.getVelocity();
|
||||
i++;
|
||||
}
|
||||
EXPECT_EQ(2, i);
|
||||
double expected [2]= {10.0, 30.0};
|
||||
for (int j = 0; j < 2; ++j) {
|
||||
EXPECT_EQ(expected[j], result[j]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(VelocityIteratorTest, test1_neg) {
|
||||
double result[5];
|
||||
int i = 0;
|
||||
for(base_local_planner::VelocityIterator x_it(-30, -10, 1); !x_it.isFinished(); x_it++) {
|
||||
result[i] = x_it.getVelocity();
|
||||
i++;
|
||||
}
|
||||
EXPECT_EQ(2, i);
|
||||
double expected [2]= {-30.0, -10.0};
|
||||
for (int j = 0; j < 2; ++j) {
|
||||
EXPECT_EQ(expected[j], result[j]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(VelocityIteratorTest, test3) {
|
||||
double result[5];
|
||||
int i = 0;
|
||||
for(base_local_planner::VelocityIterator x_it(-30, 30, 3); !x_it.isFinished(); x_it++) {
|
||||
result[i] = x_it.getVelocity();
|
||||
i++;
|
||||
}
|
||||
EXPECT_EQ(3, i);
|
||||
double expected [3]= {-30.0, 0.0, 30};
|
||||
for (int j = 0; j < 3; ++j) {
|
||||
EXPECT_EQ(expected[j], result[j]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(VelocityIteratorTest, test4) {
|
||||
double result[5];
|
||||
int i = 0;
|
||||
for(base_local_planner::VelocityIterator x_it(-30, 30, 4); !x_it.isFinished(); x_it++) {
|
||||
result[i] = x_it.getVelocity();
|
||||
i++;
|
||||
}
|
||||
EXPECT_EQ(5, i);
|
||||
double expected [5]= {-30.0, -10.0, 0.0, 10.0, 30};
|
||||
for (int j = 0; j < 5; ++j) {
|
||||
EXPECT_EQ(expected[j], result[j]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(VelocityIteratorTest, test_shifted) {
|
||||
// test where zero is not in the middle
|
||||
double result[5];
|
||||
int i = 0;
|
||||
for(base_local_planner::VelocityIterator x_it(-10, 50, 4); !x_it.isFinished(); x_it++) {
|
||||
result[i] = x_it.getVelocity();
|
||||
i++;
|
||||
}
|
||||
EXPECT_EQ(5, i);
|
||||
double expected [5]= {-10.0, 0.0, 10.0, 30, 50};
|
||||
for (int j = 0; j < 5; ++j) {
|
||||
EXPECT_EQ(expected[j], result[j]);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(VelocityIteratorTest, test_cranky) {
|
||||
// test where one value is almost zero (nothing to do about that)
|
||||
double result[5];
|
||||
int i = 0;
|
||||
for(base_local_planner::VelocityIterator x_it(-10.00001, 10, 3); !x_it.isFinished(); x_it++) {
|
||||
result[i] = x_it.getVelocity();
|
||||
i++;
|
||||
}
|
||||
EXPECT_EQ(4, i);
|
||||
for (int j = 0; j < 5; ++j) {
|
||||
double expected [5]= {-10.00001, -0.000005, 0.0, 10.0};
|
||||
EXPECT_FLOAT_EQ(expected[j], result[j]);
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
50
navigations/base_local_planner/test/wavefront_map_accessor.h
Executable file
50
navigations/base_local_planner/test/wavefront_map_accessor.h
Executable file
@@ -0,0 +1,50 @@
|
||||
/*
|
||||
* wavefront_map_accessor.h
|
||||
*
|
||||
* Created on: May 2, 2012
|
||||
* Author: tkruse
|
||||
*/
|
||||
|
||||
#ifndef WAVEFRONT_MAP_ACCESSOR_H_
|
||||
#define WAVEFRONT_MAP_ACCESSOR_H_
|
||||
#include <costmap_2d/cost_values.h>
|
||||
namespace base_local_planner {
|
||||
|
||||
/**
|
||||
* Map_grids rely on costmaps to identify obstacles. We need a costmap that we can easily manipulate for unit tests.
|
||||
* This class has a grid map where we can set grid cell state, and a synchronize method to make the costmap match.
|
||||
*/
|
||||
class WavefrontMapAccessor : public costmap_2d::Costmap2D {
|
||||
public:
|
||||
WavefrontMapAccessor(MapGrid* map, double outer_radius)
|
||||
: costmap_2d::Costmap2D(map->size_x_, map->size_y_, 1, 0, 0),
|
||||
map_(map), outer_radius_(outer_radius) {
|
||||
synchronize();
|
||||
}
|
||||
|
||||
virtual ~WavefrontMapAccessor(){};
|
||||
|
||||
void synchronize(){
|
||||
// Write Cost Data from the map
|
||||
for(unsigned int x = 0; x < size_x_; x++){
|
||||
for (unsigned int y = 0; y < size_y_; y++){
|
||||
unsigned int ind = x + (y * size_x_);
|
||||
if(map_->operator ()(x, y).target_dist == 1) {
|
||||
costmap_[ind] = costmap_2d::LETHAL_OBSTACLE;
|
||||
} else {
|
||||
costmap_[ind] = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
MapGrid* map_;
|
||||
double outer_radius_;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
#endif /* WAVEFRONT_MAP_ACCESSOR_H_ */
|
||||
Reference in New Issue
Block a user