From 524f976288f743f832371b75cbbfd8ccfea55529 Mon Sep 17 00:00:00 2001 From: Johannes Meyer Date: Wed, 8 May 2019 01:40:13 +0200 Subject: [PATCH] rtt_roscomm/rtt_std_srvs: add wrappers for common operation signatures 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. --- .../cmake/GenerateRTTROSCommPackage.cmake.em | 2 + .../rtt_roscomm/rtt_rosservice_proxy.h | 150 ++++++++++++++---- .../src/templates/service/CMakeLists.txt | 12 +- .../service/rtt_rosservice_proxies.cpp.in | 2 +- .../test/transport_tests.cpp | 51 ++++-- typekits/rtt_std_srvs/CMakeLists.txt | 9 +- .../rtt_std_srvs/include/rtt_std_srvs/Empty.h | 32 ++++ .../include/rtt_std_srvs/SetBool.h | 55 +++++++ .../include/rtt_std_srvs/Trigger.h | 44 +++++ 9 files changed, 310 insertions(+), 47 deletions(-) create mode 100644 typekits/rtt_std_srvs/include/rtt_std_srvs/Empty.h create mode 100644 typekits/rtt_std_srvs/include/rtt_std_srvs/SetBool.h create mode 100644 typekits/rtt_std_srvs/include/rtt_std_srvs/Trigger.h diff --git a/rtt_roscomm/cmake/GenerateRTTROSCommPackage.cmake.em b/rtt_roscomm/cmake/GenerateRTTROSCommPackage.cmake.em index 979aec3d..612481bc 100644 --- a/rtt_roscomm/cmake/GenerateRTTROSCommPackage.cmake.em +++ b/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) diff --git a/rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h b/rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h index 994d1d8e..cb715500 100644 --- a/rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h +++ b/rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h @@ -23,67 +23,149 @@ 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 proxy_operation_caller_; }; template -class ROSServiceServerProxy : public ROSServiceServerProxyBase -{ +class ROSServiceServerOperationCallerBase { public: - //! Operation caller for a ROS service server proxy - typedef RTT::OperationCaller ProxyOperationCallerType; + typedef boost::shared_ptr > Ptr; + virtual ~ROSServiceServerOperationCallerBase() {} + virtual bool call(typename ROS_SERVICE_T::Request& request, typename ROS_SERVICE_T::Response& response) const = 0; +}; + +template +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 +struct ROSServiceServerOperationCallerWrapper { + typedef bool Signature(typename ROS_SERVICE_T::Request&, typename ROS_SERVICE_T::Response&); + typedef RTT::OperationCaller ProxyOperationCallerType; + template static bool call(Callable& call, typename ROS_SERVICE_T::Request& request, typename ROS_SERVICE_T::Response& response) { + return call(request, response); + } +}; + +template +class ROSServiceServerOperationCaller : public ROSServiceServerOperationCallerBase { +public: + using typename ROSServiceServerOperationCallerBase::Ptr; + + //! The wrapper type for this variant + typedef ROSServiceServerOperationCallerWrapper Wrapper; + + //! Default operation caller for a ROS service server proxy + 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 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 + struct Void { + typedef void type; + }; + + template + struct EnableIfHasNextVariant {}; + + template + struct EnableIfHasNextVariant::ProxyOperationCallerType>::type> { + typedef R type; + }; + + template + struct DisableIfHasNextVariant { + typedef R type; + }; + + template + struct DisableIfHasNextVariant::type> {}; + + template + static typename EnableIfHasNextVariant::type tryNextVariant(RTT::OperationInterfacePart* operation) { + return ROSServiceServerOperationCaller::connect(operation); + } + + template + static typename DisableIfHasNextVariant::type tryNextVariant(RTT::OperationInterfacePart* operation) { + return Ptr(); + } + + ROSServiceServerOperationCaller(const boost::shared_ptr& impl) + : proxy_operation_caller_(impl) {} + + ProxyOperationCallerTypePtr proxy_operation_caller_; +}; + +template +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_callback, + service_name, + &ROSServiceServerProxy::ros_service_callback, this); } ~ROSServiceServerProxy() { // Clean-up advertized ROS services - server_.shutdown(); + server_.shutdown(); + } + + virtual bool connect(RTT::TaskContext *, RTT::OperationInterfacePart* operation) + { + impl_ = ROSServiceServerOperationCaller::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(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 > impl_; }; @@ -91,7 +173,7 @@ class ROSServiceServerProxy : public ROSServiceServerProxyBase class ROSServiceClientProxyBase : public ROSServiceProxyBase { public: - ROSServiceClientProxyBase(const std::string &service_name) : + ROSServiceClientProxyBase(const std::string &service_name) : ROSServiceProxyBase(service_name), proxy_operation_() { } @@ -107,12 +189,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 proxy_operation_; }; template -class ROSServiceClientProxy : public ROSServiceClientProxyBase +class ROSServiceClientProxy : public ROSServiceClientProxyBase { public: @@ -122,7 +204,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 +219,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 +229,7 @@ class ROSServiceClientProxy : public ROSServiceClientProxyBase //! Abstract factory for ROS Service Proxy Factories -class ROSServiceProxyFactoryBase +class ROSServiceProxyFactoryBase { public: @@ -166,7 +248,7 @@ class ROSServiceProxyFactoryBase }; template -class ROSServiceProxyFactory : public ROSServiceProxyFactoryBase +class ROSServiceProxyFactory : public ROSServiceProxyFactoryBase { public: diff --git a/rtt_roscomm/src/templates/service/CMakeLists.txt b/rtt_roscomm/src/templates/service/CMakeLists.txt index 380d1469..7624c572 100644 --- a/rtt_roscomm/src/templates/service/CMakeLists.txt +++ b/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) diff --git a/rtt_roscomm/src/templates/service/rtt_rosservice_proxies.cpp.in b/rtt_roscomm/src/templates/service/rtt_rosservice_proxies.cpp.in index f41c00b5..2f8a7959 100644 --- a/rtt_roscomm/src/templates/service/rtt_rosservice_proxies.cpp.in +++ b/rtt_roscomm/src/templates/service/rtt_rosservice_proxies.cpp.in @@ -7,7 +7,7 @@ #include //////////////////////////////////////////////////////////////////////////////// -@ROS_SRV_HEADERS@ +@ROS_SRV_HEADERS@@ROS_SRV_EXTRA_HEADERS@ //////////////////////////////////////////////////////////////////////////////// namespace rtt_@ROSPACKAGE@_ros_service_proxies { diff --git a/tests/rtt_roscomm_tests/test/transport_tests.cpp b/tests/rtt_roscomm_tests/test/transport_tests.cpp index 8c832e02..9edb0e1d 100644 --- a/tests/rtt_roscomm_tests/test/transport_tests.cpp +++ b/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,7 +146,9 @@ 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 rosservice; @@ -143,25 +156,43 @@ TEST(TransportTest, ServiceServerTest) 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 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 service_caller0("empty0"); + RTT::OperationCaller service_caller1("empty1"); + RTT::OperationCaller 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)); diff --git a/typekits/rtt_std_srvs/CMakeLists.txt b/typekits/rtt_std_srvs/CMakeLists.txt index f892cc70..7eddda35 100644 --- a/typekits/rtt_std_srvs/CMakeLists.txt +++ b/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 diff --git a/typekits/rtt_std_srvs/include/rtt_std_srvs/Empty.h b/typekits/rtt_std_srvs/include/rtt_std_srvs/Empty.h new file mode 100644 index 00000000..a985b3f6 --- /dev/null +++ b/typekits/rtt_std_srvs/include/rtt_std_srvs/Empty.h @@ -0,0 +1,32 @@ +#ifndef RTT_STD_SRVS_EMPTY_H +#define RTT_STD_SRVS_EMPTY_H + +#include +#include + +// Specialized implementations of ROSServiceServerOperationCaller for std_srvs/Empty. +// +// Accepted signatures: +// - bool empty(std_srvs::Empty::Request&, std_srvs::Empty::Response&) +// - bool empty() +// - void empty() +// + +template<> struct ROSServiceServerOperationCallerWrapper { + typedef bool Signature(); + typedef RTT::OperationCaller ProxyOperationCallerType; + template static bool call(Callable& call, std_srvs::Empty::Request&, std_srvs::Empty::Response&) { + return call(); + } +}; + +template<> struct ROSServiceServerOperationCallerWrapper { + typedef void Signature(); + typedef RTT::OperationCaller ProxyOperationCallerType; + template static bool call(Callable& call, std_srvs::Empty::Request&, std_srvs::Empty::Response&) { + call(); + return true; + } +}; + +#endif // RTT_STD_SRVS_EMPTY_H diff --git a/typekits/rtt_std_srvs/include/rtt_std_srvs/SetBool.h b/typekits/rtt_std_srvs/include/rtt_std_srvs/SetBool.h new file mode 100644 index 00000000..8e075668 --- /dev/null +++ b/typekits/rtt_std_srvs/include/rtt_std_srvs/SetBool.h @@ -0,0 +1,55 @@ +#ifndef RTT_STD_SRVS_SETBOOL_H +#define RTT_STD_SRVS_SETBOOL_H + +#include +#include + +// Specialized implementation of ROSServiceServerOperationCaller for std_srvs/SetBool. +// +// Accepted signatures: +// - bool setBool(std_srvs::SetBool::Request&, std_srvs::SetBool::Response&) +// - 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 +// + +template<> struct ROSServiceServerOperationCallerWrapper { + typedef bool Signature(bool, std::string&); + typedef RTT::OperationCaller ProxyOperationCallerType; + template static bool call(Callable& call, std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response) { + response.success = call(request.data, response.message); + return true; + } +}; + +template<> struct ROSServiceServerOperationCallerWrapper { + typedef bool Signature(bool); + typedef RTT::OperationCaller ProxyOperationCallerType; + template static bool call(Callable& call, std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response) { + response.success = call(request.data); + return true; + } +}; + +template<> struct ROSServiceServerOperationCallerWrapper { + typedef std::string Signature(bool); + typedef RTT::OperationCaller ProxyOperationCallerType; + template static bool call(Callable& call, std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response) { + response.message = call(request.data); + response.success = true; + return true; + } +}; + +template<> struct ROSServiceServerOperationCallerWrapper { + typedef void Signature(bool); + typedef RTT::OperationCaller ProxyOperationCallerType; + template static bool call(Callable& call, std_srvs::SetBool::Request& request, std_srvs::SetBool::Response& response) { + call(request.data); + response.success = true; + return true; + } +}; + +#endif // RTT_STD_SRVS_SETBOOL_H diff --git a/typekits/rtt_std_srvs/include/rtt_std_srvs/Trigger.h b/typekits/rtt_std_srvs/include/rtt_std_srvs/Trigger.h new file mode 100644 index 00000000..6e1f4f49 --- /dev/null +++ b/typekits/rtt_std_srvs/include/rtt_std_srvs/Trigger.h @@ -0,0 +1,44 @@ +#ifndef RTT_STD_SRVS_TRIGGER_H +#define RTT_STD_SRVS_TRIGGER_H + +#include +#include + +// Specialized implementations of ROSServiceServerOperationCaller for std_srvs/Trigger. +// +// Accepted signatures: +// - bool trigger(std_srvs::Trigger::Request&, std_srvs::Trigger::Response&) +// - bool trigger(std::string &message_out) +// - bool trigger() // response.message will be empty +// - std::string trigger() // response.success = true +// + +template<> struct ROSServiceServerOperationCallerWrapper { + typedef bool Signature(std::string &message_out); + typedef RTT::OperationCaller ProxyOperationCallerType; + template static bool call(Callable& call, std_srvs::Trigger::Request& request, std_srvs::Trigger::Response& response) { + response.success = call(response.message); + return true; + } +}; + +template<> struct ROSServiceServerOperationCallerWrapper { + typedef bool Signature(); + typedef RTT::OperationCaller ProxyOperationCallerType; + template static bool call(Callable& call, std_srvs::Trigger::Request& request, std_srvs::Trigger::Response& response) { + response.success = call(); + return true; + } +}; + +template<> struct ROSServiceServerOperationCallerWrapper { + typedef std::string Signature(); + typedef RTT::OperationCaller ProxyOperationCallerType; + template static bool call(Callable& call, std_srvs::Trigger::Request& request, std_srvs::Trigger::Response& response) { + response.message = call(); + response.success = true; + return true; + } +}; + +#endif // RTT_STD_SRVS_TRIGGER_H