Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add wrappers for common operation signatures to rtt_std_srvs #123

Merged
merged 3 commits into from
May 8, 2019
Merged

Add wrappers for common operation signatures to rtt_std_srvs #123

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
524f976
rtt_roscomm/rtt_std_srvs: add wrappers for common operation signature…
meyerj May 7, 2019
e53cde1
fix build for pre-C++11 compatibility
francisco-miguel-almeida May 8, 2019
fd14e71
fix the build correctly
francisco-miguel-almeida May 8, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 2 additions & 0 deletions rtt_roscomm/cmake/GenerateRTTROSCommPackage.cmake.em
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
150 changes: 116 additions & 34 deletions rtt_roscomm/include/rtt_roscomm/rtt_rosservice_proxy.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,75 +23,157 @@ 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 typename EnableIfHasNextVariant<Ptr>::type tryNextVariant(RTT::OperationInterfacePart* operation) {
return ROSServiceServerOperationCaller<ROS_SERVICE_T, variant + 1>::connect(operation);
}

template<typename Dummy = void>
static typename DisableIfHasNextVariant<Ptr>::type tryNextVariant(RTT::OperationInterfacePart* operation) {
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_()
{ }
Expand All @@ -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<RTT::base::OperationBase> proxy_operation_;
};

template<class ROS_SERVICE_T>
class ROSServiceClientProxy : public ROSServiceClientProxyBase
class ROSServiceClientProxy : public ROSServiceClientProxyBase
{
public:

Expand All @@ -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
Expand All @@ -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)
Expand All @@ -147,7 +229,7 @@ class ROSServiceClientProxy : public ROSServiceClientProxyBase


//! Abstract factory for ROS Service Proxy Factories
class ROSServiceProxyFactoryBase
class ROSServiceProxyFactoryBase
{
public:

Expand All @@ -166,7 +248,7 @@ class ROSServiceProxyFactoryBase
};

template<class ROS_SERVICE_T>
class ROSServiceProxyFactory : public ROSServiceProxyFactoryBase
class ROSServiceProxyFactory : public ROSServiceProxyFactoryBase
{
public:

Expand Down
12 changes: 11 additions & 1 deletion rtt_roscomm/src/templates/service/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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)
Expand Down
2 changes: 1 addition & 1 deletion rtt_roscomm/src/templates/service/rtt_rosservice_proxies.cpp.in
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down
51 changes: 41 additions & 10 deletions tests/rtt_roscomm_tests/test/transport_tests.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -118,12 +118,23 @@ TEST(TransportTest, VectorTest)
}

static int callback_called = 0;
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

question: do we control in which thread the callbacks are made? If not, maybe this counter should be atomic. Not important right now, just curious.

Copy link
Member Author

@meyerj meyerj May 8, 2019 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The callbacks are executed by a thread pool owned by a ros::AsyncSpinner in rtt_rosnode. They could execute in parallel, but the service calls in the unit tests are synchronous and block until the callback has been processed and the results are returned through the network stack and roscpp. So no concurrency.

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");
Expand All @@ -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));
Expand Down
9 changes: 8 additions & 1 deletion typekits/rtt_std_srvs/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
Loading