utest.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of the Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 #include <gtest/gtest.h>
35 #include <iostream>
36 #include <vector>
37 #include <utility>
38 
39 #include <base_local_planner/map_cell.h>
40 #include <base_local_planner/map_grid.h>
41 #include <base_local_planner/trajectory.h>
42 #include <base_local_planner/trajectory_planner.h>
43 #include <base_local_planner/costmap_model.h>
44 #include <costmap_2d/costmap_2d.h>
45 #include <math.h>
46 
47 #include <geometry_msgs/Point.h>
48 #include <base_local_planner/Position2DInt.h>
49 
50 #include "wavefront_map_accessor.h"
51 
52 using namespace std;
53 
54 namespace base_local_planner {
55 
56 class TrajectoryPlannerTest : public testing::Test {
57  public:
58  TrajectoryPlannerTest(MapGrid* g, WavefrontMapAccessor* wave, const costmap_2d::Costmap2D& map, std::vector<geometry_msgs::Point> footprint_spec);
59  void correctFootprint();
60  void footprintObstacles();
61  void checkGoalDistance();
62  void checkPathDistance();
63  virtual void TestBody(){}
64 
65  MapGrid* map_;
66  WavefrontMapAccessor* wa;
67  CostmapModel cm;
68  TrajectoryPlanner tc;
69 };
70 
71 TrajectoryPlannerTest::TrajectoryPlannerTest(MapGrid* g, WavefrontMapAccessor* wave, const costmap_2d::Costmap2D& map, std::vector<geometry_msgs::Point> footprint_spec)
72 : map_(g), wa(wave), cm(map), tc(cm, map, footprint_spec, 0.0, 1.0, 1.0, 1.0, 1.0, 2.0)
73 {}
74 
75 
76 
77 void TrajectoryPlannerTest::footprintObstacles(){
78  //place an obstacle
79  map_->operator ()(4, 6).target_dist = 1;
80  wa->synchronize();
81  EXPECT_EQ(wa->getCost(4,6), costmap_2d::LETHAL_OBSTACLE);
82  Trajectory traj(0, 0, 0, 0.1, 30);
83  //tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj, 2, 30);
84  tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 4, 0, 0, 4, 0, 0, DBL_MAX, traj);
85  //we expect this path to hit the obstacle
86  EXPECT_FLOAT_EQ(traj.cost_, -1.0);
87 
88  //place a wall next to the footprint of the robot
89  tc.path_map_(7, 1).target_dist = 1;
90  tc.path_map_(7, 3).target_dist = 1;
91  tc.path_map_(7, 4).target_dist = 1;
92  tc.path_map_(7, 5).target_dist = 1;
93  tc.path_map_(7, 6).target_dist = 1;
94  tc.path_map_(7, 7).target_dist = 1;
95  wa->synchronize();
96 
97  //try to rotate into it
98  //tc->generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj, 2, 30);
99  tc.generateTrajectory(4.5, 4.5, M_PI_2, 0, 0, 0, 0, 0, M_PI_2, 0, 0, M_PI_4, 100, traj);
100  //we expect this path to hit the obstacle
101  EXPECT_FLOAT_EQ(traj.cost_, -1.0);
102 }
103 
104 void TrajectoryPlannerTest::checkGoalDistance(){
105  //let's box a cell in and make sure that its distance gets set to max
106  map_->operator ()(1, 2).target_dist = 1;
107  map_->operator ()(1, 1).target_dist = 1;
108  map_->operator ()(1, 0).target_dist = 1;
109  map_->operator ()(2, 0).target_dist = 1;
110  map_->operator ()(3, 0).target_dist = 1;
111  map_->operator ()(3, 1).target_dist = 1;
112  map_->operator ()(3, 2).target_dist = 1;
113  map_->operator ()(2, 2).target_dist = 1;
114  wa->synchronize();
115 
116  //set a goal
117  tc.path_map_.resetPathDist();
118  queue<MapCell*> target_dist_queue;
119  MapCell& current = tc.path_map_(4, 9);
120  current.target_dist = 0.0;
121  current.target_mark = true;
122  target_dist_queue.push(&current);
123  tc.path_map_.computeTargetDistance(target_dist_queue, tc.costmap_);
124 
125  EXPECT_FLOAT_EQ(tc.path_map_(4, 8).target_dist, 1.0);
126  EXPECT_FLOAT_EQ(tc.path_map_(4, 7).target_dist, 2.0);
127  EXPECT_FLOAT_EQ(tc.path_map_(4, 6).target_dist, 100.0); //there's an obstacle here placed above
128  EXPECT_FLOAT_EQ(tc.path_map_(4, 5).target_dist, 6.0);
129  EXPECT_FLOAT_EQ(tc.path_map_(4, 4).target_dist, 7.0);
130  EXPECT_FLOAT_EQ(tc.path_map_(4, 3).target_dist, 8.0);
131  EXPECT_FLOAT_EQ(tc.path_map_(4, 2).target_dist, 9.0);
132  EXPECT_FLOAT_EQ(tc.path_map_(4, 1).target_dist, 10.0);
133  EXPECT_FLOAT_EQ(tc.path_map_(4, 0).target_dist, 11.0);
134  EXPECT_FLOAT_EQ(tc.path_map_(5, 8).target_dist, 2.0);
135  EXPECT_FLOAT_EQ(tc.path_map_(9, 4).target_dist, 10.0);
136 
137  //check the boxed in cell
138  EXPECT_FLOAT_EQ(100.0, tc.path_map_(2, 2).target_dist);
139 
140 }
141 
142 void TrajectoryPlannerTest::checkPathDistance(){
143  tc.path_map_.resetPathDist();
144  queue<MapCell*> target_dist_queue;
145  MapCell& current = tc.path_map_(4, 9);
146  current.target_dist = 0.0;
147  current.target_mark = true;
148  target_dist_queue.push(&current);
149  tc.path_map_.computeTargetDistance(target_dist_queue, tc.costmap_);
150 
151  EXPECT_FLOAT_EQ(tc.path_map_(4, 8).target_dist, 1.0);
152  EXPECT_FLOAT_EQ(tc.path_map_(4, 7).target_dist, 2.0);
153  EXPECT_FLOAT_EQ(tc.path_map_(4, 6).target_dist, 100.0); //there's an obstacle here placed above
154  EXPECT_FLOAT_EQ(tc.path_map_(4, 5).target_dist, 6.0);
155  EXPECT_FLOAT_EQ(tc.path_map_(4, 4).target_dist, 7.0);
156  EXPECT_FLOAT_EQ(tc.path_map_(4, 3).target_dist, 8.0);
157  EXPECT_FLOAT_EQ(tc.path_map_(4, 2).target_dist, 9.0);
158  EXPECT_FLOAT_EQ(tc.path_map_(4, 1).target_dist, 10.0);
159  EXPECT_FLOAT_EQ(tc.path_map_(4, 0).target_dist, 11.0);
160  EXPECT_FLOAT_EQ(tc.path_map_(5, 8).target_dist, 2.0);
161  EXPECT_FLOAT_EQ(tc.path_map_(9, 4).target_dist, 10.0);
162 
163  //check the boxed in cell
164  EXPECT_FLOAT_EQ(tc.path_map_(2, 2).target_dist, 100.0);
165 
166 }
167 
168 
169 TrajectoryPlannerTest* tct = NULL;
170 
171 TrajectoryPlannerTest* setup_testclass_singleton() {
172  if (tct == NULL) {
173  MapGrid* mg = new MapGrid (10, 10);
174  WavefrontMapAccessor* wa = new WavefrontMapAccessor(mg, .25);
175  const costmap_2d::Costmap2D& map = *wa;
176  std::vector<geometry_msgs::Point> footprint_spec;
177  geometry_msgs::Point pt;
178  //create a square footprint
179  pt.x = 2;
180  pt.y = 2;
181  footprint_spec.push_back(pt);
182  pt.x = 2;
183  pt.y = -2;
184  footprint_spec.push_back(pt);
185  pt.x = -2;
186  pt.y = -2;
187  footprint_spec.push_back(pt);
188  pt.x = -2;
189  pt.y = 2;
190  footprint_spec.push_back(pt);
191 
192  tct = new base_local_planner::TrajectoryPlannerTest(mg, wa, map, footprint_spec);
193  }
194  return tct;
195 }
196 
197 //make sure that trajectories that intersect obstacles are invalidated
198 TEST(TrajectoryPlannerTest, footprintObstacles){
199  TrajectoryPlannerTest* tct = setup_testclass_singleton();
200  tct->footprintObstacles();
201 }
202 
203 //make sure that goal distance is being computed as expected
204 TEST(TrajectoryPlannerTest, checkGoalDistance){
205  TrajectoryPlannerTest* tct = setup_testclass_singleton();
206  tct->checkGoalDistance();
207 }
208 
209 //make sure that path distance is being computed as expected
210 TEST(TrajectoryPlannerTest, checkPathDistance){
211  TrajectoryPlannerTest* tct = setup_testclass_singleton();
212  tct->checkPathDistance();
213 }
214 
215 }; //namespace
base_local_planner::tct
TrajectoryPlannerTest * tct
Definition: utest.cpp:169
base_local_planner::TrajectoryPlannerTest::wa
WavefrontMapAccessor * wa
Definition: utest.cpp:66
trajectory.h
base_local_planner::setup_testclass_singleton
TrajectoryPlannerTest * setup_testclass_singleton()
Definition: utest.cpp:171
base_local_planner::WavefrontMapAccessor
Definition: wavefront_map_accessor.h:17
base_local_planner::TEST
TEST(FootprintHelperTest, correctFootprint)
Definition: footprint_helper_test.cpp:135
base_local_planner::TrajectoryPlannerTest::tc
TrajectoryPlanner tc
Definition: utest.cpp:68
costmap_2d.h
wavefront_map_accessor.h
costmap_2d::Costmap2D
base_local_planner::TrajectoryPlanner::costmap_
const costmap_2d::Costmap2D & costmap_
Provides access to cost map information.
Definition: trajectory_planner.h:296
map_grid.h
costmap_model.h
base_local_planner::TrajectoryPlanner::path_map_
MapGrid path_map_
The local map grid where we propagate path distance.
Definition: trajectory_planner.h:294
base_local_planner::CostmapModel
A class that implements the WorldModel interface to provide grid based collision checks for the traje...
Definition: costmap_model.h:85
base_local_planner::Trajectory::cost_
double cost_
The cost/score of the trajectory.
Definition: trajectory.h:94
base_local_planner::MapGrid::resetPathDist
void resetPathDist()
reset path distance fields for all cells
Definition: map_grid.cpp:125
base_local_planner::WavefrontMapAccessor::synchronize
void synchronize()
Definition: wavefront_map_accessor.h:27
base_local_planner::MapGrid::computeTargetDistance
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.
Definition: map_grid.cpp:255
costmap_2d::LETHAL_OBSTACLE
static const unsigned char LETHAL_OBSTACLE
base_local_planner::TrajectoryPlannerTest::footprintObstacles
void footprintObstacles()
Definition: utest.cpp:77
base_local_planner::TrajectoryPlanner::generateTrajectory
void generateTrajectory(double x, double y, double theta, double vx, double vy, double vtheta, double vx_samp, double vy_samp, double vtheta_samp, double acc_x, double acc_y, double acc_theta, double impossible_cost, Trajectory &traj)
Generate and score a single trajectory.
Definition: trajectory_planner.cpp:214
map_cell.h
base_local_planner::MapCell::target_dist
double target_dist
Distance to planner's path.
Definition: map_cell.h:91
DBL_MAX
#define DBL_MAX
Definition: trajectory_inc.h:40
base_local_planner::MapGrid
A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controll...
Definition: map_grid.h:84
trajectory_planner.h
base_local_planner::TrajectoryPlannerTest::map_
MapGrid * map_
Definition: utest.cpp:65
base_local_planner::MapCell::target_mark
bool target_mark
Marks for computing path/goal distances.
Definition: map_cell.h:93
std
base_local_planner::TrajectoryPlannerTest::TestBody
virtual void TestBody()
Definition: utest.cpp:63
base_local_planner::Trajectory
Holds a trajectory generated by considering an x, y, and theta velocity.
Definition: trajectory.h:76
base_local_planner::TrajectoryPlannerTest::checkPathDistance
void checkPathDistance()
Definition: utest.cpp:142
base_local_planner::TrajectoryPlannerTest::cm
CostmapModel cm
Definition: utest.cpp:67
base_local_planner::TrajectoryPlanner
Computes control velocities for a robot given a costmap, a plan, and the robot's position in the worl...
Definition: trajectory_planner.h:101
base_local_planner::TrajectoryPlannerTest
Definition: utest.cpp:56
base_local_planner::TrajectoryPlannerTest::checkGoalDistance
void checkGoalDistance()
Definition: utest.cpp:104
base_local_planner
Definition: costmap_model.h:44
costmap_2d::Costmap2D::getCost
unsigned char getCost(unsigned int mx, unsigned int my) const
base_local_planner::MapCell
Stores path distance and goal distance information used for scoring trajectories.
Definition: map_cell.h:76

base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:24