8 #include <gtest/gtest.h> 33 std::vector<base_local_planner::Position2DInt> footprint;
35 EXPECT_EQ(11, footprint.size());
36 EXPECT_EQ(footprint[0].
x, 0);
37 EXPECT_EQ(footprint[0].
y, 0);
38 EXPECT_EQ(footprint[5].x, 5);
39 EXPECT_EQ(footprint[5].y, 5);
40 EXPECT_EQ(footprint[10].x, 10);
41 EXPECT_EQ(footprint[10].y, 10);
49 std::vector<geometry_msgs::Point> footprint_spec;
50 geometry_msgs::Point pt;
54 footprint_spec.push_back(pt);
57 footprint_spec.push_back(pt);
60 footprint_spec.push_back(pt);
63 footprint_spec.push_back(pt);
65 Eigen::Vector3f pos(4.5, 4.5, 0);
67 std::vector<base_local_planner::Position2DInt> footprint = fh.
getFootprintCells(pos, footprint_spec, map,
false);
69 EXPECT_EQ(20, footprint.size());
71 EXPECT_EQ(footprint[0].
x, 6); EXPECT_EQ(footprint[0].
y, 6);
72 EXPECT_EQ(footprint[1].x, 6); EXPECT_EQ(footprint[1].y, 5);
73 EXPECT_EQ(footprint[2].x, 6); EXPECT_EQ(footprint[2].y, 4);
74 EXPECT_EQ(footprint[3].x, 6); EXPECT_EQ(footprint[3].y, 3);
75 EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 2);
78 EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 2);
79 EXPECT_EQ(footprint[6].x, 5); EXPECT_EQ(footprint[6].y, 2);
80 EXPECT_EQ(footprint[7].x, 4); EXPECT_EQ(footprint[7].y, 2);
81 EXPECT_EQ(footprint[8].x, 3); EXPECT_EQ(footprint[8].y, 2);
82 EXPECT_EQ(footprint[9].x, 2); EXPECT_EQ(footprint[9].y, 2);
85 EXPECT_EQ(footprint[10].x, 2); EXPECT_EQ(footprint[10].y, 2);
86 EXPECT_EQ(footprint[11].x, 2); EXPECT_EQ(footprint[11].y, 3);
87 EXPECT_EQ(footprint[12].x, 2); EXPECT_EQ(footprint[12].y, 4);
88 EXPECT_EQ(footprint[13].x, 2); EXPECT_EQ(footprint[13].y, 5);
89 EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 6);
92 EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 6);
93 EXPECT_EQ(footprint[16].x, 3); EXPECT_EQ(footprint[16].y, 6);
94 EXPECT_EQ(footprint[17].x, 4); EXPECT_EQ(footprint[17].y, 6);
95 EXPECT_EQ(footprint[18].x, 5); EXPECT_EQ(footprint[18].y, 6);
96 EXPECT_EQ(footprint[19].x, 6); EXPECT_EQ(footprint[19].y, 6);
99 pos = Eigen::Vector3f(4.5, 4.5, M_PI_2);
104 EXPECT_EQ(footprint[0].x, 2); EXPECT_EQ(footprint[0].y, 6);
105 EXPECT_EQ(footprint[1].x, 3); EXPECT_EQ(footprint[1].y, 6);
106 EXPECT_EQ(footprint[2].x, 4); EXPECT_EQ(footprint[2].y, 6);
107 EXPECT_EQ(footprint[3].x, 5); EXPECT_EQ(footprint[3].y, 6);
108 EXPECT_EQ(footprint[4].x, 6); EXPECT_EQ(footprint[4].y, 6);
111 EXPECT_EQ(footprint[5].x, 6); EXPECT_EQ(footprint[5].y, 6);
112 EXPECT_EQ(footprint[6].x, 6); EXPECT_EQ(footprint[6].y, 5);
113 EXPECT_EQ(footprint[7].x, 6); EXPECT_EQ(footprint[7].y, 4);
114 EXPECT_EQ(footprint[8].x, 6); EXPECT_EQ(footprint[8].y, 3);
115 EXPECT_EQ(footprint[9].x, 6); EXPECT_EQ(footprint[9].y, 2);
118 EXPECT_EQ(footprint[10].x, 6); EXPECT_EQ(footprint[10].y, 2);
119 EXPECT_EQ(footprint[11].x, 5); EXPECT_EQ(footprint[11].y, 2);
120 EXPECT_EQ(footprint[12].x, 4); EXPECT_EQ(footprint[12].y, 2);
121 EXPECT_EQ(footprint[13].x, 3); EXPECT_EQ(footprint[13].y, 2);
122 EXPECT_EQ(footprint[14].x, 2); EXPECT_EQ(footprint[14].y, 2);
125 EXPECT_EQ(footprint[15].x, 2); EXPECT_EQ(footprint[15].y, 2);
126 EXPECT_EQ(footprint[16].x, 2); EXPECT_EQ(footprint[16].y, 3);
127 EXPECT_EQ(footprint[17].x, 2); EXPECT_EQ(footprint[17].y, 4);
128 EXPECT_EQ(footprint[18].x, 2); EXPECT_EQ(footprint[18].y, 5);
129 EXPECT_EQ(footprint[19].x, 2); EXPECT_EQ(footprint[19].y, 6);
TrajectoryPlannerTest * tct
TFSIMD_FORCE_INLINE const tfScalar & y() const
A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controll...
TFSIMD_FORCE_INLINE const tfScalar & x() const
TEST(FootprintHelperTest, correctFootprint)