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 #include "test_constants.h"
40 
41 int g_argc;
42 char** g_argv;
43 
44 class MapClientTest : public testing::Test
45 {
46  public:
47  // A node is needed to make a service call
49 
51  {
52  ros::init(g_argc, g_argv, "map_client_test");
53  n_ = new ros::NodeHandle();
54  got_map_ = false;
55  got_map_metadata_ = false;
56  }
57 
59  {
60  delete n_;
61  }
62 
63  bool got_map_;
66  {
67  map_ = map;
68  got_map_ = true;
69  }
70 
74  {
75  map_metadata_ = map_metadata;
76  got_map_metadata_ = true;
77  }
78 };
79 
80 /* Try to retrieve the map via service, and compare to ground truth */
81 TEST_F(MapClientTest, call_service)
82 {
83  nav_msgs::GetMap::Request req;
84  nav_msgs::GetMap::Response resp;
85  ASSERT_TRUE(ros::service::waitForService("static_map", 5000));
86  ASSERT_TRUE(ros::service::call("static_map", req, resp));
87  ASSERT_FLOAT_EQ(resp.map.info.resolution, g_valid_image_res);
88  ASSERT_EQ(resp.map.info.width, g_valid_image_width);
89  ASSERT_EQ(resp.map.info.height, g_valid_image_height);
90  ASSERT_STREQ(resp.map.header.frame_id.c_str(), "map");
91  for(unsigned int i=0; i < resp.map.info.width * resp.map.info.height; i++)
92  ASSERT_EQ(g_valid_image_content[i], resp.map.data[i]);
93 }
94 
95 /* Try to retrieve the map via topic, and compare to ground truth */
96 TEST_F(MapClientTest, subscribe_topic)
97 {
98  ros::Subscriber sub = n_->subscribe<nav_msgs::OccupancyGrid>("map", 1, boost::bind(&MapClientTest::mapCallback, this, _1));
99 
100  // Try a few times, because the server may not be up yet.
101  int i=20;
102  while(!got_map_ && i > 0)
103  {
104  ros::spinOnce();
106  d.sleep();
107  i--;
108  }
109  ASSERT_TRUE(got_map_);
110  ASSERT_FLOAT_EQ(map_->info.resolution, g_valid_image_res);
111  ASSERT_EQ(map_->info.width, g_valid_image_width);
112  ASSERT_EQ(map_->info.height, g_valid_image_height);
113  ASSERT_STREQ(map_->header.frame_id.c_str(), "map");
114  for(unsigned int i=0; i < map_->info.width * map_->info.height; i++)
115  ASSERT_EQ(g_valid_image_content[i], map_->data[i]);
116 }
117 
118 /* Try to retrieve the metadata via topic, and compare to ground truth */
119 TEST_F(MapClientTest, subscribe_topic_metadata)
120 {
121  ros::Subscriber sub = n_->subscribe<nav_msgs::MapMetaData>("map_metadata", 1, boost::bind(&MapClientTest::mapMetaDataCallback, this, _1));
122 
123  // Try a few times, because the server may not be up yet.
124  int i=20;
125  while(!got_map_metadata_ && i > 0)
126  {
127  ros::spinOnce();
129  d.sleep();
130  i--;
131  }
132  ASSERT_TRUE(got_map_metadata_);
133  ASSERT_FLOAT_EQ(map_metadata_->resolution, g_valid_image_res);
134  ASSERT_EQ(map_metadata_->width, g_valid_image_width);
135  ASSERT_EQ(map_metadata_->height, g_valid_image_height);
136 }
137 
138 int main(int argc, char **argv)
139 {
140  testing::InitGoogleTest(&argc, argv);
141 
142  g_argc = argc;
143  g_argv = argv;
144 
145  return RUN_ALL_TESTS();
146 }
d
char ** g_argv
Definition: rtest.cpp:42
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
ros::NodeHandle * n_
Definition: rtest.cpp:48
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)
boost::shared_ptr< nav_msgs::MapMetaData const > map_metadata_
Definition: rtest.cpp:72
const unsigned int g_valid_image_height
const unsigned int g_valid_image_width
int main(int argc, char **argv)
Definition: rtest.cpp:138
Duration & fromSec(double t)
bool got_map_metadata_
Definition: rtest.cpp:71
boost::shared_ptr< nav_msgs::OccupancyGrid const > map_
Definition: rtest.cpp:64
void mapCallback(const boost::shared_ptr< nav_msgs::OccupancyGrid const > &map)
Definition: rtest.cpp:65
const float g_valid_image_res
MapClientTest()
Definition: rtest.cpp:50
void mapMetaDataCallback(const boost::shared_ptr< nav_msgs::MapMetaData const > &map_metadata)
Definition: rtest.cpp:73
int g_argc
Definition: rtest.cpp:41
~MapClientTest()
Definition: rtest.cpp:58
bool got_map_
Definition: rtest.cpp:63
ROSCPP_DECL void spinOnce()
TEST_F(MapClientTest, call_service)
Definition: rtest.cpp:81
const char g_valid_image_content[]
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)


map_server
Author(s): Brian Gerkey, Tony Pratkanis, contradict@gmail.com
autogenerated on Thu Jan 21 2021 04:05:35