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
00033
00034
00035
00036 #include <string>
00037
00038 #include <gtest/gtest.h>
00039
00040 #include "ros/ros.h"
00041 #include "ros/time.h"
00042 #include "ros/service.h"
00043 #include "ros/connection.h"
00044 #include "ros/service_client.h"
00045 #include <test_roscpp/TestStringString.h>
00046 #include <test_roscpp/BadTestStringString.h>
00047
00048 TEST(SrvCall, callSrv)
00049 {
00050 test_roscpp::TestStringString::Request req;
00051 test_roscpp::TestStringString::Response res;
00052
00053 req.str = std::string("case_FLIP");
00054
00055 ASSERT_TRUE(ros::service::waitForService("service_adv"));
00056 ASSERT_TRUE(ros::service::call("service_adv", req, res));
00057
00058 ASSERT_STREQ(res.str.c_str(), "CASE_flip");
00059 }
00060
00061 TEST(SrvCall, callSrvMultipleTimes)
00062 {
00063 test_roscpp::TestStringString::Request req;
00064 test_roscpp::TestStringString::Response res;
00065
00066 req.str = std::string("case_FLIP");
00067
00068 ASSERT_TRUE(ros::service::waitForService("service_adv"));
00069
00070 ros::Time start = ros::Time::now();
00071
00072 for (int i = 0; i < 100; ++i)
00073 {
00074 ASSERT_TRUE(ros::service::call("service_adv", req, res));
00075 }
00076
00077 ros::Time end = ros::Time::now();
00078 ros::Duration d = end - start;
00079 printf("100 calls took %f secs\n", d.toSec());
00080
00081 ASSERT_STREQ(res.str.c_str(), "CASE_flip");
00082 }
00083
00084 TEST(SrvCall, callSrvWithWrongType)
00085 {
00086 test_roscpp::BadTestStringString::Request req;
00087 test_roscpp::BadTestStringString::Response res;
00088
00089 ASSERT_TRUE(ros::service::waitForService("service_adv"));
00090
00091 for ( int i = 0; i < 4; ++i )
00092 {
00093 bool call_result = ros::service::call("service_adv", req, res);
00094 ASSERT_FALSE(call_result);
00095 }
00096 }
00097
00098 TEST(SrvCall, callSrvHandle)
00099 {
00100 test_roscpp::TestStringString::Request req;
00101 test_roscpp::TestStringString::Response res;
00102
00103 req.str = std::string("case_FLIP");
00104
00105 std::map<std::string, std::string> header;
00106 header["test1"] = "testing 1";
00107 header["test2"] = "testing 2";
00108 ros::NodeHandle nh;
00109 ros::ServiceClient handle = nh.serviceClient<test_roscpp::TestStringString>("service_adv", false, header);
00110 ASSERT_TRUE(handle.waitForExistence());
00111
00112 ros::Time start = ros::Time::now();
00113
00114 for (int i = 0; i < 100; ++i)
00115 {
00116 ASSERT_TRUE(handle.call(req, res));
00117 }
00118
00119 ros::Time end = ros::Time::now();
00120 ros::Duration d = end - start;
00121 printf("100 calls took %f secs\n", d.toSec());
00122
00123 ASSERT_STREQ(res.str.c_str(), "CASE_flip");
00124 }
00125
00126 TEST(SrvCall, callSrvPersistentHandle)
00127 {
00128 test_roscpp::TestStringString::Request req;
00129 test_roscpp::TestStringString::Response res;
00130
00131 req.str = std::string("case_FLIP");
00132
00133 ASSERT_TRUE(ros::service::waitForService("service_adv"));
00134
00135 std::map<std::string, std::string> header;
00136 header["test1"] = "testing 1";
00137 header["test2"] = "testing 2";
00138 ros::NodeHandle nh;
00139 ros::ServiceClient handle = nh.serviceClient<test_roscpp::TestStringString>("service_adv", true, header);
00140
00141 ros::Time start = ros::Time::now();
00142
00143 for (int i = 0; i < 10000; ++i)
00144 {
00145 ASSERT_TRUE(handle.call(req, res));
00146 }
00147
00148 ros::Time end = ros::Time::now();
00149 ros::Duration d = end - start;
00150 printf("10000 calls took %f secs\n", d.toSec());
00151
00152 ASSERT_STREQ(res.str.c_str(), "CASE_flip");
00153 }
00154
00155 TEST(SrvCall, callSrvLongRunning)
00156 {
00157 test_roscpp::TestStringString::Request req;
00158 test_roscpp::TestStringString::Response res;
00159
00160 req.str = std::string("case_FLIP");
00161
00162 ASSERT_TRUE(ros::service::waitForService("service_adv_long"));
00163 ASSERT_TRUE(ros::service::call("service_adv_long", req, res));
00164
00165 ASSERT_STREQ(res.str.c_str(), "CASE_flip");
00166 }
00167
00168 TEST(SrvCall, callSrvWhichUnadvertisesInCallback)
00169 {
00170 test_roscpp::TestStringString::Request req;
00171 test_roscpp::TestStringString::Response res;
00172
00173 req.str = std::string("case_FLIP");
00174
00175 ASSERT_TRUE(ros::service::waitForService("service_adv_unadv_in_callback"));
00176 ASSERT_FALSE(ros::service::call("service_adv_unadv_in_callback", req, res));
00177 }
00178
00179 TEST(SrvCall, handleValid)
00180 {
00181 test_roscpp::TestStringString::Request req;
00182 test_roscpp::TestStringString::Response res;
00183
00184 req.str = std::string("case_FLIP");
00185
00186 ASSERT_TRUE(ros::service::waitForService("service_adv"));
00187
00188 std::map<std::string, std::string> header;
00189 header["test1"] = "testing 1";
00190 header["test2"] = "testing 2";
00191 ros::NodeHandle nh;
00192 ros::ServiceClient handle = nh.serviceClient<test_roscpp::TestStringString>("service_adv", true, header);
00193 ASSERT_TRUE(handle.call(req, res));
00194 ASSERT_TRUE(handle.isValid());
00195 handle.shutdown();
00196 ASSERT_FALSE(handle.isValid());
00197
00198 ASSERT_STREQ(res.str.c_str(), "CASE_flip");
00199 }
00200
00201 TEST(SrvCall, waitForServiceTimeout)
00202 {
00203 ros::NodeHandle nh;
00204 ASSERT_FALSE(ros::service::waitForService("iojergoiwjoiewg", 1000));
00205 ASSERT_FALSE(ros::service::waitForService("iojergoiwjoiewg", ros::Duration(1)));
00206
00207 ros::ServiceClient handle = nh.serviceClient<test_roscpp::TestStringString>("migowiowejowieuhwejg", false);
00208 ASSERT_FALSE(handle.waitForExistence(ros::Duration(1)));
00209 }
00210
00211 int
00212 main(int argc, char** argv)
00213 {
00214 testing::InitGoogleTest(&argc, argv);
00215
00216 ros::init(argc, argv, "service_call");
00217 ros::NodeHandle nh;
00218
00219 int ret = RUN_ALL_TESTS();
00220
00221
00222
00223 return ret;
00224 }
00225
00226