mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-14 10:07:39 +08:00
272 lines
9.2 KiB
C++
272 lines
9.2 KiB
C++
/*
|
|
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
|
|
*/
|
|
|
|
#if __GNUC__
|
|
// We need auto_ptr for compatibility reasons
|
|
# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
|
|
#endif
|
|
|
|
#include <gtest/gtest.h>
|
|
#include <vector>
|
|
#include <uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp>
|
|
#include <uavcan/node/publisher.hpp>
|
|
#include "event_tracer.hpp"
|
|
#include "get_node_info_mock_server.hpp"
|
|
#include "../helpers.hpp"
|
|
|
|
using namespace uavcan::dynamic_node_id_server;
|
|
|
|
|
|
class NodeDiscoveryHandler : public uavcan::dynamic_node_id_server::INodeDiscoveryHandler
|
|
{
|
|
public:
|
|
struct NodeInfo
|
|
{
|
|
UniqueID unique_id;
|
|
uavcan::NodeID node_id;
|
|
bool committed;
|
|
|
|
NodeInfo() : committed(false) { }
|
|
};
|
|
|
|
bool can_discover;
|
|
std::vector<NodeInfo> nodes;
|
|
|
|
NodeDiscoveryHandler() : can_discover(false) { }
|
|
|
|
virtual bool canDiscoverNewNodes() const
|
|
{
|
|
return can_discover;
|
|
}
|
|
|
|
virtual NodeAwareness checkNodeAwareness(uavcan::NodeID node_id) const
|
|
{
|
|
const NodeInfo* const ni = const_cast<NodeDiscoveryHandler*>(this)->findNode(node_id);
|
|
if (ni == NULL)
|
|
{
|
|
return NodeAwarenessUnknown;
|
|
}
|
|
return ni->committed ? NodeAwarenessKnownAndCommitted : NodeAwarenessKnownButNotCommitted;
|
|
}
|
|
|
|
virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, uavcan::NodeID node_id)
|
|
{
|
|
NodeInfo info;
|
|
if (unique_id_or_null != NULL)
|
|
{
|
|
info.unique_id = *unique_id_or_null;
|
|
}
|
|
info.node_id = node_id;
|
|
nodes.push_back(info);
|
|
}
|
|
|
|
NodeInfo* findNode(const UniqueID& unique_id)
|
|
{
|
|
for (unsigned i = 0; i < nodes.size(); i++)
|
|
{
|
|
if (nodes.at(i).unique_id == unique_id)
|
|
{
|
|
return &nodes.at(i);
|
|
}
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
NodeInfo* findNode(const uavcan::NodeID& node_id)
|
|
{
|
|
for (unsigned i = 0; i < nodes.size(); i++)
|
|
{
|
|
if (nodes.at(i).node_id == node_id)
|
|
{
|
|
return &nodes.at(i);
|
|
}
|
|
}
|
|
return NULL;
|
|
}
|
|
};
|
|
|
|
|
|
TEST(dynamic_node_id_server_NodeDiscoverer, Basic)
|
|
{
|
|
using namespace uavcan::protocol::dynamic_node_id::server;
|
|
|
|
uavcan::GlobalDataTypeRegistry::instance().reset();
|
|
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
|
|
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
|
|
|
|
EventTracer tracer;
|
|
InterlinkedTestNodesWithSysClock nodes;
|
|
NodeDiscoveryHandler handler;
|
|
|
|
NodeDiscoverer disc(nodes.a, tracer, handler);
|
|
|
|
/*
|
|
* Initialization
|
|
*/
|
|
ASSERT_LE(0, disc.init());
|
|
|
|
ASSERT_FALSE(disc.hasUnknownNodes());
|
|
|
|
/*
|
|
* Publishing NodeStatus, discovery is disabled
|
|
*/
|
|
handler.can_discover = false;
|
|
|
|
uavcan::Publisher<uavcan::protocol::NodeStatus> node_status_pub(nodes.b);
|
|
ASSERT_LE(0, node_status_pub.init());
|
|
|
|
uavcan::protocol::NodeStatus node_status;
|
|
node_status.status_code = node_status.STATUS_OK; // Status will be ignored anyway
|
|
node_status.uptime_sec = 0;
|
|
ASSERT_LE(0, node_status_pub.broadcast(node_status));
|
|
|
|
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
|
|
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); // The timer runs as long as there are unknown nodes
|
|
ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); // Querying is disabled!
|
|
ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
|
|
ASSERT_TRUE(disc.hasUnknownNodes());
|
|
|
|
/*
|
|
* Enabling discovery - the querying will continue despite the fact that NodeStatus messages are not arriving
|
|
*/
|
|
handler.can_discover = true;
|
|
|
|
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
|
|
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
|
|
ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
|
|
ASSERT_LE(1, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
|
|
ASSERT_LE(1, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
|
|
ASSERT_TRUE(disc.hasUnknownNodes());
|
|
|
|
/*
|
|
* Publishing NodeStatus
|
|
*/
|
|
node_status.uptime_sec += 5U;
|
|
ASSERT_LE(0, node_status_pub.broadcast(node_status));
|
|
|
|
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
|
|
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
|
|
ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
|
|
ASSERT_LE(2, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
|
|
ASSERT_LE(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
|
|
ASSERT_TRUE(disc.hasUnknownNodes());
|
|
|
|
/*
|
|
* Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized
|
|
*/
|
|
GetNodeInfoMockServer get_node_info_server(nodes.b);
|
|
get_node_info_server.response.hardware_version.unique_id[0] = 123; // Arbitrary data
|
|
get_node_info_server.response.hardware_version.unique_id[6] = 213;
|
|
get_node_info_server.response.hardware_version.unique_id[14] = 52;
|
|
ASSERT_LE(0, get_node_info_server.start());
|
|
|
|
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900));
|
|
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop));
|
|
ASSERT_LE(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
|
|
ASSERT_LE(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized));
|
|
ASSERT_FALSE(disc.hasUnknownNodes());
|
|
|
|
/*
|
|
* Checking the results
|
|
*/
|
|
ASSERT_TRUE(handler.findNode(get_node_info_server.response.hardware_version.unique_id));
|
|
ASSERT_EQ(2, handler.findNode(get_node_info_server.response.hardware_version.unique_id)->node_id.get());
|
|
}
|
|
|
|
|
|
TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts)
|
|
{
|
|
using namespace uavcan::protocol::dynamic_node_id::server;
|
|
|
|
uavcan::GlobalDataTypeRegistry::instance().reset();
|
|
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
|
|
uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
|
|
|
|
EventTracer tracer;
|
|
InterlinkedTestNodesWithSysClock nodes;
|
|
NodeDiscoveryHandler handler;
|
|
|
|
NodeDiscoverer disc(nodes.a, tracer, handler);
|
|
|
|
/*
|
|
* Initialization
|
|
*/
|
|
ASSERT_LE(0, disc.init());
|
|
|
|
ASSERT_FALSE(disc.hasUnknownNodes());
|
|
|
|
/*
|
|
* Publishing NodeStatus once to trigger querying
|
|
* Querying for 2 seconds, no responses will be sent (there's no server)
|
|
*/
|
|
handler.can_discover = true;
|
|
|
|
uavcan::Publisher<uavcan::protocol::NodeStatus> node_status_pub(nodes.b);
|
|
ASSERT_LE(0, node_status_pub.init());
|
|
|
|
uavcan::protocol::NodeStatus node_status;
|
|
node_status.status_code = node_status.STATUS_OK; // Status will be ignored anyway
|
|
node_status.uptime_sec = 10; // Nonzero
|
|
ASSERT_LE(0, node_status_pub.broadcast(node_status));
|
|
|
|
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3400));
|
|
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
|
|
ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
|
|
ASSERT_LE(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
|
|
ASSERT_LE(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
|
|
ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized));
|
|
ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
|
|
ASSERT_TRUE(disc.hasUnknownNodes());
|
|
|
|
/*
|
|
* Emulating node restart
|
|
*/
|
|
node_status.status_code = node_status.STATUS_OK; // Status will be ignored anyway
|
|
node_status.uptime_sec = 9; // Less than previous
|
|
ASSERT_LE(0, node_status_pub.broadcast(node_status));
|
|
|
|
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3400));
|
|
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
|
|
ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
|
|
ASSERT_LE(6, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
|
|
ASSERT_LE(6, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
|
|
ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized));
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
|
|
ASSERT_TRUE(disc.hasUnknownNodes());
|
|
|
|
/*
|
|
* Waiting for timeout
|
|
*/
|
|
nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3400));
|
|
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop));
|
|
ASSERT_LE(8, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
|
|
ASSERT_LE(8, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized));
|
|
ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
|
|
ASSERT_FALSE(disc.hasUnknownNodes());
|
|
|
|
/*
|
|
* Checking the results
|
|
*/
|
|
ASSERT_TRUE(handler.findNode(UniqueID()));
|
|
ASSERT_EQ(2, handler.findNode(UniqueID())->node_id.get());
|
|
}
|