00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include <gtest/gtest.h>
00033 #include <ros/service.h>
00034 #include <ros/ros.h>
00035 #include <nav_msgs/GetMap.h>
00036 #include <nav_msgs/OccupancyGrid.h>
00037 #include <nav_msgs/MapMetaData.h>
00038
00039
00040 ros::NodeHandle* g_n=NULL;
00041 double g_res, g_width, g_height, g_min_free_ratio, g_max_free_ratio;
00042
00043 class MapClientTest : public testing::Test
00044 {
00045 public:
00046 MapClientTest()
00047 {
00048 got_map_ = false;
00049 got_map_metadata_ = false;
00050 }
00051
00052 ~MapClientTest()
00053 {
00054 }
00055
00056 bool got_map_;
00057 boost::shared_ptr<nav_msgs::OccupancyGrid const> map_;
00058 void mapCallback(const boost::shared_ptr<nav_msgs::OccupancyGrid const>& map)
00059 {
00060 map_ = map;
00061 got_map_ = true;
00062 }
00063
00064 bool got_map_metadata_;
00065 boost::shared_ptr<nav_msgs::MapMetaData const> map_metadata_;
00066 void mapMetaDataCallback(const boost::shared_ptr<nav_msgs::MapMetaData const>& map_metadata)
00067 {
00068 map_metadata_ = map_metadata;
00069 got_map_metadata_ = true;
00070 }
00071
00072 void checkMapMetaData(const nav_msgs::MapMetaData& map_metadata)
00073 {
00074 EXPECT_FLOAT_EQ(map_metadata.resolution, g_res);
00075 EXPECT_FLOAT_EQ(map_metadata.width, g_width);
00076 EXPECT_FLOAT_EQ(map_metadata.height, g_height);
00077 }
00078
00079 void checkMapData(const nav_msgs::OccupancyGrid& map)
00080 {
00081 unsigned int i;
00082 unsigned int free_cnt = 0;
00083 for(i=0; i < map.info.width * map.info.height; i++)
00084 {
00085 if(map.data[i] == 0)
00086 free_cnt++;
00087 }
00088 double free_ratio = free_cnt / (double)(i);
00089 EXPECT_GE(free_ratio, g_min_free_ratio);
00090 EXPECT_LE(free_ratio, g_max_free_ratio);
00091 }
00092 };
00093
00094
00095 TEST_F(MapClientTest, call_service)
00096 {
00097 nav_msgs::GetMap::Request req;
00098 nav_msgs::GetMap::Response resp;
00099 ASSERT_TRUE(ros::service::waitForService("dynamic_map", 5000));
00100 ASSERT_TRUE(ros::service::call("dynamic_map", req, resp));
00101 checkMapMetaData(resp.map.info);
00102 checkMapData(resp.map);
00103 }
00104
00105
00106 TEST_F(MapClientTest, subscribe_topic)
00107 {
00108 ros::Subscriber sub = g_n->subscribe<nav_msgs::OccupancyGrid>("map", 1, boost::bind(&MapClientTest::mapCallback, this, _1));
00109
00110
00111 int i=20;
00112 while(!got_map_ && i > 0)
00113 {
00114 ros::spinOnce();
00115 ros::WallDuration d(0.25);
00116 d.sleep();
00117 i--;
00118 }
00119 ASSERT_TRUE(got_map_);
00120 checkMapMetaData(map_->info);
00121 checkMapData(*(map_.get()));
00122 }
00123
00124
00125 TEST_F(MapClientTest, subscribe_topic_metadata)
00126 {
00127 ros::Subscriber sub = g_n->subscribe<nav_msgs::MapMetaData>("map_metadata", 1, boost::bind(&MapClientTest::mapMetaDataCallback, this, _1));
00128
00129
00130 int i=20;
00131 while(!got_map_metadata_ && i > 0)
00132 {
00133 ros::spinOnce();
00134 ros::WallDuration d(0.25);
00135 d.sleep();
00136 i--;
00137 }
00138 ASSERT_TRUE(got_map_metadata_);
00139 checkMapMetaData(*(map_metadata_.get()));
00140 }
00141
00142 int main(int argc, char **argv)
00143 {
00144 testing::InitGoogleTest(&argc, argv);
00145
00146 ros::init(argc, argv, "map_client_test");
00147 g_n = new ros::NodeHandle();
00148
00149
00150
00151 ROS_ASSERT(argc == 7);
00152 ros::Duration delay = ros::Duration(atof(argv[1]));
00153 g_res = atof(argv[2]);
00154 g_width = atof(argv[3]);
00155 g_height = atof(argv[4]);
00156 g_min_free_ratio = atof(argv[5]);
00157 g_max_free_ratio = atof(argv[6]);
00158
00159 while(ros::Time::now().toSec() == 0.0)
00160 {
00161 ROS_INFO("Waiting for initial time publication");
00162 ros::Duration(0.25).sleep();
00163 ros::spinOnce();
00164 }
00165 ros::Time start_time = ros::Time::now();
00166 while((ros::Time::now() - start_time) < delay)
00167 {
00168 ROS_INFO("Waiting for delay expiration (%.3f - %.3f) < %.3f",
00169 ros::Time::now().toSec(),
00170 start_time.toSec(),
00171 delay.toSec());
00172 ros::Duration(0.25).sleep();
00173 ros::spinOnce();
00174 }
00175
00176 int result = RUN_ALL_TESTS();
00177 delete g_n;
00178 return result;
00179 }