git commit -m "first commit"

navigations/base_local_planner/test/footprint_helper_test.cpp

@@ -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();
+}
+
+}