service_client.hpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #ifndef UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED
00006 #define UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED
00007 
00008 #include <uavcan/build_config.hpp>
00009 #include <uavcan/util/multiset.hpp>
00010 #include <uavcan/node/generic_publisher.hpp>
00011 #include <uavcan/node/generic_subscriber.hpp>
00012 
00013 #if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
00014 # error UAVCAN_CPP_VERSION
00015 #endif
00016 
00017 #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
00018 # include <functional>
00019 #endif
00020 
00021 namespace uavcan
00022 {
00023 /**
00024  * This struct describes a pending service call.
00025  * Refer to @ref ServiceClient to learn more about service calls.
00026  */
00027 struct ServiceCallID
00028 {
00029     NodeID server_node_id;
00030     TransferID transfer_id;
00031 
00032     ServiceCallID() { }
00033 
00034     ServiceCallID(NodeID arg_server_node_id, TransferID arg_transfer_id)
00035         : server_node_id(arg_server_node_id)
00036         , transfer_id(arg_transfer_id)
00037     { }
00038 
00039     bool operator==(const ServiceCallID rhs) const
00040     {
00041         return (rhs.server_node_id == server_node_id) &&
00042                (rhs.transfer_id == transfer_id);
00043     }
00044 
00045     bool isValid() const { return server_node_id.isUnicast(); }
00046 };
00047 
00048 /**
00049  * Object of this type will be returned to the application as a result of service call.
00050  * Note that application ALWAYS gets this result, even when it times out or fails because of some other reason.
00051  * The class is made noncopyable because it keeps a reference to a stack-allocated object.
00052  */
00053 template <typename DataType>
00054 class UAVCAN_EXPORT ServiceCallResult : Noncopyable
00055 {
00056 public:
00057     typedef ReceivedDataStructure<typename DataType::Response> ResponseFieldType;
00058 
00059     enum Status { Success, ErrorTimeout };
00060 
00061 private:
00062     const Status status_;               ///< Whether successful or not. Failure to decode the response causes timeout.
00063     ServiceCallID call_id_;             ///< Identifies the call
00064     ResponseFieldType& response_;       ///< Returned data structure. Value undefined if the service call has failed.
00065 
00066 public:
00067     ServiceCallResult(Status arg_status, ServiceCallID arg_call_id, ResponseFieldType& arg_response)
00068         : status_(arg_status)
00069         , call_id_(arg_call_id)
00070         , response_(arg_response)
00071     {
00072         UAVCAN_ASSERT(call_id_.isValid());
00073         UAVCAN_ASSERT((status_ == Success) || (status_ == ErrorTimeout));
00074     }
00075 
00076     /**
00077      * Shortcut to quickly check whether the call was successful.
00078      */
00079     bool isSuccessful() const { return status_ == Success; }
00080 
00081     Status getStatus() const { return status_; }
00082 
00083     ServiceCallID getCallID() const { return call_id_; }
00084 
00085     /**
00086      * Returned reference points to a stack-allocated object.
00087      */
00088     const ResponseFieldType& getResponse() const { return response_; }
00089     ResponseFieldType& getResponse() { return response_; }
00090 };
00091 
00092 /**
00093  * This operator neatly prints the service call result prepended with extra data like Server Node ID.
00094  * The extra data will be represented as YAML comment.
00095  */
00096 template <typename Stream, typename DataType>
00097 static Stream& operator<<(Stream& s, const ServiceCallResult<DataType>& scr)
00098 {
00099     s << "# Service call result [" << DataType::getDataTypeFullName() << "] "
00100       << (scr.isSuccessful() ? "OK" : "FAILURE")
00101       << " server_node_id=" << int(scr.getCallID().server_node_id.get())
00102       << " tid=" << int(scr.getCallID().transfer_id.get()) << "\n";
00103     if (scr.isSuccessful())
00104     {
00105         s << scr.getResponse();
00106     }
00107     else
00108     {
00109         s << "# (no data)";
00110     }
00111     return s;
00112 }
00113 
00114 /**
00115  * Do not use directly.
00116  */
00117 class ServiceClientBase : protected ITransferAcceptanceFilter
00118                         , protected DeadlineHandler
00119 {
00120     const DataTypeDescriptor* data_type_descriptor_;  ///< This will be initialized at the time of first call
00121 
00122 protected:
00123     class CallState : DeadlineHandler
00124     {
00125         ServiceClientBase& owner_;
00126         const ServiceCallID id_;
00127         bool timed_out_;
00128 
00129         virtual void handleDeadline(MonotonicTime);
00130 
00131     public:
00132         CallState(INode& node, ServiceClientBase& owner, ServiceCallID call_id)
00133             : DeadlineHandler(node.getScheduler())
00134             , owner_(owner)
00135             , id_(call_id)
00136             , timed_out_(false)
00137         {
00138             UAVCAN_ASSERT(id_.isValid());
00139             DeadlineHandler::startWithDelay(owner_.request_timeout_);
00140         }
00141 
00142         ServiceCallID getCallID() const { return id_; }
00143 
00144         bool hasTimedOut() const { return timed_out_; }
00145 
00146         static bool hasTimedOutPredicate(const CallState& cs) { return cs.hasTimedOut(); }
00147 
00148         bool operator==(const CallState& rhs) const
00149         {
00150             return (&owner_ == &rhs.owner_) && (id_ == rhs.id_);
00151         }
00152     };
00153 
00154     struct CallStateMatchingPredicate
00155     {
00156         const ServiceCallID id;
00157         CallStateMatchingPredicate(ServiceCallID reference) : id(reference) { }
00158         bool operator()(const CallState& state) const { return (state.getCallID() == id) && !state.hasTimedOut(); }
00159     };
00160 
00161     struct ServerSearchPredicate
00162     {
00163         const NodeID server_node_id;
00164         ServerSearchPredicate(NodeID nid) : server_node_id(nid) { }
00165         bool operator()(const CallState& state) const { return state.getCallID().server_node_id == server_node_id; }
00166     };
00167 
00168     MonotonicDuration request_timeout_;
00169 
00170     ServiceClientBase(INode& node)
00171         : DeadlineHandler(node.getScheduler())
00172         , data_type_descriptor_(UAVCAN_NULLPTR)
00173         , request_timeout_(getDefaultRequestTimeout())
00174     { }
00175 
00176     virtual ~ServiceClientBase() { }
00177 
00178     int prepareToCall(INode& node, const char* dtname, NodeID server_node_id, ServiceCallID& out_call_id);
00179 
00180 public:
00181     /**
00182      * It's not recommended to override default timeouts.
00183      * Change of this value will not affect pending calls.
00184      */
00185     static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(1000); }
00186     static MonotonicDuration getMinRequestTimeout() { return MonotonicDuration::fromMSec(10); }
00187     static MonotonicDuration getMaxRequestTimeout() { return MonotonicDuration::fromMSec(60000); }
00188 };
00189 
00190 /**
00191  * Use this class to invoke services on remote nodes.
00192  *
00193  * This class can manage multiple concurrent calls to the same or different remote servers. Number of concurrent
00194  * calls is limited only by amount of available pool memory.
00195  *
00196  * Note that the reference passed to the callback points to a stack-allocated object, which means that the
00197  * reference invalidates once the callback returns. If you want to use this object after the callback execution,
00198  * you need to copy it somewhere.
00199  *
00200  * Note that by default, service client objects use lower priority than message publishers. Use @ref setPriority()
00201  * to override the default if necessary.
00202  *
00203  * @tparam DataType_        Service data type.
00204  *
00205  * @tparam Callback_        Service response will be delivered through the callback of this type.
00206  *                          In C++11 mode this type defaults to std::function<>.
00207  *                          In C++03 mode this type defaults to a plain function pointer; use binder to
00208  *                          call member functions as callbacks.
00209  */
00210 template <typename DataType_,
00211 #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
00212           typename Callback_ = std::function<void (const ServiceCallResult<DataType_>&)>
00213 #else
00214           typename Callback_ = void (*)(const ServiceCallResult<DataType_>&)
00215 #endif
00216           >
00217 class UAVCAN_EXPORT ServiceClient
00218     : public GenericSubscriber<DataType_,
00219                                typename DataType_::Response, TransferListenerWithFilter>
00220     , public ServiceClientBase
00221 {
00222 public:
00223     typedef DataType_ DataType;
00224     typedef typename DataType::Request RequestType;
00225     typedef typename DataType::Response ResponseType;
00226     typedef ServiceCallResult<DataType> ServiceCallResultType;
00227     typedef Callback_ Callback;
00228 
00229 private:
00230     typedef ServiceClient<DataType, Callback> SelfType;
00231     typedef GenericPublisher<DataType, RequestType> PublisherType;
00232     typedef GenericSubscriber<DataType, ResponseType, TransferListenerWithFilter> SubscriberType;
00233 
00234     typedef Multiset<CallState> CallRegistry;
00235 
00236     struct TimeoutCallbackCaller
00237     {
00238         ServiceClient& owner;
00239 
00240         TimeoutCallbackCaller(ServiceClient& arg_owner) : owner(arg_owner) { }
00241 
00242         void operator()(const CallState& state)
00243         {
00244             if (state.hasTimedOut())
00245             {
00246                 UAVCAN_TRACE("ServiceClient::TimeoutCallbackCaller", "Timeout from nid=%d, tid=%d, dtname=%s",
00247                              int(state.getCallID().server_node_id.get()), int(state.getCallID().transfer_id.get()),
00248                              DataType::getDataTypeFullName());
00249 
00250                 typename SubscriberType::ReceivedDataStructureSpec rx_struct; // Default-initialized
00251 
00252                 ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, state.getCallID(),
00253                                              rx_struct);    // Mutable!
00254 
00255                 owner.invokeCallback(result);
00256             }
00257         }
00258     };
00259 
00260     CallRegistry call_registry_;
00261 
00262     PublisherType publisher_;
00263     Callback callback_;
00264 
00265     virtual bool shouldAcceptFrame(const RxFrame& frame) const; // Called from the transfer listener
00266 
00267     void invokeCallback(ServiceCallResultType& result);
00268 
00269     virtual void handleReceivedDataStruct(ReceivedDataStructure<ResponseType>& response);
00270 
00271     virtual void handleDeadline(MonotonicTime);
00272 
00273     int addCallState(ServiceCallID call_id);
00274 
00275 public:
00276     /**
00277      * @param node      Node instance this client will be registered with.
00278      * @param callback  Callback instance. Optional, can be assigned later.
00279      */
00280     explicit ServiceClient(INode& node, const Callback& callback = Callback())
00281         : SubscriberType(node)
00282         , ServiceClientBase(node)
00283         , call_registry_(node.getAllocator())
00284         , publisher_(node, getDefaultRequestTimeout())
00285         , callback_(callback)
00286     {
00287         setPriority(TransferPriority::MiddleLower);
00288         setRequestTimeout(getDefaultRequestTimeout());
00289 #if UAVCAN_DEBUG
00290         UAVCAN_ASSERT(getRequestTimeout() == getDefaultRequestTimeout());  // Making sure default values are OK
00291 #endif
00292     }
00293 
00294     virtual ~ServiceClient() { cancelAllCalls(); }
00295 
00296     /**
00297      * Shall be called before first use.
00298      * Returns negative error code.
00299      */
00300     int init()
00301     {
00302         return publisher_.init();
00303     }
00304 
00305     /**
00306      * Shall be called before first use.
00307      * This overload allows to set the priority, otherwise it's the same.
00308      * Returns negative error code.
00309      */
00310     int init(TransferPriority priority)
00311     {
00312         return publisher_.init(priority);
00313     }
00314 
00315     /**
00316      * Performs non-blocking service call.
00317      * This method transmits the service request and returns immediately.
00318      *
00319      * Service response will be delivered into the application via the callback.
00320      * Note that the callback will ALWAYS be called even if the service call times out; the
00321      * actual result of the call (success/failure) will be passed to the callback as well.
00322      *
00323      * Returns negative error code.
00324      */
00325     int call(NodeID server_node_id, const RequestType& request);
00326 
00327     /**
00328      * Same as plain @ref call() above, but this overload also returns the call ID of the new call.
00329      * The call ID structure can be used to cancel this request later if needed.
00330      */
00331     int call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id);
00332 
00333     /**
00334      * Cancels certain call referred via call ID structure.
00335      */
00336     void cancelCall(ServiceCallID call_id);
00337 
00338     /**
00339      * Cancels all pending calls.
00340      */
00341     void cancelAllCalls();
00342 
00343     /**
00344      * Checks whether there's currently a pending call addressed to the specified node ID.
00345      */
00346     bool hasPendingCallToServer(NodeID server_node_id) const;
00347 
00348     /**
00349      * This method allows to traverse pending calls. If the index is out of range, an invalid call ID will be returned.
00350      * Warning: average complexity is O(index); worst case complexity is O(size).
00351      */
00352     ServiceCallID getCallIDByIndex(unsigned index) const;
00353 
00354     /**
00355      * Service response callback must be set prior service call.
00356      */
00357     const Callback& getCallback() const { return callback_; }
00358     void setCallback(const Callback& cb) { callback_ = cb; }
00359 
00360     /**
00361      * Complexity is O(N) of number of pending calls.
00362      * Note that the number of pending calls will not be updated until the callback is executed.
00363      */
00364     unsigned getNumPendingCalls() const { return call_registry_.getSize(); }
00365 
00366     /**
00367      * Complexity is O(1).
00368      * Note that the number of pending calls will not be updated until the callback is executed.
00369      */
00370     bool hasPendingCalls() const { return !call_registry_.isEmpty(); }
00371 
00372     /**
00373      * Returns the number of failed attempts to decode received response. Generally, a failed attempt means either:
00374      * - Transient failure in the transport layer.
00375      * - Incompatible data types.
00376      */
00377     uint32_t getResponseFailureCount() const { return SubscriberType::getFailureCount(); }
00378 
00379     /**
00380      * Request timeouts. Note that changing the request timeout will not affect calls that are already pending.
00381      * There is no such config as TX timeout - TX timeouts are configured automagically according to request timeouts.
00382      * Not recommended to change.
00383      */
00384     MonotonicDuration getRequestTimeout() const { return request_timeout_; }
00385     void setRequestTimeout(MonotonicDuration timeout)
00386     {
00387         timeout = max(timeout, getMinRequestTimeout());
00388         timeout = min(timeout, getMaxRequestTimeout());
00389 
00390         publisher_.setTxTimeout(timeout);
00391         request_timeout_ = max(timeout, publisher_.getTxTimeout());  // No less than TX timeout
00392     }
00393 
00394     /**
00395      * Priority of outgoing request transfers.
00396      * The remote server is supposed to use the same priority for the response, but it's not guaranteed by
00397      * the specification.
00398      */
00399     TransferPriority getPriority() const { return publisher_.getPriority(); }
00400     void setPriority(const TransferPriority prio) { publisher_.setPriority(prio); }
00401 };
00402 
00403 // ----------------------------------------------------------------------------
00404 
00405 template <typename DataType_, typename Callback_>
00406 void ServiceClient<DataType_, Callback_>::invokeCallback(ServiceCallResultType& result)
00407 {
00408     if (coerceOrFallback<bool>(callback_, true))
00409     {
00410         callback_(result);
00411     }
00412     else
00413     {
00414         handleFatalError("Srv client clbk");
00415     }
00416 }
00417 
00418 template <typename DataType_, typename Callback_>
00419 bool ServiceClient<DataType_, Callback_>::shouldAcceptFrame(const RxFrame& frame) const
00420 {
00421     UAVCAN_ASSERT(frame.getTransferType() == TransferTypeServiceResponse); // Other types filtered out by dispatcher
00422 
00423     return UAVCAN_NULLPTR != call_registry_.find(CallStateMatchingPredicate(ServiceCallID(frame.getSrcNodeID(),
00424                                                                                           frame.getTransferID())));
00425 
00426 }
00427 
00428 template <typename DataType_, typename Callback_>
00429 void ServiceClient<DataType_, Callback_>::handleReceivedDataStruct(ReceivedDataStructure<ResponseType>& response)
00430 {
00431     UAVCAN_ASSERT(response.getTransferType() == TransferTypeServiceResponse);
00432 
00433     ServiceCallID call_id(response.getSrcNodeID(), response.getTransferID());
00434     cancelCall(call_id);
00435     ServiceCallResultType result(ServiceCallResultType::Success, call_id, response);    // Mutable!
00436     invokeCallback(result);
00437 }
00438 
00439 
00440 template <typename DataType_, typename Callback_>
00441 void ServiceClient<DataType_, Callback_>::handleDeadline(MonotonicTime)
00442 {
00443     UAVCAN_TRACE("ServiceClient", "Shared deadline event received");
00444     /*
00445      * Invoking callbacks for timed out call state objects.
00446      */
00447     TimeoutCallbackCaller callback_caller(*this);
00448     call_registry_.template forEach<TimeoutCallbackCaller&>(callback_caller);
00449     /*
00450      * Removing timed out objects.
00451      * This operation cannot be merged with the previous one because that will not work with recursive calls.
00452      */
00453     call_registry_.removeAllWhere(&CallState::hasTimedOutPredicate);
00454     /*
00455      * Subscriber does not need to be registered if we don't have any pending calls.
00456      * Removing it makes processing of incoming frames a bit faster.
00457      */
00458     if (call_registry_.isEmpty())
00459     {
00460         SubscriberType::stop();
00461     }
00462 }
00463 
00464 template <typename DataType_, typename Callback_>
00465 int ServiceClient<DataType_, Callback_>::addCallState(ServiceCallID call_id)
00466 {
00467     if (call_registry_.isEmpty())
00468     {
00469         const int subscriber_res = SubscriberType::startAsServiceResponseListener();
00470         if (subscriber_res < 0)
00471         {
00472             UAVCAN_TRACE("ServiceClient", "Failed to start the subscriber, error: %i", subscriber_res);
00473             return subscriber_res;
00474         }
00475     }
00476 
00477     if (UAVCAN_NULLPTR == call_registry_.template emplace<INode&, ServiceClientBase&,
00478                                                           ServiceCallID>(SubscriberType::getNode(), *this, call_id))
00479     {
00480         SubscriberType::stop();
00481         return -ErrMemory;
00482     }
00483 
00484     return 0;
00485 }
00486 
00487 template <typename DataType_, typename Callback_>
00488 int ServiceClient<DataType_, Callback_>::call(NodeID server_node_id, const RequestType& request)
00489 {
00490    ServiceCallID dummy;
00491    return call(server_node_id, request, dummy);
00492 }
00493 
00494 template <typename DataType_, typename Callback_>
00495 int ServiceClient<DataType_, Callback_>::call(NodeID server_node_id, const RequestType& request,
00496                                               ServiceCallID& out_call_id)
00497 {
00498     if (!coerceOrFallback<bool>(callback_, true))
00499     {
00500         UAVCAN_TRACE("ServiceClient", "Invalid callback");
00501         return -ErrInvalidConfiguration;
00502     }
00503 
00504     /*
00505      * Common procedures that don't depend on the struct data type
00506      */
00507     const int prep_res =
00508         prepareToCall(SubscriberType::getNode(), DataType::getDataTypeFullName(), server_node_id, out_call_id);
00509     if (prep_res < 0)
00510     {
00511         UAVCAN_TRACE("ServiceClient", "Failed to prepare the call, error: %i", prep_res);
00512         return prep_res;
00513     }
00514 
00515     /*
00516      * Initializing the call state - this will start the subscriber ad-hoc
00517      */
00518     const int call_state_res = addCallState(out_call_id);
00519     if (call_state_res < 0)
00520     {
00521         UAVCAN_TRACE("ServiceClient", "Failed to add call state, error: %i", call_state_res);
00522         return call_state_res;
00523     }
00524 
00525     /*
00526      * Configuring the listener so it will accept only the matching responses
00527      * TODO move to init(), but this requires to somewhat refactor GenericSubscriber<> (remove TransferForwarder)
00528      */
00529     TransferListenerWithFilter* const tl = SubscriberType::getTransferListener();
00530     if (tl == UAVCAN_NULLPTR)
00531     {
00532         UAVCAN_ASSERT(0);  // Must have been created
00533         cancelCall(out_call_id);
00534         return -ErrLogic;
00535     }
00536     tl->installAcceptanceFilter(this);
00537 
00538     /*
00539      * Publishing the request
00540      */
00541     const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id,
00542                                                  out_call_id.transfer_id);
00543     if (publisher_res < 0)
00544     {
00545         cancelCall(out_call_id);
00546         return publisher_res;
00547     }
00548 
00549     UAVCAN_ASSERT(server_node_id == out_call_id.server_node_id);
00550     return publisher_res;
00551 }
00552 
00553 template <typename DataType_, typename Callback_>
00554 void ServiceClient<DataType_, Callback_>::cancelCall(ServiceCallID call_id)
00555 {
00556     call_registry_.removeFirstWhere(CallStateMatchingPredicate(call_id));
00557     if (call_registry_.isEmpty())
00558     {
00559         SubscriberType::stop();
00560     }
00561 }
00562 
00563 template <typename DataType_, typename Callback_>
00564 void ServiceClient<DataType_, Callback_>::cancelAllCalls()
00565 {
00566     call_registry_.clear();
00567     SubscriberType::stop();
00568 }
00569 
00570 template <typename DataType_, typename Callback_>
00571 bool ServiceClient<DataType_, Callback_>::hasPendingCallToServer(NodeID server_node_id) const
00572 {
00573     return UAVCAN_NULLPTR != call_registry_.find(ServerSearchPredicate(server_node_id));
00574 }
00575 
00576 template <typename DataType_, typename Callback_>
00577 ServiceCallID ServiceClient<DataType_, Callback_>::getCallIDByIndex(unsigned index) const
00578 {
00579     const CallState* const id = call_registry_.getByIndex(index);
00580     return (id == UAVCAN_NULLPTR) ? ServiceCallID() : id->getCallID();
00581 }
00582 
00583 }
00584 
00585 #endif // UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED