146 lines
4.8 KiB
C++
Executable File
146 lines
4.8 KiB
C++
Executable File
/*
|
|
* 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();
|
|
}
|
|
|
|
}
|