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 #include "test_constants.h"
00040
00041 int g_argc;
00042 char** g_argv;
00043
00044 class MapClientTest : public testing::Test
00045 {
00046 public:
00047
00048 ros::NodeHandle* n_;
00049
00050 MapClientTest()
00051 {
00052 ros::init(g_argc, g_argv, "map_client_test");
00053 n_ = new ros::NodeHandle();
00054 got_map_ = false;
00055 got_map_metadata_ = false;
00056 }
00057
00058 ~MapClientTest()
00059 {
00060 delete n_;
00061 }
00062
00063 bool got_map_;
00064 boost::shared_ptr<nav_msgs::OccupancyGrid const> map_;
00065 void mapCallback(const boost::shared_ptr<nav_msgs::OccupancyGrid const>& map)
00066 {
00067 map_ = map;
00068 got_map_ = true;
00069 }
00070
00071 bool got_map_metadata_;
00072 boost::shared_ptr<nav_msgs::MapMetaData const> map_metadata_;
00073 void mapMetaDataCallback(const boost::shared_ptr<nav_msgs::MapMetaData const>& map_metadata)
00074 {
00075 map_metadata_ = map_metadata;
00076 got_map_metadata_ = true;
00077 }
00078 };
00079
00080
00081 TEST_F(MapClientTest, call_service)
00082 {
00083 nav_msgs::GetMap::Request req;
00084 nav_msgs::GetMap::Response resp;
00085 ASSERT_TRUE(ros::service::waitForService("static_map", 5000));
00086 ASSERT_TRUE(ros::service::call("static_map", req, resp));
00087 ASSERT_FLOAT_EQ(resp.map.info.resolution, g_valid_image_res);
00088 ASSERT_EQ(resp.map.info.width, g_valid_image_width);
00089 ASSERT_EQ(resp.map.info.height, g_valid_image_height);
00090 ASSERT_STREQ(resp.map.header.frame_id.c_str(), "map");
00091 for(unsigned int i=0; i < resp.map.info.width * resp.map.info.height; i++)
00092 ASSERT_EQ(g_valid_image_content[i], resp.map.data[i]);
00093 }
00094
00095
00096 TEST_F(MapClientTest, subscribe_topic)
00097 {
00098 ros::Subscriber sub = n_->subscribe<nav_msgs::OccupancyGrid>("map", 1, boost::bind(&MapClientTest::mapCallback, this, _1));
00099
00100
00101 int i=20;
00102 while(!got_map_ && i > 0)
00103 {
00104 ros::spinOnce();
00105 ros::Duration d = ros::Duration().fromSec(0.25);
00106 d.sleep();
00107 i--;
00108 }
00109 ASSERT_TRUE(got_map_);
00110 ASSERT_FLOAT_EQ(map_->info.resolution, g_valid_image_res);
00111 ASSERT_EQ(map_->info.width, g_valid_image_width);
00112 ASSERT_EQ(map_->info.height, g_valid_image_height);
00113 ASSERT_STREQ(map_->header.frame_id.c_str(), "map");
00114 for(unsigned int i=0; i < map_->info.width * map_->info.height; i++)
00115 ASSERT_EQ(g_valid_image_content[i], map_->data[i]);
00116 }
00117
00118
00119 TEST_F(MapClientTest, subscribe_topic_metadata)
00120 {
00121 ros::Subscriber sub = n_->subscribe<nav_msgs::MapMetaData>("map_metadata", 1, boost::bind(&MapClientTest::mapMetaDataCallback, this, _1));
00122
00123
00124 int i=20;
00125 while(!got_map_metadata_ && i > 0)
00126 {
00127 ros::spinOnce();
00128 ros::Duration d = ros::Duration().fromSec(0.25);
00129 d.sleep();
00130 i--;
00131 }
00132 ASSERT_TRUE(got_map_metadata_);
00133 ASSERT_FLOAT_EQ(map_metadata_->resolution, g_valid_image_res);
00134 ASSERT_EQ(map_metadata_->width, g_valid_image_width);
00135 ASSERT_EQ(map_metadata_->height, g_valid_image_height);
00136 }
00137
00138 int main(int argc, char **argv)
00139 {
00140 testing::InitGoogleTest(&argc, argv);
00141
00142 g_argc = argc;
00143 g_argv = argv;
00144
00145 return RUN_ALL_TESTS();
00146 }