test_map_organizer.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2019, the neonavigation authors
3  * All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions are met:
7  *
8  * * Redistributions of source code must retain the above copyright
9  * notice, this list of conditions and the following disclaimer.
10  * * Redistributions in binary form must reproduce the above copyright
11  * notice, this list of conditions and the following disclaimer in the
12  * documentation and/or other materials provided with the distribution.
13  * * Neither the name of the copyright holder nor the names of its
14  * contributors may be used to endorse or promote products derived from
15  * this software without specific prior written permission.
16  *
17  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
18  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
19  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
20  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
21  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
22  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
23  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
24  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
25  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
26  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
27  * POSSIBILITY OF SUCH DAMAGE.
28  */
29 
30 #include <string>
31 
32 #include <ros/ros.h>
33 #include <std_msgs/Int32.h>
34 #include <map_organizer_msgs/OccupancyGridArray.h>
35 #include <nav_msgs/OccupancyGrid.h>
36 
37 #include <gtest/gtest.h>
38 
39 void validateMap0(const nav_msgs::OccupancyGrid& map, const double z)
40 {
41  ASSERT_EQ("map_ground", map.header.frame_id);
42  ASSERT_EQ(2u, map.info.width);
43  ASSERT_EQ(4u, map.data.size());
44  ASSERT_FLOAT_EQ(0.0, map.info.origin.orientation.x);
45  ASSERT_FLOAT_EQ(0.0, map.info.origin.orientation.y);
46  ASSERT_FLOAT_EQ(0.0, map.info.origin.orientation.z);
47  ASSERT_FLOAT_EQ(1.0, map.info.origin.orientation.w);
48  ASSERT_FLOAT_EQ(0.1, map.info.resolution);
49  ASSERT_FLOAT_EQ(0.0, map.info.origin.position.x);
50  ASSERT_FLOAT_EQ(0.0, map.info.origin.position.y);
51  ASSERT_FLOAT_EQ(z, map.info.origin.position.z);
52  ASSERT_EQ(100, map.data[0]);
53  ASSERT_EQ(0, map.data[1]);
54  ASSERT_EQ(100, map.data[2]);
55  ASSERT_EQ(100, map.data[3]);
56 }
57 void validateMap1(const nav_msgs::OccupancyGrid& map, const double z)
58 {
59  ASSERT_EQ("map_ground", map.header.frame_id);
60  ASSERT_EQ(2u, map.info.width);
61  ASSERT_EQ(4u, map.data.size());
62  ASSERT_FLOAT_EQ(0.0, map.info.origin.orientation.x);
63  ASSERT_FLOAT_EQ(0.0, map.info.origin.orientation.y);
64  ASSERT_FLOAT_EQ(0.0, map.info.origin.orientation.z);
65  ASSERT_FLOAT_EQ(1.0, map.info.origin.orientation.w);
66  ASSERT_FLOAT_EQ(0.2, map.info.resolution);
67  ASSERT_FLOAT_EQ(0.1, map.info.origin.position.x);
68  ASSERT_FLOAT_EQ(0.1, map.info.origin.position.y);
69  ASSERT_FLOAT_EQ(z, map.info.origin.position.z);
70  ASSERT_EQ(100, map.data[0]);
71  ASSERT_EQ(0, map.data[1]);
72  ASSERT_EQ(0, map.data[2]);
73  ASSERT_EQ(100, map.data[3]);
74 }
75 
76 TEST(MapOrganizer, MapArray)
77 {
78  ros::NodeHandle nh;
79 
80  map_organizer_msgs::OccupancyGridArray::ConstPtr maps;
81  const boost::function<void(const map_organizer_msgs::OccupancyGridArray::ConstPtr&)>
82  cb = [&maps](const map_organizer_msgs::OccupancyGridArray::ConstPtr& msg) -> void
83  {
84  maps = msg;
85  };
86  ros::Subscriber sub = nh.subscribe("maps", 1, cb);
87 
88  ros::Rate rate(10.0);
89  for (int i = 0; i < 100 && ros::ok(); ++i)
90  {
91  rate.sleep();
92  ros::spinOnce();
93  if (maps)
94  break;
95  }
96  ASSERT_TRUE(static_cast<bool>(maps));
97  ASSERT_EQ(2u, maps->maps.size());
98 
99  validateMap0(maps->maps[0], 1.0);
100  validateMap1(maps->maps[1], 10.0);
101 }
102 
103 TEST(MapOrganizer, Maps)
104 {
105  ros::NodeHandle nh;
106 
107  nav_msgs::OccupancyGrid::ConstPtr map[2];
108  const boost::function<void(const nav_msgs::OccupancyGrid::ConstPtr&, int)>
109  cb = [&map](const nav_msgs::OccupancyGrid::ConstPtr& msg,
110  const int id) -> void
111  {
112  map[id] = msg;
113  };
114  ros::Subscriber sub0 =
115  nh.subscribe<nav_msgs::OccupancyGrid>("map0", 1, boost::bind(cb, _1, 0));
116  ros::Subscriber sub1 =
117  nh.subscribe<nav_msgs::OccupancyGrid>("map1", 1, boost::bind(cb, _1, 1));
118 
119  ros::Rate rate(10.0);
120  for (int i = 0; i < 100 && ros::ok(); ++i)
121  {
122  rate.sleep();
123  ros::spinOnce();
124  if (map[0] && map[1])
125  break;
126  }
127  for (int i = 0; i < 2; ++i)
128  {
129  ASSERT_TRUE(static_cast<bool>(map[i]));
130  }
131  validateMap0(*(map[0]), 1.0);
132  validateMap1(*(map[1]), 10.0);
133 }
134 
135 TEST(MapOrganizer, SelectMap)
136 {
137  ros::NodeHandle nh;
138 
139  nav_msgs::OccupancyGrid::ConstPtr map;
140  const boost::function<void(const nav_msgs::OccupancyGrid::ConstPtr&)>
141  cb = [&map](const nav_msgs::OccupancyGrid::ConstPtr& msg) -> void
142  {
143  map = msg;
144  };
145  ros::Subscriber sub =
146  nh.subscribe<nav_msgs::OccupancyGrid>("map", 1, cb);
147  ros::Publisher pub = nh.advertise<std_msgs::Int32>("floor", 1);
148 
149  ros::Rate rate(10.0);
150  for (int i = 0; i < 100 && ros::ok(); ++i)
151  {
152  rate.sleep();
153  ros::spinOnce();
154  if (pub.getNumSubscribers() > 0 && map)
155  break;
156  }
157  ASSERT_GT(pub.getNumSubscribers(), 0u);
158  ASSERT_TRUE(static_cast<bool>(map));
159  validateMap0(*map, 0.0);
160 
161  std_msgs::Int32 floor;
162  floor.data = 2; // invalid floor must be ignored
163  pub.publish(floor);
164 
165  map = nullptr;
166  for (int i = 0; i < 10 && ros::ok(); ++i)
167  {
168  rate.sleep();
169  ros::spinOnce();
170  if (map)
171  break;
172  }
173  ASSERT_FALSE(static_cast<bool>(map));
174 
175  floor.data = 1;
176  pub.publish(floor);
177 
178  map = nullptr;
179  for (int i = 0; i < 100 && ros::ok(); ++i)
180  {
181  rate.sleep();
182  ros::spinOnce();
183  if (map)
184  break;
185  }
186  ASSERT_TRUE(static_cast<bool>(map));
187  validateMap1(*map, 0.0);
188 }
189 
190 TEST(MapOrganizer, SavedMapArray)
191 {
192  ros::NodeHandle nh;
193 
194  map_organizer_msgs::OccupancyGridArray::ConstPtr maps;
195  const boost::function<void(const map_organizer_msgs::OccupancyGridArray::ConstPtr&)>
196  cb = [&maps](const map_organizer_msgs::OccupancyGridArray::ConstPtr& msg) -> void
197  {
198  maps = msg;
199  };
200  ros::Subscriber sub = nh.subscribe("saved/maps", 1, cb);
201 
202  ros::Rate rate(10.0);
203  for (int i = 0; i < 100 && ros::ok(); ++i)
204  {
205  rate.sleep();
206  ros::spinOnce();
207  if (maps)
208  break;
209  }
210  ASSERT_TRUE(static_cast<bool>(maps));
211  ASSERT_EQ(2u, maps->maps.size());
212 
213  validateMap0(maps->maps[0], 1.0);
214  validateMap1(maps->maps[1], 10.0);
215 
216  // clean temporary files
217  ros::NodeHandle pnh("~");
218  std::string file_prefix;
219  if (pnh.getParam("file_prefix", file_prefix))
220  {
221  ASSERT_EQ(0, system(std::string("rm -f " + file_prefix + "*").c_str()));
222  }
223 }
224 
225 int main(int argc, char** argv)
226 {
227  testing::InitGoogleTest(&argc, argv);
228  ros::init(argc, argv, "test_map_organizer");
229 
230  return RUN_ALL_TESTS();
231 }
maps
map_organizer_msgs::OccupancyGridArray maps
Definition: select_map.cpp:41
ros::NodeHandle
TEST
TEST(MapOrganizer, MapArray)
Definition: test_map_organizer.cpp:76
ros::NodeHandle::subscribe
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::init
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
validateMap0
void validateMap0(const nav_msgs::OccupancyGrid &map, const double z)
Definition: test_map_organizer.cpp:39
ros::ok
ROSCPP_DECL bool ok()
main
int main(int argc, char **argv)
Definition: test_map_organizer.cpp:225
ros::NodeHandle::advertise
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
ros::Rate::sleep
bool sleep()
validateMap1
void validateMap1(const nav_msgs::OccupancyGrid &map, const double z)
Definition: test_map_organizer.cpp:57
ros::NodeHandle::getParam
bool getParam(const std::string &key, std::string &s) const
ros::spinOnce
ROSCPP_DECL void spinOnce()

map_organizer
Author(s): Atsushi Watanabe
autogenerated on Tue Jul 9 2019 04:59:56