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

NodeDiscoverer: fixes and test

Browse Source
This commit is contained in:
Pavel Kirienko
2015-05-11 17:50:36 +03:00
parent c62b871f20
commit 558171bf71
3 changed files with 226 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp
+6 -6
View File
@@ -9,6 +9,7 @@
#include <uavcan/util/map.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/util/bitset.hpp>
#include <uavcan/node/timer.hpp>
#include <uavcan/node/subscriber.hpp>
#include <uavcan/node/service_client.hpp>
#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
@@ -89,7 +90,7 @@ class NodeDiscoverer : TimerBase
* When this number of attempts has been made, the discoverer will give up and assume that the node
* does not implement this service.
*/
enum { MaxAttemptsToGetNodeInfo = 3 };
enum { MaxAttemptsToGetNodeInfo = 5 };
/*
* States
@@ -217,6 +218,7 @@ class NodeDiscoverer : TimerBase
if (data->num_get_node_info_attempts >= MaxAttemptsToGetNodeInfo)
{
finalizeNodeDiscovery(NULL, result.server_node_id);
// Warning: data pointer is invalidated now
}
}
}
@@ -230,15 +232,13 @@ class NodeDiscoverer : TimerBase
if (!handler_.canDiscoverNewNodes())
{
trace(TraceDiscoveryTimerStop, 0);
stop();
return;
return; // Timer must continue to run in order to not stuck when it unlocks
}
const NodeID node_id = pickNextNodeToQuery();
if (!node_id.isUnicast())
{
trace(TraceDiscoveryTimerStop, 1);
trace(TraceDiscoveryTimerStop, 0);
stop();
return;
}
@@ -282,7 +282,7 @@ class NodeDiscoverer : TimerBase
}
data->last_seen_uptime = msg.uptime_sec;
if (!isRunning() && handler_.canDiscoverNewNodes())
if (!isRunning())
{
trace(TraceDiscoveryTimerStart, get_node_info_client_.getRequestTimeout().toUSec());
startPeriodic(get_node_info_client_.getRequestTimeout());
libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp
+35
View File
@@ -0,0 +1,35 @@
/*
* Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
*/
#pragma once
#include <iostream>
#include <uavcan/protocol/dynamic_node_id_server/types.hpp>
#include <uavcan/util/method_binder.hpp>
#include <uavcan/node/service_server.hpp>
#include <uavcan/protocol/GetNodeInfo.hpp>
class GetNodeInfoMockServer
{
typedef uavcan::MethodBinder<GetNodeInfoMockServer*,
void (GetNodeInfoMockServer::*)(const uavcan::ReceivedDataStructure<uavcan::protocol::GetNodeInfo::Request>&,
uavcan::protocol::GetNodeInfo::Response&) const>
GetNodeInfoCallback;
uavcan::ServiceServer<uavcan::protocol::GetNodeInfo, GetNodeInfoCallback> server_;
void handleRequest(const uavcan::ReceivedDataStructure<uavcan::protocol::GetNodeInfo::Request>& req,
uavcan::protocol::GetNodeInfo::Response& res) const
{
res = response;
std::cout << "GET NODE INFO:\nREQUEST\n" << req << "RESPONSE\n" << res << std::endl;
}
public:
uavcan::protocol::GetNodeInfo::Response response;
GetNodeInfoMockServer(uavcan::INode& node) : server_(node) { }
int start() { return server_.start(GetNodeInfoCallback(this, &GetNodeInfoMockServer::handleRequest)); }
};
libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp
+185
View File
@@ -0,0 +1,185 @@
/*
* 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());
}