diff --git a/rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h b/rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h index 8db777d1..ee4d8fc3 100644 --- a/rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h +++ b/rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h @@ -7,6 +7,9 @@ #include #include +#include +#include + //! Abstract ROS Service Proxy class ROSServiceProxyBase { @@ -46,7 +49,9 @@ class ROSServiceServerOperationCallerBase { }; template -struct ROSServiceServerOperationCallerWrapper; +struct ROSServiceServerOperationCallerWrapper { + typedef void ProxyOperationCallerType; +}; // 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 @@ -66,7 +71,7 @@ struct ROSServiceServerOperationCallerWrapper { template class ROSServiceServerOperationCaller : public ROSServiceServerOperationCallerBase { public: - using typename ROSServiceServerOperationCallerBase::Ptr; + typedef typename ROSServiceServerOperationCallerBase::Ptr Ptr; //! The wrapper type for this variant typedef ROSServiceServerOperationCallerWrapper Wrapper; @@ -75,14 +80,7 @@ class ROSServiceServerOperationCaller : public ROSServiceServerOperationCallerBa typedef typename Wrapper::ProxyOperationCallerType ProxyOperationCallerType; typedef boost::shared_ptr ProxyOperationCallerTypePtr; - static Ptr connect(RTT::OperationInterfacePart* operation) { - ProxyOperationCallerTypePtr proxy_operation_caller - = boost::make_shared(operation->getLocalOperation(), RTT::internal::GlobalEngine::Instance()); - if (proxy_operation_caller->ready()) { - return Ptr(new ROSServiceServerOperationCaller(proxy_operation_caller)); - } - return NextVariant::connect(operation); - } + static Ptr connect(RTT::OperationInterfacePart* 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. @@ -91,32 +89,41 @@ class ROSServiceServerOperationCaller : public ROSServiceServerOperationCallerBa } private: - template struct Void { typedef void type; }; + ROSServiceServerOperationCaller(const boost::shared_ptr& impl) + : proxy_operation_caller_(impl) {} - template struct EnableIfHasNextVariant { }; + ProxyOperationCallerTypePtr proxy_operation_caller_; +}; - template struct EnableIfHasNextVariant::ProxyOperationCallerType>::type> { - typedef R type; - }; +namespace { - template - struct NextVariant { - static Ptr connect(RTT::OperationInterfacePart*) { return Ptr(); } - }; +template +struct ROSServiceServerOperationCallerWrapperNextVariant { + typedef typename ROSServiceServerOperationCallerBase::Ptr Ptr; + static Ptr connect(RTT::OperationInterfacePart*) { return Ptr(); } +}; - template - struct NextVariant::type> { - static Ptr connect(RTT::OperationInterfacePart* operation) { - return ROSServiceServerOperationCaller::connect(operation); - } - }; +template +struct ROSServiceServerOperationCallerWrapperNextVariant::ProxyOperationCallerType> >::type> { + typedef typename ROSServiceServerOperationCallerBase::Ptr Ptr; + static Ptr connect(RTT::OperationInterfacePart* operation) { + return ROSServiceServerOperationCaller::connect(operation); + } +}; - ROSServiceServerOperationCaller(const boost::shared_ptr& impl) - : proxy_operation_caller_(impl) {} +} - ProxyOperationCallerTypePtr proxy_operation_caller_; -}; +template +typename ROSServiceServerOperationCaller::Ptr +ROSServiceServerOperationCaller::connect(RTT::OperationInterfacePart* operation) { + ProxyOperationCallerTypePtr proxy_operation_caller + = boost::make_shared(operation->getLocalOperation(), RTT::internal::GlobalEngine::Instance()); + if (proxy_operation_caller->ready()) { + return Ptr(new ROSServiceServerOperationCaller(proxy_operation_caller)); + } + return ROSServiceServerOperationCallerWrapperNextVariant::connect(operation); +} template class ROSServiceServerProxy : public ROSServiceServerProxyBase