rtest.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage, Inc. 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 /* Author: Brian Gerkey */
31 
32 #include <gtest/gtest.h>
33 #include <ros/service.h>
34 #include <ros/ros.h>
35 #include <nav_msgs/GetMap.h>
36 #include <nav_msgs/OccupancyGrid.h>
37 #include <nav_msgs/MapMetaData.h>
38 
39 
42 
43 class MapClientTest : public testing::Test
44 {
45  public:
47  {
48  got_map_ = false;
49  got_map_metadata_ = false;
50  }
51 
53  {
54  }
55 
56  bool got_map_;
59  {
60  map_ = map;
61  got_map_ = true;
62  }
63 
67  {
68  map_metadata_ = map_metadata;
69  got_map_metadata_ = true;
70  }
71 
72  void checkMapMetaData(const nav_msgs::MapMetaData& map_metadata)
73  {
74  EXPECT_FLOAT_EQ(map_metadata.resolution, g_res);
75  EXPECT_FLOAT_EQ(map_metadata.width, g_width);
76  EXPECT_FLOAT_EQ(map_metadata.height, g_height);
77  }
78 
79  void checkMapData(const nav_msgs::OccupancyGrid& map)
80  {
81  unsigned int i;
82  unsigned int free_cnt = 0;
83  for(i=0; i < map.info.width * map.info.height; i++)
84  {
85  if(map.data[i] == 0)
86  free_cnt++;
87  }
88  double free_ratio = free_cnt / (double)(i);
89  ROS_INFO("Min / ratio / Max free ratio: %f / %f / %f", g_min_free_ratio, free_ratio, g_max_free_ratio);
90  EXPECT_GE(free_ratio, g_min_free_ratio);
91  EXPECT_LE(free_ratio, g_max_free_ratio);
92  }
93 };
94 
95 /* Try to retrieve the map via service, and compare to ground truth */
96 TEST_F(MapClientTest, call_service)
97 {
98  nav_msgs::GetMap::Request req;
99  nav_msgs::GetMap::Response resp;
100  ASSERT_TRUE(ros::service::waitForService("dynamic_map", 5000));
101  ASSERT_TRUE(ros::service::call("dynamic_map", req, resp));
102  checkMapMetaData(resp.map.info);
103  checkMapData(resp.map);
104 }
105 
106 /* Try to retrieve the map via topic, and compare to ground truth */
107 TEST_F(MapClientTest, subscribe_topic)
108 {
109  ros::Subscriber sub = g_n->subscribe<nav_msgs::OccupancyGrid>("map", 1, boost::bind(&MapClientTest::mapCallback, this, _1));
110 
111  // Try a few times, because the server may not be up yet.
112  int i=20;
113  while(!got_map_ && i > 0)
114  {
115  ros::spinOnce();
116  ros::WallDuration d(0.25);
117  d.sleep();
118  i--;
119  }
120  ASSERT_TRUE(got_map_);
121  checkMapMetaData(map_->info);
122  checkMapData(*(map_.get()));
123 }
124 
125 /* Try to retrieve the metadata via topic, and compare to ground truth */
126 TEST_F(MapClientTest, subscribe_topic_metadata)
127 {
128  ros::Subscriber sub = g_n->subscribe<nav_msgs::MapMetaData>("map_metadata", 1, boost::bind(&MapClientTest::mapMetaDataCallback, this, _1));
129 
130  // Try a few times, because the server may not be up yet.
131  int i=20;
132  while(!got_map_metadata_ && i > 0)
133  {
134  ros::spinOnce();
135  ros::WallDuration d(0.25);
136  d.sleep();
137  i--;
138  }
139  ASSERT_TRUE(got_map_metadata_);
141 }
142 
143 int main(int argc, char **argv)
144 {
145  testing::InitGoogleTest(&argc, argv);
146 
147  ros::init(argc, argv, "map_client_test");
148  g_n = new ros::NodeHandle();
149 
150  // Required args are, in order:
151  // <delay> <resolution> <width> <height> <min_free_ratio> <max_free_ratio>
152  ROS_ASSERT(argc == 7);
153  ros::Duration delay = ros::Duration(atof(argv[1]));
154  g_res = atof(argv[2]);
155  g_width = atof(argv[3]);
156  g_height = atof(argv[4]);
157  g_min_free_ratio = atof(argv[5]);
158  g_max_free_ratio = atof(argv[6]);
159 
160  while(ros::Time::now().toSec() == 0.0)
161  {
162  ROS_INFO("Waiting for initial time publication");
163  ros::Duration(0.25).sleep();
164  ros::spinOnce();
165  }
166  ros::Time start_time = ros::Time::now();
167  while((ros::Time::now() - start_time) < delay)
168  {
169  ROS_INFO("Waiting for delay expiration (%.3f - %.3f) < %.3f",
170  ros::Time::now().toSec(),
171  start_time.toSec(),
172  delay.toSec());
173  ros::Duration(0.25).sleep();
174  ros::spinOnce();
175  }
176 
177  int result = RUN_ALL_TESTS();
178  delete g_n;
179  return result;
180 }
d
void checkMapMetaData(const nav_msgs::MapMetaData &map_metadata)
Definition: rtest.cpp:72
ros::NodeHandle * g_n
Definition: rtest.cpp:40
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
bool call(const std::string &service_name, MReq &req, MRes &res)
bool sleep() const
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
double g_min_free_ratio
Definition: rtest.cpp:41
boost::shared_ptr< nav_msgs::MapMetaData const > map_metadata_
Definition: rtest.cpp:65
double g_max_free_ratio
Definition: rtest.cpp:41
double g_width
Definition: rtest.cpp:41
double g_height
Definition: rtest.cpp:41
int main(int argc, char **argv)
Definition: rtest.cpp:143
void checkMapData(const nav_msgs::OccupancyGrid &map)
Definition: rtest.cpp:79
bool sleep() const
#define ROS_INFO(...)
bool got_map_metadata_
Definition: rtest.cpp:64
boost::shared_ptr< nav_msgs::OccupancyGrid const > map_
Definition: rtest.cpp:57
void mapCallback(const boost::shared_ptr< nav_msgs::OccupancyGrid const > &map)
Definition: rtest.cpp:58
MapClientTest()
Definition: rtest.cpp:46
void mapMetaDataCallback(const boost::shared_ptr< nav_msgs::MapMetaData const > &map_metadata)
Definition: rtest.cpp:66
double g_res
Definition: rtest.cpp:41
static Time now()
~MapClientTest()
Definition: rtest.cpp:52
#define ROS_ASSERT(cond)
bool got_map_
Definition: rtest.cpp:56
ROSCPP_DECL void spinOnce()
TEST_F(MapClientTest, call_service)
Definition: rtest.cpp:96
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)


gmapping
Author(s): Brian Gerkey
autogenerated on Wed Jun 5 2019 21:48:25