9 #include <gtest/gtest.h> 18 TEST(MapGridTest, initNull){
24 TEST(MapGridTest, operatorBrackets){
26 map_grid(3, 5).target_dist = 5;
30 TEST(MapGridTest, copyConstructor){
32 map_grid(3, 5).target_dist = 5;
35 EXPECT_EQ(5, map_grid(3, 5).target_dist);
38 TEST(MapGridTest, getIndex){
40 EXPECT_EQ(53, map_grid.
getIndex(3, 5));
45 map_grid(0, 0).target_dist = 1;
46 map_grid(0, 0).target_mark =
true;
47 map_grid(0, 0).within_robot =
true;
48 map_grid(3, 5).target_dist = 1;
49 map_grid(3, 5).target_mark =
true;
50 map_grid(3, 5).within_robot =
true;
51 map_grid(9, 9).target_dist = 1;
52 map_grid(9, 9).target_mark =
true;
53 map_grid(9, 9).within_robot =
true;
54 EXPECT_EQ(1, map_grid(0, 0).target_dist);
55 EXPECT_EQ(
true, map_grid(0, 0).target_mark);
56 EXPECT_EQ(
true, map_grid(0, 0).within_robot);
57 EXPECT_EQ(1, map_grid(3, 5).target_dist);
58 EXPECT_EQ(
true, map_grid(3, 5).target_mark);
59 EXPECT_EQ(
true, map_grid(3, 5).within_robot);
60 EXPECT_EQ(1, map_grid(9, 9).target_dist);
61 EXPECT_EQ(
true, map_grid(9, 9).target_mark);
62 EXPECT_EQ(
true, map_grid(9, 9).within_robot);
67 EXPECT_EQ(
false, map_grid(9, 9).target_mark);
68 EXPECT_EQ(
false, map_grid(9, 9).within_robot);
70 EXPECT_EQ(
false, map_grid(3, 5).target_mark);
71 EXPECT_EQ(
false, map_grid(3, 5).within_robot);
73 EXPECT_EQ(
false, map_grid(0, 0).target_mark);
74 EXPECT_EQ(
false, map_grid(0, 0).within_robot);
78 TEST(MapGridTest, properGridConstruction){
82 for(
int i = 0; i < 10; ++i){
83 for(
int j = 0; j < 10; ++j){
84 EXPECT_FLOAT_EQ(mg(i, j).cx, i);
85 EXPECT_FLOAT_EQ(mg(i, j).cy, j);
90 TEST(MapGridTest, sizeCheck){
96 for(
int i = 0; i < 20; ++i){
97 for(
int j = 0; j < 25; ++j){
98 EXPECT_FLOAT_EQ(mg(i, j).cx, i);
99 EXPECT_FLOAT_EQ(mg(i, j).cy, j);
104 TEST(MapGridTest, adjustPlanEmpty){
106 const std::vector<geometry_msgs::PoseStamped> global_plan_in;
107 std::vector<geometry_msgs::PoseStamped> global_plan_out;
108 double resolution = 0;
110 EXPECT_EQ(0, global_plan_out.size());
115 std::vector<geometry_msgs::PoseStamped> global_plan_in;
116 std::vector<geometry_msgs::PoseStamped> global_plan_out;
117 double resolution = 1;
118 geometry_msgs::PoseStamped start;
119 start.pose.position.x = 1;
120 start.pose.position.y = 1;
121 geometry_msgs::PoseStamped end;
122 end.pose.position.x = 5;
123 end.pose.position.y = 5;
124 global_plan_in.push_back(start);
125 global_plan_in.push_back(end);
128 EXPECT_EQ(1, global_plan_out[0].pose.position.x);
129 EXPECT_EQ(1, global_plan_out[0].pose.position.y);
130 EXPECT_EQ(5, global_plan_out.back().pose.position.x);
131 EXPECT_EQ(5, global_plan_out.back().pose.position.y);
133 for (
unsigned int i = 1; i < global_plan_out.size(); ++i)
135 geometry_msgs::Point& p0 = global_plan_out[i - 1].pose.position;
136 geometry_msgs::Point& p1 = global_plan_out[i].pose.position;
137 double d =
hypot(p0.x - p1.x, p0.y - p1.y);
138 EXPECT_LT(d, resolution);
142 TEST(MapGridTest, adjustPlan2){
143 std::vector<geometry_msgs::PoseStamped> base_plan, result;
146 base_plan.push_back(geometry_msgs::PoseStamped());
147 base_plan.push_back(geometry_msgs::PoseStamped());
148 base_plan.back().pose.position.y = 1.0;
152 EXPECT_EQ(2, result.size());
155 EXPECT_EQ(2, result.size());
160 EXPECT_EQ(3, result.size());
163 EXPECT_EQ(3, result.size());
168 EXPECT_EQ(4, result.size());
173 EXPECT_EQ(5, result.size());
177 EXPECT_EQ(11, result.size());
181 TEST(MapGridTest, distancePropagation){
185 std::queue<MapCell*> dist_queue;
187 EXPECT_EQ(
false, mg(0, 0).target_mark);
192 dist_queue.push(&mc);
194 EXPECT_EQ(
true, mg(0, 0).target_mark);
195 EXPECT_EQ(0.0, mg(0, 0).target_dist);
196 EXPECT_EQ(
true, mg(1, 1).target_mark);
197 EXPECT_EQ(2.0, mg(1, 1).target_dist);
198 EXPECT_EQ(
true, mg(0, 4).target_mark);
199 EXPECT_EQ(4.0, mg(0, 4).target_dist);
200 EXPECT_EQ(
true, mg(4, 0).target_mark);
201 EXPECT_EQ(4.0, mg(4, 0).target_dist);
202 EXPECT_EQ(
true, mg(9, 9).target_mark);
203 EXPECT_EQ(18.0, mg(9, 9).target_dist);
double unreachableCellCosts()
void computeTargetDistance(std::queue< MapCell * > &dist_queue, const costmap_2d::Costmap2D &costmap)
Compute the distance from each cell in the local map grid to the planned path.
unsigned int size_y_
The dimensions of the grid.
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controll...
static void adjustPlanResolution(const std::vector< geometry_msgs::PoseStamped > &global_plan_in, std::vector< geometry_msgs::PoseStamped > &global_plan_out, double resolution)
size_t getIndex(int x, int y)
Returns a 1D index into the MapCell array for a 2D index.
void sizeCheck(unsigned int size_x, unsigned int size_y)
check if we need to resize
bool target_mark
Marks for computing path/goal distances.
double target_dist
Distance to planner's path.
Stores path distance and goal distance information used for scoring trajectories. ...
MapCell & getCell(unsigned int x, unsigned int y)
TEST(FootprintHelperTest, correctFootprint)
void resetPathDist()
reset path distance fields for all cells