diff --git a/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp b/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp new file mode 100644 index 0000000000..6f07c3c8a8 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp @@ -0,0 +1,49 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class DataTypeInfoProvider : Noncopyable +{ + typedef MethodBinder + ComputeAggregateTypeSignatureCallback; + + typedef MethodBinder GetDataTypeInfoCallback; + + ServiceServer cats_srv_; + ServiceServer gdti_srv_; + + INode& getNode() { return cats_srv_.getNode(); } + + static bool isValidDataTypeKind(DataTypeKind kind); + + void handleComputeAggregateTypeSignatureRequest(const protocol::ComputeAggregateTypeSignature::Request& request, + protocol::ComputeAggregateTypeSignature::Response& response); + + void handleGetDataTypeInfoRequest(const protocol::GetDataTypeInfo::Request& request, + protocol::GetDataTypeInfo::Response& response); + +public: + DataTypeInfoProvider(INode& node) + : cats_srv_(node) + , gdti_srv_(node) + { } + + int start(); +}; + +} diff --git a/libuavcan/src/protocol/data_type_info_provider.cpp b/libuavcan/src/protocol/data_type_info_provider.cpp new file mode 100644 index 0000000000..5e6d8ec4a5 --- /dev/null +++ b/libuavcan/src/protocol/data_type_info_provider.cpp @@ -0,0 +1,108 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +namespace uavcan +{ + +bool DataTypeInfoProvider::isValidDataTypeKind(DataTypeKind kind) +{ + return (kind == DataTypeKindMessage) || (kind == DataTypeKindService); +} + +void DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest( + const protocol::ComputeAggregateTypeSignature::Request& request, + protocol::ComputeAggregateTypeSignature::Response& response) +{ + const DataTypeKind kind = DataTypeKind(request.kind.value); + if (!isValidDataTypeKind(kind)) + { + UAVCAN_TRACE("DataTypeInfoProvider", + "ComputeAggregateTypeSignature request with invalid DataTypeKind %i", kind); + return; + } + + UAVCAN_TRACE("DataTypeInfoProvider", "ComputeAggregateTypeSignature request for dtk=%i", int(request.kind.value)); + + response.mutually_known_ids = request.known_ids; + response.aggregate_signature = + GlobalDataTypeRegistry::instance().computeAggregateSignature(kind, response.mutually_known_ids).get(); +} + +void DataTypeInfoProvider::handleGetDataTypeInfoRequest(const protocol::GetDataTypeInfo::Request& request, + protocol::GetDataTypeInfo::Response& response) +{ + const DataTypeKind kind = DataTypeKind(request.kind.value); + if (!isValidDataTypeKind(kind)) + { + UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request with invalid DataTypeKind %i", kind); + return; + } + + const DataTypeDescriptor* const desc = GlobalDataTypeRegistry::instance().find(kind, request.id); + if (!desc) + { + UAVCAN_TRACE("DataTypeInfoProvider", "Cannot process GetDataTypeInfo for nonexistent type dtid=%i dtk=%i", + int(request.id), int(request.kind.value)); + return; + } + + UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request for %s", desc->toString().c_str()); + + response.signature = desc->getSignature().get(); + response.name = desc->getFullName(); + response.mask = protocol::GetDataTypeInfo::Response::MASK_KNOWN; + + const Dispatcher& dispatcher = getNode().getDispatcher(); + + if (request.kind.value == protocol::DataTypeKind::SERVICE) + { + if (dispatcher.hasServer(request.id)) + { + response.mask |= protocol::GetDataTypeInfo::Response::MASK_SERVING; + } + } + else if (request.kind.value == protocol::DataTypeKind::MESSAGE) + { + if (dispatcher.hasSubscriber(request.id)) + { + response.mask |= protocol::GetDataTypeInfo::Response::MASK_SUBSCRIBED; + } + if (dispatcher.hasPublisher(request.id)) + { + response.mask |= protocol::GetDataTypeInfo::Response::MASK_PUBLISHING; + } + } + else + { + assert(0); // That means that GDTR somehow found a type of an unknown kind. The horror. + } +} + +int DataTypeInfoProvider::start() +{ + int res = -1; + + res = cats_srv_.start( + ComputeAggregateTypeSignatureCallback(this, &DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest)); + if (res < 0) + goto fail; + + res = gdti_srv_.start(GetDataTypeInfoCallback(this, &DataTypeInfoProvider::handleGetDataTypeInfoRequest)); + if (res < 0) + goto fail; + + assert(res >= 0); + return res; + + fail: + assert(res < 0); + cats_srv_.stop(); + gdti_srv_.stop(); + return res; +} + +} diff --git a/libuavcan/test/protocol/data_type_info_provider.cpp b/libuavcan/test/protocol/data_type_info_provider.cpp new file mode 100644 index 0000000000..84b0a74401 --- /dev/null +++ b/libuavcan/test/protocol/data_type_info_provider.cpp @@ -0,0 +1,137 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include "helpers.hpp" + +using uavcan::protocol::GetDataTypeInfo; +using uavcan::protocol::ComputeAggregateTypeSignature; +using uavcan::protocol::NodeStatus; +using uavcan::protocol::DataTypeKind; +using uavcan::ServiceCallResult; +using uavcan::DataTypeInfoProvider; +using uavcan::GlobalDataTypeRegistry; +using uavcan::DefaultDataTypeRegistrator; +using uavcan::MonotonicDuration; + +template +static bool validateDataTypeInfoResponse(const std::auto_ptr >& resp, + unsigned int mask) +{ + if (!resp.get()) + return false; + if (!resp->isSuccessful()) + return false; + if (resp->response.name != DataType::getDataTypeFullName()) + return false; + if (DataType::getDataTypeSignature().get() != resp->response.signature) + return false; + if (resp->response.mask != mask) + return false; + return true; +} + + +TEST(DataTypeInfoProvider, Basic) +{ + InterlinkedTestNodes nodes; + + DataTypeInfoProvider dtip(nodes.a); + + GlobalDataTypeRegistry::instance().reset(); + DefaultDataTypeRegistrator _reg1; + DefaultDataTypeRegistrator _reg2; + DefaultDataTypeRegistrator _reg3; + + ASSERT_LE(0, dtip.start()); + + ServiceClientWithCollector gdti_cln(nodes.b); + ServiceClientWithCollector cats_cln(nodes.b); + + /* + * GetDataTypeInfo request for GetDataTypeInfo + */ + GetDataTypeInfo::Request gdti_request; + gdti_request.id = GetDataTypeInfo::DefaultDataTypeID; + gdti_request.kind.value = DataTypeKind::SERVICE; + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(2)); + + ASSERT_TRUE(validateDataTypeInfoResponse(gdti_cln.collector.result, + GetDataTypeInfo::Response::MASK_KNOWN | + GetDataTypeInfo::Response::MASK_SERVING)); + ASSERT_EQ(1, gdti_cln.collector.result->server_node_id.get()); + + /* + * GetDataTypeInfo request for NodeStatus - not used yet + */ + gdti_request.id = NodeStatus::DefaultDataTypeID; + gdti_request.kind.value = DataTypeKind::MESSAGE; + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(2)); + + ASSERT_TRUE(validateDataTypeInfoResponse(gdti_cln.collector.result, + GetDataTypeInfo::Response::MASK_KNOWN)); + + /* + * Starting publisher and subscriber for NodeStatus, requesting info again + */ + uavcan::Publisher ns_pub(nodes.a); + SubscriberWithCollector ns_sub(nodes.a); + + ASSERT_LE(0, ns_pub.broadcast(NodeStatus())); + ASSERT_LE(0, ns_sub.start()); + + // Request again + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(2)); + + ASSERT_TRUE(validateDataTypeInfoResponse(gdti_cln.collector.result, + GetDataTypeInfo::Response::MASK_KNOWN | + GetDataTypeInfo::Response::MASK_PUBLISHING | + GetDataTypeInfo::Response::MASK_SUBSCRIBED)); + + /* + * Requesting a non-existent type + */ + gdti_request.id = ComputeAggregateTypeSignature::DefaultDataTypeID; + gdti_request.kind.value = 0xFF; // INVALID VALUE + ASSERT_LE(0, gdti_cln.call(1, gdti_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(2)); + + ASSERT_TRUE(gdti_cln.collector.result->isSuccessful()); + ASSERT_EQ(1, gdti_cln.collector.result->server_node_id.get()); + ASSERT_TRUE(gdti_cln.collector.result->response == GetDataTypeInfo::Response()); // Empty response + + /* + * ComputeAggregateTypeSignature test + */ + ComputeAggregateTypeSignature::Request cats_request; + cats_request.kind.value = DataTypeKind::MESSAGE; + cats_request.known_ids.set(); // Assuming we have all 1023 types + ASSERT_LE(0, cats_cln.call(1, cats_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(2)); + + ASSERT_TRUE(cats_cln.collector.result->isSuccessful()); + ASSERT_EQ(1, cats_cln.collector.result->server_node_id.get()); + ASSERT_EQ(NodeStatus::getDataTypeSignature().get(), cats_cln.collector.result->response.aggregate_signature); + ASSERT_TRUE(cats_cln.collector.result->response.mutually_known_ids[NodeStatus::DefaultDataTypeID]); + cats_cln.collector.result->response.mutually_known_ids[NodeStatus::DefaultDataTypeID] = false; + ASSERT_FALSE(cats_cln.collector.result->response.mutually_known_ids.any()); + + /* + * ComputeAggregateTypeSignature test for a non-existent type + */ + cats_request.kind.value = 0xFF; // INVALID + cats_request.known_ids.set(); // Assuming we have all 1023 types + ASSERT_LE(0, cats_cln.call(1, cats_request)); + nodes.spinBoth(MonotonicDuration::fromMSec(2)); + + ASSERT_TRUE(cats_cln.collector.result->isSuccessful()); + ASSERT_EQ(0, cats_cln.collector.result->response.aggregate_signature); + ASSERT_FALSE(cats_cln.collector.result->response.mutually_known_ids.any()); +}