ROS service specialization

[ahoarau]

A follow up to the discussion in #83.

std_srvs/Empty:
- 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

I started to implement this automatic conversion :

#include <rtt/RTT.hpp>
#include <rtt/plugin/ServicePlugin.hpp>
#include <rtt/internal/GlobalService.hpp>

#include <rtt_roscomm/rtt_rosservice_registry_service.h>
#include <rtt_roscomm/rtt_rosservice_proxy.h>

////////////////////////////////////////////////////////////////////////////////
#include <std_srvs/Empty.h>
#include <std_srvs/SetBool.h>
#include <std_srvs/Trigger.h>

////////////////////////////////////////////////////////////////////////////////
template<>
class ROSServiceServerProxy<std_srvs::Empty> : public ROSServiceServerProxyBase
{
public:
  //! All possible Operation callers for a ROS service server proxy
  typedef RTT::OperationCaller<bool(typename std_srvs::Empty::Request&, typename std_srvs::Empty::Response&)> ProxyOperationCallerType0;
  typedef RTT::OperationCaller<bool(void)> ProxyOperationCallerType1;
  typedef RTT::OperationCaller<void(void)> ProxyOperationCallerType2;

  /** \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_caller0_.reset(new ProxyOperationCallerType0("ROS_SERVICE_SERVER_PROXY0"));
    proxy_operation_caller1_.reset(new ProxyOperationCallerType1("ROS_SERVICE_SERVER_PROXY1"));
    proxy_operation_caller2_.reset(new ProxyOperationCallerType2("ROS_SERVICE_SERVER_PROXY2"));

    // Construct the ROS service server
    ros::NodeHandle nh;
    server_ = nh.advertiseService(
        service_name,
        &ROSServiceServerProxy<std_srvs::Empty>::ros_service_callback,
        this);
  }
  
  bool connect(RTT::TaskContext *owner, RTT::OperationInterfacePart* operation)
  {
    // Link the caller with the operation
    if(proxy_operation_caller0_->setImplementation(operation->getLocalOperation(),RTT::internal::GlobalEngine::Instance()))
    {
        return true;
    }
    if(proxy_operation_caller1_->setImplementation(operation->getLocalOperation(),RTT::internal::GlobalEngine::Instance()))
    {
        return true;
    }
    if(proxy_operation_caller2_->setImplementation(operation->getLocalOperation(),RTT::internal::GlobalEngine::Instance()))
    {
        return true;
    }
    return false;
   }
   
  ~ROSServiceServerProxy()
  {
    // Clean-up advertized ROS services
    server_.shutdown();
  }

private:
  //! The list of possible operation callers 
  boost::shared_ptr<RTT::base::OperationCallerBaseInvoker> proxy_operation_caller0_;
  boost::shared_ptr<RTT::base::OperationCallerBaseInvoker> proxy_operation_caller1_;
  boost::shared_ptr<RTT::base::OperationCallerBaseInvoker> proxy_operation_caller2_;
  //! The callback called by the ROS service server when this service is invoked
  bool ros_service_callback(std_srvs::Empty::Request& request, std_srvs::Empty::Response& response) {
    // Downcast the proxy operation caller
    ProxyOperationCallerType0 &proxy_operation_caller0 = *dynamic_cast<ProxyOperationCallerType0*>(proxy_operation_caller0_.get());
    if(proxy_operation_caller0_->ready())
    {
        return proxy_operation_caller0(request, response);
    }
    
    ProxyOperationCallerType1 &proxy_operation_caller1 = *dynamic_cast<ProxyOperationCallerType1*>(proxy_operation_caller1_.get());
    if(proxy_operation_caller1_->ready())
    {
        return proxy_operation_caller1();
    }
    
    ProxyOperationCallerType2 &proxy_operation_caller2 = *dynamic_cast<ProxyOperationCallerType2*>(proxy_operation_caller2_.get());
    if(proxy_operation_caller2_->ready())
    {
        proxy_operation_caller2();
        return true;
    }

    return false;
  }
};

namespace rtt_std_srvs_ros_service_proxies {

 bool registerROSServiceProxies() {
   // Get the ros service registry service
   ROSServiceRegistryServicePtr rosservice_registry = ROSServiceRegistryService::Instance();
   if(!rosservice_registry) {
     RTT::log(RTT::Error) << "Could not get an instance of the ROSServiceRegistryService! Not registering service proxies for std_srvs" << RTT::endlog();
     return false;
   }

   // Get the factory registration operation
   RTT::OperationCaller<bool(ROSServiceProxyFactoryBase*)> register_service_factory =
     rosservice_registry->getOperation("registerServiceFactory");

   // Make sure the registration operation is ready
   if(!register_service_factory.ready()) {
     RTT::log(RTT::Error) << "The ROSServiceRegistryService isn't ready! Not registering service proxies for std_srvs" << RTT::endlog();
     return false;
   }

   // True at the end if all factories have been registered
   bool success = true;

   //////////////////////////////////////////////////////////////////////////////
   // Example: success = success && register_service_factory(new ROSServiceProxyFactory<std_srvs::Empty>("std_srvs/Empty"));
   success = success && register_service_factory(new ROSServiceProxyFactory<std_srvs::Empty>("std_srvs/Empty"));
   success = success && register_service_factory(new ROSServiceProxyFactory<std_srvs::SetBool>("std_srvs/SetBool"));
   success = success && register_service_factory(new ROSServiceProxyFactory<std_srvs::Trigger>("std_srvs/Trigger"));

   //////////////////////////////////////////////////////////////////////////////

   return success;
 }

}

extern "C" {
 bool loadRTTPlugin(RTT::TaskContext* c) { return rtt_std_srvs_ros_service_proxies::registerROSServiceProxies(); }
 std::string getRTTPluginName () { return "rtt_std_srvs_ros_service_proxies"; }
 std::string getRTTTargetName () { return OROCOS_TARGET_NAME; }
}

A few remarks :

0. connect indeed need to be virtual.
1. It seems like it could be much sorter (this is only for the server, not client).
2. When trying to connect to the possible implementations, I get a log(Error) : Tried to construct OperationCaller from incompatible local operation. Is there a way to check the signature beforehand ?

[ahoarau]

3. It could be another package outside of rtt_ros_integration is this is too cumbersome, but I feel that Inconsistent RTT_COMPONENT_PATH overlay semantics orocos-toolchain/rtt#161 will be an issue.

[meyerj]

When trying to connect to the possible implementations, I get a log(Error) : Tried to construct OperationCaller from incompatible local operation. Is there a way to check the signature beforehand ?

Unfortunately not without a patch in RTT. The error is emitted in OperationCaller.hpp:164 during the construction of a temporary in the assignment operator in OperationCaller.hpp:222, which is called from OperationCaller<Signature>::setImplementation() in OperationCaller.hpp:335. I also feel that the setImplementation() call should silently return false in case the assigned implementation is not compatible.

It could be another package outside of rtt_ros_integration is this is too cumbersome, but I feel that orocos-toolchain/rtt#161 will be an issue.

Adding this to rtt_std_srvs would be fine and a valuable contribution that does not break backwards-compatibility. Note that there is a pending proposal to move the typekits back to a separate repository, like it has been the case until a few years ago: #96. There was no feedback on this so far and the motivation was to facilitate the release process using bloom, but unfortunately it would require users who build from source to add another repository to their workspace. Whatever change is made to rtt_std_srvs in this repository needs to be merged into https://github.com/orocos/rtt_common_msgs afterwards.