Go to the documentation of this file.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 #include <rve_interface_gen/Foo.h>
00031 #include <rve_interface_gen/Foo_multipleReturnRequest.h>
00032 #include <rve_interface_gen/Foo_multipleReturnResponse.h>
00033 #include <rve_rpc/call_handle.h>
00034 #include <rve_rpc/method_response.h>
00035 #include <rve_rpc/server.h>
00036 #include <rve_rpc/client.h>
00037
00038 #include <gtest/gtest.h>
00039
00040 #include <ros/ros.h>
00041
00042 using namespace rve_interface_gen;
00043
00044 class MyFooServer : public FooServer
00045 {
00046 public:
00047 MyFooServer(rve_rpc::Server& server)
00048 : FooServer(server)
00049 , async_call_done(false)
00050 , async_fastcall_done(false)
00051 {}
00052
00053 virtual void triple( uint32_t val , uint32_t& out_val )
00054 {
00055 out_val = val * 3;
00056 }
00057
00058 virtual void blockFor( const ros::Duration& d )
00059 {
00060 d.sleep();
00061 }
00062
00063 virtual void noBlock( )
00064 {
00065 async_call_done = false;
00066 ros::Duration(1.0).sleep();
00067 async_call_done = true;
00068 }
00069
00070 virtual void complex( const std_msgs::String& name , float val , std_msgs::String& out_ret )
00071 {
00072 std::stringstream ss;
00073 ss << name << val;
00074 out_ret.data = ss.str();
00075 }
00076
00077 virtual void multipleReturn(multipleReturnHandle& handle)
00078 {
00079 multipleReturnResponsePtr res(new multipleReturnResponse);
00080 res->val = handle.getRequest()->val;
00081 res->val_doubled = res->val * 2;
00082 handle.respond(res);
00083 }
00084
00085 virtual void withString(const std::string& str)
00086 {
00087
00088 }
00089
00090 bool async_call_done;
00091 bool async_fastcall_done;
00092 };
00093
00094 class Interfaces : public ::testing::Test
00095 {
00096 public:
00097 Interfaces()
00098 : spinner_(1)
00099 {
00100 spinner_.start();
00101 }
00102
00103 virtual void SetUp()
00104 {
00105 rpc_server_ = new rve_rpc::Server("test", nh_);
00106 rpc_client_ = new rve_rpc::Client("test", nh_);
00107 server_ = new MyFooServer(*rpc_server_);
00108 proxy_ = new FooProxy(*rpc_client_);
00109
00110 rpc_server_->ready();
00111 rpc_client_->connect();
00112 }
00113
00114 virtual void TearDown()
00115 {
00116 delete rpc_server_;
00117 delete rpc_client_;
00118 delete proxy_;
00119 delete server_;
00120
00121 }
00122
00123 ros::NodeHandle nh_;
00124 ros::AsyncSpinner spinner_;
00125 rve_rpc::Server* rpc_server_;
00126 rve_rpc::Client* rpc_client_;
00127 MyFooServer* server_;
00128 FooProxy* proxy_;
00129 };
00130
00131 TEST_F(Interfaces, callWithReturn)
00132 {
00133 uint32_t ret = 0;
00134 proxy_->triple(5, ret);
00135 EXPECT_EQ(ret, 15U);
00136 }
00137
00138 TEST_F(Interfaces, blockingCall)
00139 {
00140 ros::Time start = ros::Time::now();
00141 proxy_->blockFor(ros::Duration(1.0));
00142 ros::Time end = ros::Time::now();
00143 EXPECT_GE(end, start + ros::Duration(1.0));
00144 }
00145
00146 TEST_F(Interfaces, asyncCall)
00147 {
00148 proxy_->noBlockAsync();
00149 EXPECT_FALSE(server_->async_call_done);
00150 }
00151
00152 TEST_F(Interfaces, multipleReturn)
00153 {
00154 uint32_t ret1 = 0;
00155 uint32_t ret2 = 0;
00156 proxy_->multipleReturn(5, ret1, ret2);
00157 EXPECT_EQ(ret1, 5U);
00158 EXPECT_EQ(ret2, 10U);
00159 }
00160
00161 TEST_F(Interfaces, fastcallWithReturn)
00162 {
00163 Foo::multipleReturnRequestPtr req(new Foo::multipleReturnRequest);
00164 req->val = 5;
00165 Foo::multipleReturnResponseConstPtr res = proxy_->multipleReturn(req);
00166 EXPECT_EQ(res->val, 5U);
00167 EXPECT_EQ(res->val_doubled, 10U);
00168 }
00169
00170 struct AsyncHelper
00171 {
00172 AsyncHelper()
00173 : received(false)
00174 {}
00175
00176 void cb(const Foo::multipleReturnMethodResponse& mr)
00177 {
00178 this->mr = mr;
00179 received = true;
00180 }
00181
00182 Foo::multipleReturnMethodResponse mr;
00183 bool received;
00184 };
00185
00186 TEST_F(Interfaces, callAsyncReturn)
00187 {
00188 AsyncHelper ah;
00189 proxy_->multipleReturnAsync(5, boost::bind(&AsyncHelper::cb, &ah, _1));
00190
00191 while (!ah.received)
00192 {
00193 ros::WallDuration(0.01).sleep();
00194 }
00195
00196 EXPECT_EQ(ah.mr.response->val, 5U);
00197 EXPECT_EQ(ah.mr.response->val_doubled, 10U);
00198 }
00199
00200 TEST_F(Interfaces, fastcallAsyncReturn)
00201 {
00202 Foo::multipleReturnRequestPtr req(new Foo::multipleReturnRequest);
00203 req->val = 5;
00204 AsyncHelper ah;
00205 proxy_->multipleReturnAsync(req, boost::bind(&AsyncHelper::cb, &ah, _1));
00206
00207 while (!ah.received)
00208 {
00209 ros::WallDuration(0.01).sleep();
00210 }
00211
00212 EXPECT_EQ(ah.mr.response->val, 5U);
00213 EXPECT_EQ(ah.mr.response->val_doubled, 10U);
00214 }
00215
00216
00217 int main(int argc, char** argv)
00218 {
00219 testing::InitGoogleTest(&argc, argv);
00220 ros::init(argc, argv, "test_interface");
00221 ros::NodeHandle nh;
00222
00223 return RUN_ALL_TESTS();
00224 }
00225