rtt_roscomm/rtt_std_srvs: add wrappers for common operation signature…

…s to rtt_std_srvs (fix #101)

With this patch it is possible to create ROS service servers for some common operation signatures out of the box,
without the need to write custom wrappers or to add extra operations with the ROS specific callback signature.

Supported service types and associated signatures:

std_srvs/Empty:
- bool empty()                     // The service call fails if empty() returns false!
                                   // Use std_srvs/Trigger if the result should be returned as the response.
- void empty()

std_srvs/SetBool:
- bool setBool(bool, std::string &message_out)
- bool setBool(bool)               // response.message will be empty
- std::string setBool(bool)        // response.success = true
- void setBool(bool)               // response.success = true and response.message will be empty

std_srvs/Trigger:
- bool trigger(std::string &message_out)
- bool trigger()                   // response.message will be empty
- std::string trigger()            // response.success = true

The approach can be easily extended to other ROS service types.

rtt_roscomm/cmake/GenerateRTTROSCommPackage.cmake.em

@@ -42,11 +42,13 @@ macro(rtt_roscomm_debug)
 endmacro()
 
 macro(ros_generate_rtt_typekit package)
+  cmake_parse_arguments(ros_generate_rtt_typekit "" "" "EXTRA_INCLUDES" ${ARGN})
   set(_package ${package})
   add_subdirectory(${rtt_roscomm_TEMPLATES_DIR}/typekit ${package}_typekit)
 endmacro(ros_generate_rtt_typekit)
 
 macro(ros_generate_rtt_service_proxies package)
+  cmake_parse_arguments(ros_generate_rtt_service_proxies "" "" "EXTRA_INCLUDES" ${ARGN})
   set(_package ${package})
   add_subdirectory(${rtt_roscomm_TEMPLATES_DIR}/service ${package}_service_proxies)
 endmacro(ros_generate_rtt_service_proxies)

rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h

@@ -23,75 +23,159 @@ class ROSServiceProxyBase
 
 //! Abstract ROS Service Server Proxy
 class ROSServiceServerProxyBase : public ROSServiceProxyBase
-{ 
+{
 public:
   ROSServiceServerProxyBase(const std::string &service_name) :
-    ROSServiceProxyBase(service_name),
-    proxy_operation_caller_()
+    ROSServiceProxyBase(service_name)
   { }
-  
+
   //! Connect an RTT Operation to this ROS service server
-  bool connect(RTT::TaskContext *owner, RTT::OperationInterfacePart* operation) {
-    // Link the caller with the operation
-    return proxy_operation_caller_->setImplementation(
-        operation->getLocalOperation(),
-        RTT::internal::GlobalEngine::Instance());
-  }
+  virtual bool connect(RTT::TaskContext *owner, RTT::OperationInterfacePart* operation) = 0;
 
 protected:
   //! The underlying ROS service server
   ros::ServiceServer server_;
-  //! The underlying RTT operation caller
-  boost::shared_ptr<RTT::base::OperationCallerBaseInvoker> proxy_operation_caller_;
 };
 
 template<class ROS_SERVICE_T>
-class ROSServiceServerProxy : public ROSServiceServerProxyBase 
-{
+class ROSServiceServerOperationCallerBase {
 public:
-  //! Operation caller for a ROS service server proxy
-  typedef RTT::OperationCaller<bool(typename ROS_SERVICE_T::Request&, typename ROS_SERVICE_T::Response&)> ProxyOperationCallerType;
+  typedef boost::shared_ptr<ROSServiceServerOperationCallerBase<ROS_SERVICE_T> > Ptr;
+  virtual ~ROSServiceServerOperationCallerBase() {}
+  virtual bool call(typename ROS_SERVICE_T::Request& request, typename ROS_SERVICE_T::Response& response) const = 0;
+};
+
+template<class ROS_SERVICE_T, int variant = 0>
+struct ROSServiceServerOperationCallerWrapper;
 
+// Default implementation of an OperationCaller that fowards ROS service calls to Orocos operations
+// that have the default bool(Request&, Response&) signature. You can add more variants of this class
+// to add support for custom operation types.
+//
+// See package std_srvs for an example.
+//
+template<class ROS_SERVICE_T>
+struct ROSServiceServerOperationCallerWrapper<ROS_SERVICE_T,0> {
+  typedef bool Signature(typename ROS_SERVICE_T::Request&, typename ROS_SERVICE_T::Response&);
+  typedef RTT::OperationCaller<Signature> ProxyOperationCallerType;
+  template <typename Callable> static bool call(Callable& call, typename ROS_SERVICE_T::Request& request, typename ROS_SERVICE_T::Response& response) {
+    return call(request, response);
+  }
+};
+
+template<class ROS_SERVICE_T, int variant = 0>
+class ROSServiceServerOperationCaller : public ROSServiceServerOperationCallerBase<ROS_SERVICE_T> {
+public:
+  using typename ROSServiceServerOperationCallerBase<ROS_SERVICE_T>::Ptr;
+
+  //! The wrapper type for this variant
+  typedef ROSServiceServerOperationCallerWrapper<ROS_SERVICE_T, variant> Wrapper;
+
+  //! Default operation caller for a ROS service server proxy
+  typedef typename Wrapper::ProxyOperationCallerType ProxyOperationCallerType;
+  typedef boost::shared_ptr<ProxyOperationCallerType> ProxyOperationCallerTypePtr;
+
+  static Ptr connect(RTT::OperationInterfacePart* operation) {
+    ProxyOperationCallerTypePtr proxy_operation_caller
+        = boost::make_shared<ProxyOperationCallerType>(operation->getLocalOperation(), RTT::internal::GlobalEngine::Instance());
+    if (proxy_operation_caller->ready()) {
+      return Ptr(new ROSServiceServerOperationCaller<ROS_SERVICE_T, variant>(proxy_operation_caller));
+    }
+    return tryNextVariant(operation);
+  }
+
+  virtual bool call(typename ROS_SERVICE_T::Request& request, typename ROS_SERVICE_T::Response& response) const {
+    // Check if the operation caller is ready, and then call it.
+    if (!proxy_operation_caller_->ready()) return false;
+    return Wrapper::call(*proxy_operation_caller_, request, response);
+  }
+
+private:
+  template<typename Dummy>
+  struct Void {
+    typedef void type;
+  };
+
+  template<typename R = void, typename Enabled = void>
+  struct EnableIfHasNextVariant {};
+
+  template<typename R>
+  struct EnableIfHasNextVariant<R, typename Void<typename ROSServiceServerOperationCallerWrapper<ROS_SERVICE_T, variant + 1>::ProxyOperationCallerType>::type> {
+    typedef R type;
+  };
+
+  template<typename R = void, typename Enabled = void>
+  struct DisableIfHasNextVariant {
+    typedef R type;
+  };
+
+  template<typename R>
+  struct DisableIfHasNextVariant<R, typename EnableIfHasNextVariant<R>::type> {};
+
+  template<typename Dummy = int>
+  static Ptr tryNextVariant(RTT::OperationInterfacePart* operation,
+                            typename EnableIfHasNextVariant<Dummy>::type* = 0) {
+    return ROSServiceServerOperationCaller<ROS_SERVICE_T, variant + 1>::connect(operation);
+  }
+
+  template<typename Dummy = void>
+  static Ptr tryNextVariant(RTT::OperationInterfacePart* operation,
+                            typename DisableIfHasNextVariant<Dummy>::type* = 0) {
+    return Ptr();
+  }
+
+  ROSServiceServerOperationCaller(const boost::shared_ptr<ProxyOperationCallerType>& impl)
+      : proxy_operation_caller_(impl) {}
+
+  ProxyOperationCallerTypePtr proxy_operation_caller_;
+};
+
+template<class ROS_SERVICE_T>
+class ROSServiceServerProxy : public ROSServiceServerProxyBase
+{
+public:
   /** \brief Construct a ROS service server and associate it with an Orocos
    * task's required interface and operation caller.
    */
   ROSServiceServerProxy(const std::string &service_name) :
     ROSServiceServerProxyBase(service_name)
   {
-    // Construct operation caller
-    proxy_operation_caller_.reset(new ProxyOperationCallerType("ROS_SERVICE_SERVER_PROXY"));
-
     // Construct the ROS service server
     ros::NodeHandle nh;
     server_ = nh.advertiseService(
-        service_name, 
-        &ROSServiceServerProxy<ROS_SERVICE_T>::ros_service_callback, 
+        service_name,
+        &ROSServiceServerProxy<ROS_SERVICE_T>::ros_service_callback,
         this);
   }
 
   ~ROSServiceServerProxy()
   {
     // Clean-up advertized ROS services
-    server_.shutdown();     
+    server_.shutdown();
+  }
+
+  virtual bool connect(RTT::TaskContext *, RTT::OperationInterfacePart* operation)
+  {
+    impl_ = ROSServiceServerOperationCaller<ROS_SERVICE_T>::connect(operation);
+    return (impl_.get() != 0);
   }
 
 private:
-
   //! The callback called by the ROS service server when this service is invoked
   bool ros_service_callback(typename ROS_SERVICE_T::Request& request, typename ROS_SERVICE_T::Response& response) {
-    // Downcast the proxy operation caller
-    ProxyOperationCallerType &proxy_operation_caller = *dynamic_cast<ProxyOperationCallerType*>(proxy_operation_caller_.get());
-    // Check if the operation caller is ready, and then call it
-    return proxy_operation_caller_->ready() && proxy_operation_caller(request, response);
+    if (!impl_) return false;
+    return impl_->call(request, response);
   }
+
+  boost::shared_ptr<ROSServiceServerOperationCallerBase<ROS_SERVICE_T> > impl_;
 };
 
 
 //! Abstract ROS Service Client Proxy
 class ROSServiceClientProxyBase : public ROSServiceProxyBase
 {
 public:
-  ROSServiceClientProxyBase(const std::string &service_name) : 
+  ROSServiceClientProxyBase(const std::string &service_name) :
     ROSServiceProxyBase(service_name),
     proxy_operation_()
   { }
@@ -107,12 +191,12 @@ class ROSServiceClientProxyBase : public ROSServiceProxyBase
 protected:
   //! The underlying ROS service client
   ros::ServiceClient client_;
-  //! The underlying RTT operation 
+  //! The underlying RTT operation
   boost::shared_ptr<RTT::base::OperationBase> proxy_operation_;
 };
 
 template<class ROS_SERVICE_T>
-class ROSServiceClientProxy : public ROSServiceClientProxyBase 
+class ROSServiceClientProxy : public ROSServiceClientProxyBase
 {
 public:
 
@@ -122,7 +206,7 @@ class ROSServiceClientProxy : public ROSServiceClientProxyBase
   ROSServiceClientProxy(const std::string &service_name) :
     ROSServiceClientProxyBase(service_name)
   {
-    // Construct a new 
+    // Construct a new
     proxy_operation_.reset(new ProxyOperationType("ROS_SERVICE_CLIENT_PROXY"));
 
     // Construct the underlying service client
@@ -137,7 +221,7 @@ class ROSServiceClientProxy : public ROSServiceClientProxyBase
   }
 
 private:
-  
+
   //! The callback for the RTT operation
   bool orocos_operation_callback(typename ROS_SERVICE_T::Request& request, typename ROS_SERVICE_T::Response& response) {
     // Make sure the ROS service client exists and then call it (blocking)
@@ -147,7 +231,7 @@ class ROSServiceClientProxy : public ROSServiceClientProxyBase
 
 
 //! Abstract factory for ROS Service Proxy Factories
-class ROSServiceProxyFactoryBase 
+class ROSServiceProxyFactoryBase
 {
 public:
 
@@ -166,7 +250,7 @@ class ROSServiceProxyFactoryBase
 };
 
 template<class ROS_SERVICE_T>
-class ROSServiceProxyFactory : public ROSServiceProxyFactoryBase 
+class ROSServiceProxyFactory : public ROSServiceProxyFactoryBase
 {
 public:
 

rtt_roscomm/src/templates/service/CMakeLists.txt

@@ -76,6 +76,14 @@ foreach( FILE ${SRV_FILES} )
 
 endforeach()
 
+# Include custom headers provided by the typekit package
+set(ROS_SRV_EXTRA_HEADERS)
+if(ros_generate_rtt_service_proxies_EXTRA_INCLUDES)
+  foreach( header ${ros_generate_rtt_service_proxies_EXTRA_INCLUDES} )
+    set(ROS_SRV_EXTRA_HEADERS "${ROS_SRV_EXTRA_HEADERS}#include <${header}>\n")
+  endforeach()
+endif()
+
 # Service proxy factories
 configure_file(
   rtt_rosservice_proxies.cpp.in
@@ -89,7 +97,9 @@ include_directories(
   ${${_package}_INCLUDE_DIRS})
 
 # Targets
-set(CMAKE_BUILD_TYPE MinSizeRel)
+if(NOT DEFINED CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "Release")
+  set(CMAKE_BUILD_TYPE MinSizeRel)
+endif()
 orocos_service(         rtt_${ROSPACKAGE}_rosservice_proxies ${CMAKE_CURRENT_BINARY_DIR}/rtt_rosservice_proxies.cpp)
 target_link_libraries(  rtt_${ROSPACKAGE}_rosservice_proxies ${catkin_LIBRARIES} ${USE_OROCOS_LIBRARIES})
 if(DEFINED ${_package}_EXPORTED_TARGETS)

rtt_roscomm/src/templates/service/rtt_rosservice_proxies.cpp.in

@@ -7,7 +7,7 @@
 #include <rtt_roscomm/rtt_rosservice_proxy.h>
 
 ////////////////////////////////////////////////////////////////////////////////
-@ROS_SRV_HEADERS@
+@ROS_SRV_HEADERS@@ROS_SRV_EXTRA_HEADERS@
 ////////////////////////////////////////////////////////////////////////////////
 
 namespace rtt_@ROSPACKAGE@_ros_service_proxies { 

tests/rtt_roscomm_tests/test/transport_tests.cpp

@@ -118,12 +118,23 @@ TEST(TransportTest, VectorTest)
 }
 
 static int callback_called = 0;
-bool callback(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
+bool callback0(std_srvs::Empty::Request&, std_srvs::Empty::Response&)
 {
   ++callback_called;
   return true;
 }
 
+bool callback1()
+{
+  ++callback_called;
+  return true;
+}
+
+void callback2()
+{
+  ++callback_called;
+}
+
 TEST(TransportTest, ServiceServerTest)
 {
   std::string service = ros::names::resolve("~empty");
@@ -135,33 +146,53 @@ TEST(TransportTest, ServiceServerTest)
 
   // Create a TaskContext
   RTT::TaskContext *tc = new RTT::TaskContext("TaskContext");
-  tc->addOperation("empty", &callback);
+  tc->addOperation("empty0", &callback0);
+  tc->addOperation("empty1", &callback1);
+  tc->addOperation("empty2", &callback2);
 
   // Load the rosservice service
   boost::weak_ptr<rtt_rosservice::ROSService> rosservice;
   rosservice = tc->getProvider<rtt_rosservice::ROSService>("rosservice");
   ASSERT_FALSE(rosservice.expired());
 
   // Create a service server
-  EXPECT_TRUE(rosservice.lock()->connect("empty", service, "std_srvs/Empty"));
+  EXPECT_TRUE(rosservice.lock()->connect("empty0", service + "0", "std_srvs/Empty"));
+  EXPECT_TRUE(rosservice.lock()->connect("empty1", service + "1", "std_srvs/Empty"));
+  EXPECT_TRUE(rosservice.lock()->connect("empty2", service + "2", "std_srvs/Empty"));
 
   // Check that the service server has been successfully registered:
-  EXPECT_TRUE(ros::ServiceManager::instance()->lookupServicePublication(service).get());
+  EXPECT_TRUE(ros::ServiceManager::instance()->lookupServicePublication(service + "0").get());
+  EXPECT_TRUE(ros::ServiceManager::instance()->lookupServicePublication(service + "1").get());
+  EXPECT_TRUE(ros::ServiceManager::instance()->lookupServicePublication(service + "2").get());
 
   // Create a service client
-  RTT::OperationCaller<bool(std_srvs::Empty::Request&, std_srvs::Empty::Response&)> service_caller("empty");
-  tc->requires()->addOperationCaller(service_caller);
-  EXPECT_TRUE(rosservice.lock()->connect("empty", service, "std_srvs/Empty"));
-  EXPECT_TRUE(service_caller.ready());
+  RTT::OperationCaller<bool(std_srvs::Empty::Request&, std_srvs::Empty::Response&)> service_caller0("empty0");
+  RTT::OperationCaller<bool(std_srvs::Empty::Request&, std_srvs::Empty::Response&)> service_caller1("empty1");
+  RTT::OperationCaller<bool(std_srvs::Empty::Request&, std_srvs::Empty::Response&)> service_caller2("empty2");
+  tc->requires()->addOperationCaller(service_caller0);
+  tc->requires()->addOperationCaller(service_caller1);
+  tc->requires()->addOperationCaller(service_caller2);
+  EXPECT_TRUE(rosservice.lock()->connect("empty0", service + "0", "std_srvs/Empty"));
+  EXPECT_TRUE(service_caller0.ready());
+  EXPECT_TRUE(rosservice.lock()->connect("empty1", service + "1", "std_srvs/Empty"));
+  EXPECT_TRUE(service_caller1.ready());
+  EXPECT_TRUE(rosservice.lock()->connect("empty2", service + "2", "std_srvs/Empty"));
+  EXPECT_TRUE(service_caller2.ready());
 
   // Call the service
   EXPECT_EQ(0, callback_called);
   std_srvs::Empty empty;
-  EXPECT_TRUE(service_caller(empty.request, empty.response));
+  EXPECT_TRUE(service_caller0(empty.request, empty.response));
   EXPECT_EQ(1, callback_called);
+  EXPECT_TRUE(service_caller1(empty.request, empty.response));
+  EXPECT_EQ(2, callback_called);
+  EXPECT_TRUE(service_caller2(empty.request, empty.response));
+  EXPECT_EQ(3, callback_called);
 
   // Disconnect the service
-  EXPECT_TRUE(rosservice.lock()->disconnect(service));
+  EXPECT_TRUE(rosservice.lock()->disconnect(service + "0"));
+  EXPECT_TRUE(rosservice.lock()->disconnect(service + "1"));
+  EXPECT_TRUE(rosservice.lock()->disconnect(service + "2"));
 
   // Check that the service server has been destroyed
   EXPECT_FALSE(ros::ServiceManager::instance()->lookupServicePublication(service));