coordinates_test.cpp
Go to the documentation of this file.
1 /*
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, Locus Robotics
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 copyright holder 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 HOLDER 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 
35 // Tests ripped from https://github.com/locusrobotics/robot_navigation/blob/master/nav_grid/test/utest.cpp
36 
37 #include <gtest/gtest.h>
38 #include <costmap_2d/costmap_2d.h>
39 
40 using namespace costmap_2d;
41 
42 TEST(CostmapCoordinates, easy_coordinates_test)
43 {
44  Costmap2D costmap(2, 3, 1.0, 0, 0);
45 
46  double wx, wy;
47  costmap.mapToWorld(0u, 0u, wx, wy);
48  EXPECT_DOUBLE_EQ(wx, 0.5);
49  EXPECT_DOUBLE_EQ(wy, 0.5);
50  costmap.mapToWorld(1u, 2u, wx, wy);
51  EXPECT_DOUBLE_EQ(wx, 1.5);
52  EXPECT_DOUBLE_EQ(wy, 2.5);
53 
54  unsigned int umx, umy;
55  int mx, my;
56  ASSERT_TRUE(costmap.worldToMap(wx, wy, umx, umy));
57  EXPECT_EQ(umx, 1);
58  EXPECT_EQ(umy, 2);
59  costmap.worldToMapNoBounds(wx, wy, mx, my);
60  EXPECT_EQ(mx, 1);
61  EXPECT_EQ(my, 2);
62 
63  // Invalid Coordinate
64  wx = 2.5;
65  EXPECT_FALSE(costmap.worldToMap(wx, wy, umx, umy));
66  costmap.worldToMapEnforceBounds(wx, wy, mx, my);
67  EXPECT_EQ(mx, 1);
68  EXPECT_EQ(my, 2);
69  costmap.worldToMapNoBounds(wx, wy, mx, my);
70  EXPECT_EQ(mx, 2);
71  EXPECT_EQ(my, 2);
72 
73  // Border Cases
74  EXPECT_TRUE(costmap.worldToMap(0.0, wy, umx, umy));
75  EXPECT_EQ(umx, 0);
76  EXPECT_TRUE(costmap.worldToMap(0.25, wy, umx, umy));
77  EXPECT_EQ(umx, 0);
78  EXPECT_TRUE(costmap.worldToMap(0.75, wy, umx, umy));
79  EXPECT_EQ(umx, 0);
80  EXPECT_TRUE(costmap.worldToMap(0.9999, wy, umx, umy));
81  EXPECT_EQ(umx, 0);
82  EXPECT_TRUE(costmap.worldToMap(1.0, wy, umx, umy));
83  EXPECT_EQ(umx, 1);
84  EXPECT_TRUE(costmap.worldToMap(1.25, wy, umx, umy));
85  EXPECT_EQ(umx, 1);
86  EXPECT_TRUE(costmap.worldToMap(1.75, wy, umx, umy));
87  EXPECT_EQ(umx, 1);
88  EXPECT_TRUE(costmap.worldToMap(1.9999, wy, umx, umy));
89  EXPECT_EQ(umx, 1);
90  EXPECT_FALSE(costmap.worldToMap(2.0, wy, umx, umy));
91  costmap.worldToMapEnforceBounds(2.0, wy, mx, my);
92  EXPECT_EQ(mx, 1);
93 }
94 
95 TEST(CostmapCoordinates, hard_coordinates_test)
96 {
97  Costmap2D costmap(2, 3, 0.1, -0.2, 0.2);
98 
99  double wx, wy;
100  costmap.mapToWorld(0, 0, wx, wy);
101  EXPECT_DOUBLE_EQ(wx, -0.15);
102  EXPECT_DOUBLE_EQ(wy, 0.25);
103  costmap.mapToWorld(1, 2, wx, wy);
104  EXPECT_DOUBLE_EQ(wx, -0.05);
105  EXPECT_DOUBLE_EQ(wy, 0.45);
106 
107  unsigned int umx, umy;
108  int mx, my;
109  EXPECT_TRUE(costmap.worldToMap(wx, wy, umx, umy));
110  EXPECT_EQ(umx, 1);
111  EXPECT_EQ(umy, 2);
112  costmap.worldToMapNoBounds(wx, wy, mx, my);
113  EXPECT_EQ(mx, 1);
114  EXPECT_EQ(my, 2);
115 
116  // Invalid Coordinate
117  wx = 2.5;
118  EXPECT_FALSE(costmap.worldToMap(wx, wy, umx, umy));
119  costmap.worldToMapEnforceBounds(wx, wy, mx, my);
120  EXPECT_EQ(mx, 1);
121  EXPECT_EQ(my, 2);
122  costmap.worldToMapNoBounds(wx, wy, mx, my);
123  EXPECT_EQ(mx, 27);
124  EXPECT_EQ(my, 2);
125 }
126 
127 int main(int argc, char** argv)
128 {
129  testing::InitGoogleTest( &argc, argv );
130  return RUN_ALL_TESTS();
131 }
132 
costmap_2d::Costmap2D::worldToMap
bool worldToMap(double wx, double wy, unsigned int &mx, unsigned int &my) const
Convert from world coordinates to map coordinates.
Definition: costmap_2d.cpp:208
costmap_2d::Costmap2D
A 2D costmap provides a mapping between points in the world and their associated "costs".
Definition: costmap_2d.h:96
costmap_2d::Costmap2D::mapToWorld
void mapToWorld(unsigned int mx, unsigned int my, double &wx, double &wy) const
Convert from map coordinates to world coordinates.
Definition: costmap_2d.cpp:202
costmap_2d.h
costmap_2d::Costmap2D::worldToMapEnforceBounds
void worldToMapEnforceBounds(double wx, double wy, int &mx, int &my) const
Convert from world coordinates to map coordinates, constraining results to legal bounds.
Definition: costmap_2d.cpp:228
TEST
TEST(CostmapCoordinates, easy_coordinates_test)
Definition: coordinates_test.cpp:42
main
int main(int argc, char **argv)
Definition: coordinates_test.cpp:127
costmap_2d::Costmap2D::worldToMapNoBounds
void worldToMapNoBounds(double wx, double wy, int &mx, int &my) const
Convert from world coordinates to map coordinates without checking for legal bounds.
Definition: costmap_2d.cpp:222
costmap_2d
Definition: array_parser.h:37


costmap_2d
Author(s): Eitan Marder-Eppstein, David V. Lu!!, Dave Hershberger, contradict@gmail.com
autogenerated on Mon Mar 6 2023 03:50:17