git commit -m "first commit"

navigations/base_local_planner/test/map_grid_test.cpp

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