test_interface.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2010, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
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 


rve_interface_gen
Author(s): Josh Faust
autogenerated on Wed Dec 11 2013 14:31:00