Merge pull request #123 from orocos/rtt_std_srvs-for-standard-operations

Add wrappers for common operation signatures to rtt_std_srvs

Author: francisco-miguel-almeida

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,148 @@ 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:
+  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;
+
+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 NextVariant {
+    static Ptr connect(RTT::OperationInterfacePart*) { return Ptr(); }
+  };
+
+  template<typename R>
+  struct NextVariant<R, typename EnableIfHasNextVariant<R>::type> {
+    static Ptr connect(RTT::OperationInterfacePart* operation) {
+      return ROSServiceServerOperationCaller<ROS_SERVICE_T, variant + 1>::connect(operation);
+    }
+  };
+
 public:
-  //! Operation caller for a ROS service server proxy
-  typedef RTT::OperationCaller<bool(typename ROS_SERVICE_T::Request&, typename ROS_SERVICE_T::Response&)> ProxyOperationCallerType;
+  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 NextVariant<void>::connect(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);
+  }
+
+  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 +180,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 +195,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 +210,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 +220,7 @@ class ROSServiceClientProxy : public ROSServiceClientProxyBase
 
 
 //! Abstract factory for ROS Service Proxy Factories
-class ROSServiceProxyFactoryBase 
+class ROSServiceProxyFactoryBase
 {
 public:
 
@@ -166,7 +239,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));

typekits/rtt_std_srvs/CMakeLists.txt

@@ -3,7 +3,14 @@ project(rtt_std_srvs)
 
 find_package(catkin REQUIRED COMPONENTS rtt_roscomm)
 
-ros_generate_rtt_service_proxies(std_srvs)
+include_directories(include)
+
+ros_generate_rtt_service_proxies(std_srvs
+  EXTRA_INCLUDES
+    rtt_std_srvs/Empty.h
+    rtt_std_srvs/SetBool.h
+    rtt_std_srvs/Trigger.h
+)
 
 orocos_generate_package(
   DEPENDS rosgraph_msgs