fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-14 05:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

DataTypeInfoProvider

Browse Source
This commit is contained in:
Pavel Kirienko
2014-03-18 20:51:22 +04:00
parent 8879230798
commit 888057f3ec
3 changed files with 294 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/include/uavcan/protocol/data_type_info_provider.hpp
+49
View File
@@ -0,0 +1,49 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include <uavcan/node/service_server.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/impl_constants.hpp>
#include <uavcan/protocol/ComputeAggregateTypeSignature.hpp>
#include <uavcan/protocol/GetDataTypeInfo.hpp>
namespace uavcan
{
class DataTypeInfoProvider : Noncopyable
{
typedef MethodBinder<DataTypeInfoProvider*,
void (DataTypeInfoProvider::*)(const protocol::ComputeAggregateTypeSignature::Request&,
protocol::ComputeAggregateTypeSignature::Response&)>
ComputeAggregateTypeSignatureCallback;
typedef MethodBinder<DataTypeInfoProvider*,
void (DataTypeInfoProvider::*)(const protocol::GetDataTypeInfo::Request&,
protocol::GetDataTypeInfo::Response&)> GetDataTypeInfoCallback;
ServiceServer<protocol::ComputeAggregateTypeSignature, ComputeAggregateTypeSignatureCallback> cats_srv_;
ServiceServer<protocol::GetDataTypeInfo, GetDataTypeInfoCallback> 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();
};
}
libuavcan/src/protocol/data_type_info_provider.cpp
+108
View File
@@ -0,0 +1,108 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <uavcan/debug.hpp>
#include <uavcan/protocol/data_type_info_provider.hpp>
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;
}
}
libuavcan/test/protocol/data_type_info_provider.cpp
+137
View File
@@ -0,0 +1,137 @@
/*
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#include <gtest/gtest.h>
#include <uavcan/node/publisher.hpp>
#include <uavcan/protocol/data_type_info_provider.hpp>
#include <uavcan/protocol/NodeStatus.hpp>
#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 <typename DataType>
static bool validateDataTypeInfoResponse(const std::auto_ptr<ServiceCallResult<GetDataTypeInfo> >& 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<GetDataTypeInfo> _reg1;
DefaultDataTypeRegistrator<ComputeAggregateTypeSignature> _reg2;
DefaultDataTypeRegistrator<NodeStatus> _reg3;
ASSERT_LE(0, dtip.start());
ServiceClientWithCollector<GetDataTypeInfo> gdti_cln(nodes.b);
ServiceClientWithCollector<ComputeAggregateTypeSignature> 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<GetDataTypeInfo>(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<NodeStatus>(gdti_cln.collector.result,
GetDataTypeInfo::Response::MASK_KNOWN));
/*
* Starting publisher and subscriber for NodeStatus, requesting info again
*/
uavcan::Publisher<NodeStatus> ns_pub(nodes.a);
SubscriberWithCollector<NodeStatus> 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<NodeStatus>(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());
}