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 
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 
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 
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 
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
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 
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 
170 
172  if (tct == NULL) {
173  MapGrid* mg = new MapGrid (10, 10);
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
200  tct->footprintObstacles();
201 }
202 
203 //make sure that goal distance is being computed as expected
206  tct->checkGoalDistance();
207 }
208 
209 //make sure that path distance is being computed as expected
212  tct->checkPathDistance();
213 }
214 
215 }; //namespace
Computes control velocities for a robot given a costmap, a plan, and the robot&#39;s position in the worl...
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
const costmap_2d::Costmap2D & costmap_
Provides access to cost map information.
TrajectoryPlannerTest * tct
Definition: utest.cpp:169
MapGrid path_map_
The local map grid where we propagate path distance.
#define DBL_MAX
TrajectoryPlannerTest * setup_testclass_singleton()
Definition: utest.cpp:171
A grid of MapCell cells that is used to propagate path and goal distances for the trajectory controll...
Definition: map_grid.h:52
unsigned char getCost(unsigned int mx, unsigned int my) const
bool target_mark
Marks for computing path/goal distances.
Definition: map_cell.h:61
double target_dist
Distance to planner&#39;s path.
Definition: map_cell.h:59
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.
static const unsigned char LETHAL_OBSTACLE
Stores path distance and goal distance information used for scoring trajectories. ...
Definition: map_cell.h:44
A class that implements the WorldModel interface to provide grid based collision checks for the traje...
Definition: costmap_model.h:50
TEST(FootprintHelperTest, correctFootprint)
void resetPathDist()
reset path distance fields for all cells
Definition: map_grid.cpp:125
Holds a trajectory generated by considering an x, y, and theta velocity.
Definition: trajectory.h:44


base_local_planner
Author(s): Eitan Marder-Eppstein, Eric Perko, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:49