From 6d584734bb1b193ba906e7b221d27fa8afba8ba9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 14 Mar 2014 00:53:53 +0400 Subject: [PATCH] ServiceClient<> --- .../internal/node/generic_subscriber.hpp | 2 + .../internal/transport/transfer_listener.hpp | 120 +++++++-- libuavcan/include/uavcan/service_client.hpp | 254 ++++++++++++++++++ .../root_ns_a/99.StringService.uavcan | 4 +- libuavcan/test/service_client.cpp | 229 ++++++++++++++++ 5 files changed, 581 insertions(+), 28 deletions(-) create mode 100644 libuavcan/include/uavcan/service_client.hpp create mode 100644 libuavcan/test/service_client.cpp diff --git a/libuavcan/include/uavcan/internal/node/generic_subscriber.hpp b/libuavcan/include/uavcan/internal/node/generic_subscriber.hpp index b667c95c4b..af52fd7215 100644 --- a/libuavcan/include/uavcan/internal/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/internal/node/generic_subscriber.hpp @@ -209,6 +209,8 @@ protected: TransferListenerType* getTransferListener() { return forwarder_; } + ReceivedDataStructure& getReceivedStructStorage() { return message_; } + public: Scheduler& getScheduler() const { return scheduler_; } }; diff --git a/libuavcan/include/uavcan/internal/transport/transfer_listener.hpp b/libuavcan/include/uavcan/internal/transport/transfer_listener.hpp index 974d710fc9..d04c626f72 100644 --- a/libuavcan/include/uavcan/internal/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/internal/transport/transfer_listener.hpp @@ -81,7 +81,7 @@ public: /** * Internal, refer to transport dispatcher. */ -class TransferListenerBase : public LinkedListNode +class TransferListenerBase : public LinkedListNode, Noncopyable { const DataTypeDescriptor& data_type_; const TransferCRC crc_base_; ///< Pre-initialized with data type hash, thus constant @@ -108,37 +108,15 @@ public: }; /** - * This class should be derived by transfer receivers (subscribers, servers, callers). + * This class should be derived by transfer receivers (subscribers, servers). */ template -class TransferListener : public TransferListenerBase, Noncopyable +class TransferListener : public TransferListenerBase { typedef TransferBufferManager BufferManager; BufferManager bufmgr_; Map receivers_; - void handleFrame(const RxFrame& frame) - { - const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType()); - - TransferReceiver* recv = receivers_.access(key); - if (recv == NULL) - { - if (!frame.isFirst()) - return; - - TransferReceiver new_recv; - recv = receivers_.insert(key, new_recv); - if (recv == NULL) - { - UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str()); - return; - } - } - TransferBufferAccessor tba(bufmgr_, key); - handleReception(*recv, frame, tba); - } - class TimedOutReceiverPredicate { const MonotonicTime ts_; @@ -174,6 +152,29 @@ class TransferListener : public TransferListenerBase, Noncopyable assert(receivers_.isEmpty() ? bufmgr_.isEmpty() : 1); } +protected: + void handleFrame(const RxFrame& frame) + { + const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType()); + + TransferReceiver* recv = receivers_.access(key); + if (recv == NULL) + { + if (!frame.isFirst()) + return; + + TransferReceiver new_recv; + recv = receivers_.insert(key, new_recv); + if (recv == NULL) + { + UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str()); + return; + } + } + TransferBufferAccessor tba(bufmgr_, key); + handleReception(*recv, frame, tba); + } + public: TransferListener(const DataTypeDescriptor& data_type, IAllocator& allocator) : TransferListenerBase(data_type) @@ -183,11 +184,78 @@ public: StaticAssert<(NumStaticReceivers >= NumStaticBufs)>::check(); // Otherwise it would be meaningless } - ~TransferListener() + virtual ~TransferListener() { // Map must be cleared before bufmgr is destructed receivers_.removeAll(); } }; +/** + * This class should be derived by callers. + */ +template +class ServiceResponseTransferListener : public TransferListener +{ +public: + struct ExpectedResponseParams + { + NodeID src_node_id; + TransferID transfer_id; + + ExpectedResponseParams() + { + assert(!src_node_id.isValid()); + } + + ExpectedResponseParams(NodeID src_node_id, TransferID transfer_id) + : src_node_id(src_node_id) + , transfer_id(transfer_id) + { + assert(src_node_id.isUnicast()); + } + + bool match(const RxFrame& frame) const + { + assert(frame.getTransferType() == TransferTypeServiceResponse); + return (frame.getSrcNodeID() == src_node_id) && (frame.getTransferID() == transfer_id); + } + }; + +private: + typedef TransferListener BaseType; + + ExpectedResponseParams response_params_; + + void handleFrame(const RxFrame& frame) + { + if (!response_params_.match(frame)) + { + UAVCAN_TRACE("ServiceResponseTransferListener", "Rejected %s [need snid=%i tid=%i]", + frame.toString().c_str(), + int(response_params_.src_node_id.get()), int(response_params_.transfer_id.get())); + return; + } + UAVCAN_TRACE("ServiceResponseTransferListener", "Accepted %s", frame.toString().c_str()); + BaseType::handleFrame(frame); + } + +public: + ServiceResponseTransferListener(const DataTypeDescriptor& data_type, IAllocator& allocator) + : BaseType(data_type, allocator) + { } + + void setExpectedResponseParams(const ExpectedResponseParams& erp) + { + response_params_ = erp; + } + + const ExpectedResponseParams& getExpectedResponseParams() const { return response_params_; } + + void stopAcceptingAnything() + { + response_params_ = ExpectedResponseParams(); + } +}; + } diff --git a/libuavcan/include/uavcan/service_client.hpp b/libuavcan/include/uavcan/service_client.hpp new file mode 100644 index 0000000000..093a120cb2 --- /dev/null +++ b/libuavcan/include/uavcan/service_client.hpp @@ -0,0 +1,254 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +namespace uavcan +{ + +template +class ServiceResponseTransferListenerInstantiationHelper +{ + enum { DataTypeMaxByteLen = BitLenToByteLen::Result }; +public: + typedef ServiceResponseTransferListener Type; +}; + + +template +struct ServiceCallResult +{ + typedef ReceivedDataStructure ResponseFieldType; + + enum Status { Success, ErrorTimeout }; + + const Status status; + NodeID server_node_id; + ResponseFieldType& response; ///< Either response contents or unspecified response structure + + ServiceCallResult(Status status, NodeID server_node_id, ResponseFieldType& response) + : status(status) + , server_node_id(server_node_id) + , response(response) + { + assert(server_node_id.isUnicast()); + assert(status == Success || status == ErrorTimeout); + } + + bool isSuccessful() const { return status == Success; } +}; + + +template &)> +class ServiceClient : + public GenericSubscriber::Type >, + public DeadlineHandler +{ +public: + typedef DataType_ DataType; + typedef typename DataType::Request RequestType; + typedef typename DataType::Response ResponseType; + typedef ServiceCallResult ServiceCallResultType; + +private: + typedef ServiceClient SelfType; + typedef GenericPublisher PublisherType; + typedef typename ServiceResponseTransferListenerInstantiationHelper::Type TransferListenerType; + typedef GenericSubscriber SubscriberType; + + PublisherType publisher_; + IAllocator& allocator_; + Callback callback_; + MonotonicDuration request_timeout_; + bool pending_; + + bool isCallbackValid() const { return try_implicit_cast(callback_, true); } + + void invokeCallback(ServiceCallResultType& result) + { + if (isCallbackValid()) + callback_(result); + else + handleFatalError("Invalid caller callback"); + } + + void handleReceivedDataStruct(ReceivedDataStructure& request) + { + const TransferListenerType* const listener = SubscriberType::getTransferListener(); + if (listener) + { + const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams(); + ServiceCallResultType result(ServiceCallResultType::Success, erp.src_node_id, request); + cancel(); + invokeCallback(result); + } + else + { + assert(0); + cancel(); + } + } + + void handleDeadline(MonotonicTime) + { + const TransferListenerType* const listener = SubscriberType::getTransferListener(); + if (listener) + { + const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams(); + ReceivedDataStructure& ref = SubscriberType::getReceivedStructStorage(); + ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, erp.src_node_id, ref); + + UAVCAN_TRACE("ServiceClient", "Timeout from nid=%i, dtname=%s", + erp.src_node_id.get(), DataType::getDataTypeFullName()); + cancel(); + invokeCallback(result); + } + else + { + assert(0); + cancel(); + } + } + +public: + ServiceClient(Scheduler& scheduler, IAllocator& allocator, IMarshalBufferProvider& buffer_provider, + const Callback& callback = Callback()) + : SubscriberType(scheduler, allocator) + , DeadlineHandler(scheduler) + , publisher_(scheduler, buffer_provider, getDefaultRequestTimeout()) + , allocator_(allocator) + , callback_(callback) + , request_timeout_(getDefaultRequestTimeout()) + , pending_(false) + { + setRequestTimeout(getDefaultRequestTimeout()); +#if UAVCAN_DEBUG + assert(getRequestTimeout() == getDefaultRequestTimeout()); // Making sure default values are OK +#endif + } + + virtual ~ServiceClient() { cancel(); } + + int call(NodeID server_node_id, const RequestType& request) // TODO: Refactor, move into a non-template subclass! + { + /* + * Cancelling the pending request + */ + cancel(); + if (!isCallbackValid()) + { + UAVCAN_TRACE("ServiceClient", "Invalid callback"); + return -1; + } + pending_ = true; + + /* + * Determining the Data Type ID + */ + GlobalDataTypeRegistry::instance().freeze(); + const DataTypeDescriptor* const descr = + GlobalDataTypeRegistry::instance().find(DataTypeKindService, DataType::getDataTypeFullName()); + if (!descr) + { + UAVCAN_TRACE("ServiceClient", "Type [%s] is not registered", DataType::getDataTypeFullName()); + cancel(); + return -1; + } + + /* + * Determining the Transfer ID + */ + const OutgoingTransferRegistryKey otr_key(descr->getID(), TransferTypeServiceRequest, server_node_id); + const MonotonicTime otr_deadline = + SubscriberType::getScheduler().getMonotonicTimestamp() + TransferSender::getDefaultMaxTransferInterval(); + TransferID* const otr_tid = SubscriberType::getScheduler().getDispatcher().getOutgoingTransferRegistry() + .accessOrCreate(otr_key, otr_deadline); + if (!otr_tid) + { + UAVCAN_TRACE("ServiceClient", "OTR access failure, dtd=%s", descr->toString().c_str()); + cancel(); + return -1; + } + const TransferID transfer_id = *otr_tid; + otr_tid->increment(); + + /* + * Starting the subscriber + */ + const int subscriber_res = SubscriberType::startAsServiceResponseListener(); + if (subscriber_res <= 0) + { + UAVCAN_TRACE("ServiceClient", "Failed to start the subscriber, error: %i", subscriber_res); + cancel(); + return subscriber_res; + } + + /* + * Configuring the listener so it will accept only the matching response + */ + TransferListenerType* const tl = SubscriberType::getTransferListener(); + if (!tl) + { + assert(0); // Must have been created + cancel(); + return -1; + } + const typename TransferListenerType::ExpectedResponseParams erp(server_node_id, transfer_id); + tl->setExpectedResponseParams(erp); + + /* + * Registering the deadline handler + */ + DeadlineHandler::startWithDelay(request_timeout_); + + /* + * Publishing the request + */ + const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id, transfer_id); + if (!publisher_res) + { + cancel(); + } + return publisher_res; + } + + void cancel() + { + pending_ = false; + SubscriberType::stop(); + DeadlineHandler::stop(); + TransferListenerType* const tl = SubscriberType::getTransferListener(); + if (tl) + tl->stopAcceptingAnything(); + } + + bool isPending() const { return pending_; } + + const Callback& getCallback() const { return callback_; } + void setCallback(const Callback& cb) { callback_ = cb; } + + uint32_t getResponseFailureCount() const { return SubscriberType::getFailureCount(); } + + /* + * Request timeouts + * There is no such config as TX timeout - TX timeouts are configured automagically according to request timeouts + */ + static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(1000); } + static MonotonicDuration getMinRequestTimeout() { return MonotonicDuration::fromMSec(10); } + static MonotonicDuration getMaxRequestTimeout() { return MonotonicDuration::fromMSec(60000); } + + MonotonicDuration getRequestTimeout() const { return request_timeout_; } + void setRequestTimeout(MonotonicDuration timeout) + { + timeout = std::max(timeout, getMinRequestTimeout()); + timeout = std::min(timeout, getMaxRequestTimeout()); + + publisher_.setTxTimeout(timeout); + request_timeout_ = std::max(timeout, publisher_.getTxTimeout()); // No less than TX timeout + } +}; + +} diff --git a/libuavcan/test/dsdl_test/root_ns_a/99.StringService.uavcan b/libuavcan/test/dsdl_test/root_ns_a/99.StringService.uavcan index b32921af95..5bca3a34d2 100644 --- a/libuavcan/test/dsdl_test/root_ns_a/99.StringService.uavcan +++ b/libuavcan/test/dsdl_test/root_ns_a/99.StringService.uavcan @@ -1,3 +1,3 @@ -uint8[<=12] string_request +uint8[<=64] string_request --- -uint8[<=12] string_response +uint8[<=64] string_response diff --git a/libuavcan/test/service_client.cpp b/libuavcan/test/service_client.cpp new file mode 100644 index 0000000000..339f76f783 --- /dev/null +++ b/libuavcan/test/service_client.cpp @@ -0,0 +1,229 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include +#include "clock.hpp" +#include "internal/transport/can/can.hpp" + + +template +struct ServiceCallResultHandler +{ + typedef typename uavcan::ServiceCallResult::Status StatusType; + StatusType last_status; + uavcan::NodeID last_server_node_id; + typename DataType::Response last_response; + + void handleResponse(const uavcan::ServiceCallResult& result) + { + // TODO: stream uavcan::ServiceCallResult<> directly + std::cout << "Service call result [" << DataType::getDataTypeFullName() << "], success=" + << result.isSuccessful() << ":\n"; + std::cout << result.response << std::endl; + + last_status = result.status; + last_response = result.response; + last_server_node_id = result.server_node_id; + } + + bool match(StatusType status, uavcan::NodeID server_node_id, const typename DataType::Response& response) const + { + return + status == last_status && + server_node_id == last_server_node_id && + response == last_response; + } + + typedef uavcan::MethodBinder&)> Binder; + + Binder bind() { return Binder(this, &ServiceCallResultHandler::handleResponse); } +}; + + +static void stringServiceServerCallback(const uavcan::ReceivedDataStructure& req, + root_ns_a::StringService::Response& rsp) +{ + rsp.string_response = "Request string: "; + rsp.string_response += req.string_request.c_str(); +} + + +struct MakeshiftNode : uavcan::Noncopyable +{ + uavcan::PoolAllocator pool; + uavcan::PoolManager<1> poolmgr; + SystemClockDriver clock_driver; + uavcan::MarshalBufferProvider<> buffer_provider; + uavcan::OutgoingTransferRegistry<8> otr; + uavcan::Scheduler scheduler; + + MakeshiftNode(uavcan::ICanDriver& can_driver, uavcan::NodeID self_node_id) + : otr(poolmgr) + , scheduler(can_driver, poolmgr, clock_driver, otr, self_node_id) + { + poolmgr.addPool(&pool); + } + + void spin(uavcan::MonotonicDuration duration) + { + scheduler.spin(clock_driver.getMonotonic() + duration); + } +}; + + +struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface +{ + uavcan::ISystemClock& clock; + PairableCanDriver* other; + std::queue read_queue; + + PairableCanDriver(uavcan::ISystemClock& clock) + : clock(clock) + , other(NULL) + { } + + void linkTogether(PairableCanDriver* with) + { + this->other = with; + with->other = this; + } + + uavcan::ICanIface* getIface(int iface_index) + { + if (iface_index == 0) + return this; + return NULL; + } + + int getNumIfaces() const { return 1; } + + int select(int& inout_write_iface_mask, int& inout_read_iface_mask, uavcan::MonotonicTime blocking_deadline) + { + assert(other); + if (inout_read_iface_mask == 1) + inout_read_iface_mask = read_queue.size() ? 1 : 0; + + if (inout_read_iface_mask || inout_write_iface_mask) + return 1; + + while (clock.getMonotonic() < blocking_deadline) + usleep(1000); + + return 0; + } + + int send(const uavcan::CanFrame& frame, uavcan::MonotonicTime) + { + assert(other); + other->read_queue.push(frame); + return 1; + } + + int receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic, uavcan::UtcTime& out_ts_utc) + { + assert(other); + assert(read_queue.size()); + out_frame = read_queue.front(); + read_queue.pop(); + out_ts_monotonic = clock.getMonotonic(); + out_ts_utc = clock.getUtc(); + return 1; + } + + int configureFilters(const uavcan::CanFilterConfig*, int) { return -1; } + int getNumFilters() const { return 0; } + uint64_t getNumErrors() const { return 0; } +}; + + +TEST(ServiceClient, Basic) +{ + SystemClockDriver clock; + + PairableCanDriver can_a(clock), can_b(clock); + can_a.linkTogether(&can_b); + + MakeshiftNode node_a(can_a, 1), node_b(can_b, 2); + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + // Server + uavcan::Server server(node_a.scheduler, node_a.poolmgr, node_a.buffer_provider); + ASSERT_EQ(1, server.start(stringServiceServerCallback)); + + { + // Caller + typedef uavcan::ServiceCallResult ResultType; + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler handler; + + ClientType client1(node_b.scheduler, node_b.poolmgr, node_b.buffer_provider); + ClientType client2(node_b.scheduler, node_b.poolmgr, node_b.buffer_provider); + ClientType client3(node_b.scheduler, node_b.poolmgr, node_b.buffer_provider); + + client1.setCallback(handler.bind()); + client2.setCallback(client1.getCallback()); + client3.setCallback(client1.getCallback()); + client3.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100)); + + ASSERT_EQ(1, node_a.scheduler.getDispatcher().getNumServiceRequestListeners()); + ASSERT_EQ(0, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); // NOT listening! + + root_ns_a::StringService::Request request; + request.string_request = "Hello world"; + + ASSERT_LT(0, client1.call(1, request)); // OK + ASSERT_LT(0, client2.call(1, request)); // OK + ASSERT_LT(0, client3.call(99, request)); // Will timeout! + + ASSERT_EQ(3, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); // Listening now! + + ASSERT_TRUE(client1.isPending()); + ASSERT_TRUE(client2.isPending()); + ASSERT_TRUE(client3.isPending()); + + node_a.spin(uavcan::MonotonicDuration::fromMSec(10)); + node_b.spin(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_EQ(1, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); // Third is still listening! + + ASSERT_FALSE(client1.isPending()); + ASSERT_FALSE(client2.isPending()); + ASSERT_TRUE(client3.isPending()); + + // Validating + root_ns_a::StringService::Response expected_response; + expected_response.string_response = "Request string: Hello world"; + ASSERT_TRUE(handler.match(ResultType::Success, 1, expected_response)); + + node_a.spin(uavcan::MonotonicDuration::fromMSec(100)); + node_b.spin(uavcan::MonotonicDuration::fromMSec(100)); + + ASSERT_FALSE(client1.isPending()); + ASSERT_FALSE(client2.isPending()); + ASSERT_FALSE(client3.isPending()); + + ASSERT_EQ(0, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); // Third has timed out :( + + // Validating + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response())); + + // Stray request + ASSERT_LT(0, client3.call(99, request)); // Will timeout! + ASSERT_TRUE(client3.isPending()); + ASSERT_EQ(1, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); + } + + // All destroyed - nobody listening + ASSERT_EQ(0, node_b.scheduler.getDispatcher().getNumServiceResponseListeners()); +}