42 #include <gtest/gtest.h> 46 using geometry_msgs::Point;
50 std::vector<Point> polygon;
67 nh.
setParam(
"/inflation_tests/inflation/inflation_radius", inflation_radius);
77 std::map<double, std::vector<CellData> > m;
79 m[0].push_back(initial);
80 for (std::map<
double, std::vector<CellData> >::iterator bin = m.begin(); bin != m.end(); ++bin)
82 for (
int i = 0; i < bin->second.size(); ++i)
84 const CellData& cell = bin->second[i];
90 double dist =
hypot(dx, dy);
92 unsigned char expected_cost = ilayer->
computeCost(dist);
93 ASSERT_TRUE(costmap->
getCost(cell.
x_, cell.
y_) >= expected_cost);
95 if (dist > inflation_radius)
104 m[dist].push_back(data);
110 m[dist].push_back(data);
116 m[dist].push_back(data);
122 m[dist].push_back(data);
130 TEST(costmap, testAdjacentToObstacleCanStillMove){
137 std::vector<Point> polygon =
setRadii(layers, 2.1, 2.3, 4.1);
156 TEST(costmap, testInflationShouldNotCreateUnknowns){
163 std::vector<Point> polygon =
setRadii(layers, 2.1, 2.3, 4.1);
181 TEST(costmap, testCostFunctionCorrectness){
188 std::vector<Point> polygon =
setRadii(layers, 5.0, 6.25, 10.5);
203 for(
unsigned int i = 0; i <= (
unsigned int)ceil(5.0); i++){
219 for(
unsigned int i = (
unsigned int)(ceil(5.0) + 1); i <= (
unsigned int)ceil(10.5); i++){
220 unsigned char expectedValue = ilayer->
computeCost(i/1.0);
221 ASSERT_EQ(map->
getCost(50 + i, 50), expectedValue);
249 TEST(costmap, testInflationOrderCorrectness){
256 const double inflation_radius = 4.1;
257 std::vector<Point> polygon =
setRadii(layers, 2.1, 2.3, inflation_radius);
284 std::vector<Point> polygon =
setRadii(layers, 1, 1, 1);
348 std::vector<Point> polygon =
setRadii(layers, 1, 1, 1);
376 std::vector<Point> polygon =
setRadii(layers, 1, 1.75, 3);
406 int main(
int argc,
char** argv){
407 ros::init(argc, argv,
"inflation_tests");
408 testing::InitGoogleTest(&argc, argv);
409 return RUN_ALL_TESTS();
unsigned char computeCost(double distance) const
Given a distance, compute a cost.
unsigned int countValues(costmap_2d::Costmap2D &costmap, unsigned char value, bool equal=true)
void resizeMap(unsigned int size_x, unsigned int size_y, double resolution, double origin_x, double origin_y, bool size_locked=false)
int main(int argc, char **argv)
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
INLINE Rall1d< T, V, S > hypot(const Rall1d< T, V, S > &y, const Rall1d< T, V, S > &x)
costmap_2d::ObstacleLayer * addObstacleLayer(costmap_2d::LayeredCostmap &layers, tf::TransformListener &tf)
void setFootprint(const std::vector< geometry_msgs::Point > &footprint_spec)
Updates the stored footprint, updates the circumscribed and inscribed radii, and calls onFootprintCha...
void addStaticLayer(costmap_2d::LayeredCostmap &layers, tf::TransformListener &tf)
static const unsigned char FREE_SPACE
TFSIMD_FORCE_INLINE const tfScalar & x() const
unsigned char getCost(unsigned int mx, unsigned int my) const
Get the cost of a cell in the costmap.
static const unsigned char INSCRIBED_INFLATED_OBSTACLE
TFSIMD_FORCE_INLINE const tfScalar & y() const
unsigned int getSizeInCellsY() const
Accessor for the y size of the costmap in cells.
TEST(costmap, testAdjacentToObstacleCanStillMove)
void validatePointInflation(unsigned int mx, unsigned int my, Costmap2D *costmap, InflationLayer *ilayer, double inflation_radius)
Storage for cell information used during obstacle inflation.
std::vector< Point > setRadii(LayeredCostmap &layers, double length, double width, double inflation_radius)
unsigned int getSizeInCellsX() const
Accessor for the x size of the costmap in cells.
static const unsigned char LETHAL_OBSTACLE
INLINE Rall1d< T, V, S > abs(const Rall1d< T, V, S > &x)
costmap_2d::InflationLayer * addInflationLayer(costmap_2d::LayeredCostmap &layers, tf::TransformListener &tf)
static const unsigned char NO_INFORMATION
void addObservation(costmap_2d::ObstacleLayer *olayer, double x, double y, double z=0.0, double ox=0.0, double oy=0.0, double oz=MAX_Z)
A 2D costmap provides a mapping between points in the world and their associated "costs".
void printMap(costmap_2d::Costmap2D &costmap)
Instantiates different layer plugins and aggregates them into one score.
void updateMap(double robot_x, double robot_y, double robot_yaw)
Update the underlying costmap with new data. If you want to update the map outside of the update loop...
void setParam(const std::string &key, const XmlRpc::XmlRpcValue &v) const
unsigned int getIndex(unsigned int mx, unsigned int my) const
Given two map coordinates... compute the associated index.