rtt_rosservice_proxy.h
Go to the documentation of this file.
00001 #ifndef __RTT_ROSCOMM_RTT_ROSSERVICE_PROXY_H
00002 #define __RTT_ROSCOMM_RTT_ROSSERVICE_PROXY_H
00003 
00004 #include <ros/ros.h>
00005 
00006 #include <rtt/RTT.hpp>
00007 #include <rtt/internal/GlobalEngine.hpp>
00008 #include <rtt/plugin/ServicePlugin.hpp>
00009 
00011 class ROSServiceProxyBase
00012 {
00013 public:
00014   ROSServiceProxyBase(const std::string &service_name) : service_name_(service_name) { }
00015   virtual ~ROSServiceProxyBase() { }
00017   const std::string& getServiceName() const { return service_name_; }
00018 private:
00020   std::string service_name_;
00021 };
00022 
00023 
00025 class ROSServiceServerProxyBase : public ROSServiceProxyBase
00026 { 
00027 public:
00028   ROSServiceServerProxyBase(const std::string &service_name) :
00029     ROSServiceProxyBase(service_name),
00030     proxy_operation_caller_()
00031   { }
00032   
00034   bool connect(RTT::TaskContext *owner, RTT::OperationInterfacePart* operation) {
00035     // Link the caller with the operation
00036     return proxy_operation_caller_->setImplementation(
00037         operation->getLocalOperation(),
00038         RTT::internal::GlobalEngine::Instance());
00039   }
00040 
00041 protected:
00043   ros::ServiceServer server_;
00045   boost::shared_ptr<RTT::base::OperationCallerBaseInvoker> proxy_operation_caller_;
00046 };
00047 
00048 template<class ROS_SERVICE_T>
00049 class ROSServiceServerProxy : public ROSServiceServerProxyBase 
00050 {
00051 public:
00053   typedef RTT::OperationCaller<bool(typename ROS_SERVICE_T::Request&, typename ROS_SERVICE_T::Response&)> ProxyOperationCallerType;
00054 
00058   ROSServiceServerProxy(const std::string &service_name) :
00059     ROSServiceServerProxyBase(service_name)
00060   {
00061     // Construct operation caller
00062     proxy_operation_caller_.reset(new ProxyOperationCallerType("ROS_SERVICE_SERVER_PROXY"));
00063 
00064     // Construct the ROS service server
00065     ros::NodeHandle nh;
00066     server_ = nh.advertiseService(
00067         service_name, 
00068         &ROSServiceServerProxy<ROS_SERVICE_T>::ros_service_callback, 
00069         this);
00070   }
00071 
00072   ~ROSServiceServerProxy()
00073   {
00074     // Clean-up advertized ROS services
00075     server_.shutdown();     
00076   }
00077 
00078 private:
00079   
00081   bool ros_service_callback(typename ROS_SERVICE_T::Request& request, typename ROS_SERVICE_T::Response& response) {
00082     // Downcast the proxy operation caller
00083     ProxyOperationCallerType &proxy_operation_caller = *dynamic_cast<ProxyOperationCallerType*>(proxy_operation_caller_.get());
00084     // Check if the operation caller is ready, and then call it
00085     return proxy_operation_caller_->ready() && proxy_operation_caller(request, response);
00086   }
00087 };
00088 
00089 
00091 class ROSServiceClientProxyBase : public ROSServiceProxyBase
00092 {
00093 public:
00094   ROSServiceClientProxyBase(const std::string &service_name) : 
00095     ROSServiceProxyBase(service_name),
00096     proxy_operation_()
00097   { }
00098 
00100   bool connect(RTT::TaskContext *owner, RTT::base::OperationCallerBaseInvoker* operation_caller) {
00101     return proxy_operation_.get() != NULL &&
00102       operation_caller->setImplementation(
00103           proxy_operation_->getImplementation(),
00104           owner->engine());
00105   }
00106 
00107 protected:
00109   ros::ServiceClient client_;
00111   boost::shared_ptr<RTT::base::OperationBase> proxy_operation_;
00112 };
00113 
00114 template<class ROS_SERVICE_T>
00115 class ROSServiceClientProxy : public ROSServiceClientProxyBase 
00116 {
00117 public:
00118 
00120   typedef RTT::Operation<bool(typename ROS_SERVICE_T::Request&, typename ROS_SERVICE_T::Response&)> ProxyOperationType;
00121 
00122   ROSServiceClientProxy(const std::string &service_name) :
00123     ROSServiceClientProxyBase(service_name)
00124   {
00125     // Construct a new 
00126     proxy_operation_.reset(new ProxyOperationType("ROS_SERVICE_CLIENT_PROXY"));
00127 
00128     // Construct the underlying service client
00129     ros::NodeHandle nh;
00130     client_ = nh.serviceClient<ROS_SERVICE_T>(service_name);
00131 
00132     // Link the operation with the service client
00133     dynamic_cast<ProxyOperationType*>(proxy_operation_.get())->calls(
00134         &ROSServiceClientProxy<ROS_SERVICE_T>::orocos_operation_callback,
00135         this,
00136         RTT::ClientThread);
00137   }
00138 
00139 private:
00140   
00142   bool orocos_operation_callback(typename ROS_SERVICE_T::Request& request, typename ROS_SERVICE_T::Response& response) {
00143     // Make sure the ROS service client exists and then call it (blocking)
00144     return client_.exists() && client_.isValid() && client_.call(request, response);
00145   }
00146 };
00147 
00148 
00150 class ROSServiceProxyFactoryBase 
00151 {
00152 public:
00153 
00154   ROSServiceProxyFactoryBase(const std::string &service_type) : service_type_(service_type) { }
00155 
00157   const std::string& getType() { return service_type_; }
00158 
00160   virtual ROSServiceClientProxyBase* create_client_proxy(const std::string &service_name) = 0;
00162   virtual ROSServiceServerProxyBase* create_server_proxy(const std::string &service_name) = 0;
00163 
00164 private:
00165   std::string service_type_;
00166 };
00167 
00168 template<class ROS_SERVICE_T>
00169 class ROSServiceProxyFactory : public ROSServiceProxyFactoryBase 
00170 {
00171 public:
00172 
00173   ROSServiceProxyFactory(const std::string &service_type) : ROSServiceProxyFactoryBase(service_type) { }
00174 
00175   virtual ROSServiceClientProxyBase* create_client_proxy(const std::string &service_name) {
00176     return new ROSServiceClientProxy<ROS_SERVICE_T>(service_name);
00177   }
00178 
00179   virtual ROSServiceServerProxyBase* create_server_proxy( const std::string &service_name) {
00180     return new ROSServiceServerProxy<ROS_SERVICE_T>(service_name);
00181   }
00182 };
00183 
00184 #endif // ifndef __RTT_ROSCOMM_RTT_ROSSERVICE_PROXY_H


rtt_roscomm
Author(s): Ruben Smits, Jonathan Bohren
autogenerated on Thu Jun 6 2019 18:06:05