node_discoverer.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
3  */
4 
5 #if __GNUC__
6 # pragma GCC diagnostic ignored "-Wzero-as-null-pointer-constant"
7 #endif
8 
9 #include <gtest/gtest.h>
10 #include <vector>
11 #include <uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp>
12 #include <uavcan/node/publisher.hpp>
13 #include "event_tracer.hpp"
14 #include "get_node_info_mock_server.hpp"
15 #include "../helpers.hpp"
16 
17 using namespace uavcan::dynamic_node_id_server;
18 
19 
20 class NodeDiscoveryHandler : public uavcan::dynamic_node_id_server::INodeDiscoveryHandler
21 {
22 public:
23  struct NodeInfo
24  {
25  UniqueID unique_id;
26  uavcan::NodeID node_id;
27  bool committed;
28 
29  NodeInfo() : committed(false) { }
30  };
31 
32  bool can_discover;
33  std::vector<NodeInfo> nodes;
34 
35  NodeDiscoveryHandler() : can_discover(false) { }
36 
37  virtual bool canDiscoverNewNodes() const
38  {
39  return can_discover;
40  }
41 
42  virtual NodeAwareness checkNodeAwareness(uavcan::NodeID node_id) const
43  {
44  const NodeInfo* const ni = const_cast<NodeDiscoveryHandler*>(this)->findNode(node_id);
45  if (ni == UAVCAN_NULLPTR)
46  {
47  return NodeAwarenessUnknown;
48  }
49  return ni->committed ? NodeAwarenessKnownAndCommitted : NodeAwarenessKnownButNotCommitted;
50  }
51 
52  virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, uavcan::NodeID node_id)
53  {
54  NodeInfo info;
55  if (unique_id_or_null != UAVCAN_NULLPTR)
56  {
57  info.unique_id = *unique_id_or_null;
58  }
59  info.node_id = node_id;
60  nodes.push_back(info);
61  }
62 
63  NodeInfo* findNode(const UniqueID& unique_id)
64  {
65  for (unsigned i = 0; i < nodes.size(); i++)
66  {
67  if (nodes.at(i).unique_id == unique_id)
68  {
69  return &nodes.at(i);
70  }
71  }
72  return UAVCAN_NULLPTR;
73  }
74 
75  NodeInfo* findNode(const uavcan::NodeID& node_id)
76  {
77  for (unsigned i = 0; i < nodes.size(); i++)
78  {
79  if (nodes.at(i).node_id == node_id)
80  {
81  return &nodes.at(i);
82  }
83  }
84  return UAVCAN_NULLPTR;
85  }
86 };
87 
88 
89 TEST(dynamic_node_id_server_NodeDiscoverer, Basic)
90 {
91  using namespace uavcan::protocol::dynamic_node_id::server;
92 
93  uavcan::GlobalDataTypeRegistry::instance().reset();
94  uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
95  uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
96 
97  EventTracer tracer;
98  InterlinkedTestNodesWithSysClock nodes;
99  NodeDiscoveryHandler handler;
100 
101  NodeDiscoverer disc(nodes.a, tracer, handler);
102 
103  /*
104  * Initialization
105  */
106  ASSERT_LE(0, disc.init(uavcan::TransferPriority::OneHigherThanLowest));
107 
108  ASSERT_FALSE(disc.hasUnknownNodes());
109 
110  /*
111  * Publishing NodeStatus, discovery is disabled
112  */
113  std::cout << "!!! Publishing NodeStatus, discovery is disabled" << std::endl;
114  handler.can_discover = false;
115 
116  uavcan::Publisher<uavcan::protocol::NodeStatus> node_status_pub(nodes.b);
117  ASSERT_LE(0, node_status_pub.init());
118 
119  uavcan::protocol::NodeStatus node_status;
120  node_status.uptime_sec = 0;
121  ASSERT_LE(0, node_status_pub.broadcast(node_status));
122 
123  nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
124 
125  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
126  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); // The timer runs as long as there are unknown nodes
127  ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); // Querying is disabled!
128  ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
129  ASSERT_TRUE(disc.hasUnknownNodes());
130 
131  /*
132  * Enabling discovery - the querying will continue despite the fact that NodeStatus messages are not arriving
133  */
134  std::cout << "!!! Enabling discovery" << std::endl;
135  handler.can_discover = true;
136 
137  nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1150));
138 
139  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
140  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
141  ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
142  ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
143  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
144  ASSERT_TRUE(disc.hasUnknownNodes());
145 
146  /*
147  * Publishing NodeStatus
148  */
149  std::cout << "!!! Publishing NodeStatus" << std::endl;
150 
151  node_status.uptime_sec += 5U;
152  ASSERT_LE(0, node_status_pub.broadcast(node_status));
153 
154  nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1250));
155 
156  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
157  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
158  ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
159  ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
160  ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
161  ASSERT_TRUE(disc.hasUnknownNodes());
162 
163  /*
164  * Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized
165  */
166  std::cout << "!!! Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized" << std::endl;
167 
168  GetNodeInfoMockServer get_node_info_server(nodes.b);
169  get_node_info_server.response.hardware_version.unique_id[0] = 123; // Arbitrary data
170  get_node_info_server.response.hardware_version.unique_id[6] = 213;
171  get_node_info_server.response.hardware_version.unique_id[14] = 52;
172  ASSERT_LE(0, get_node_info_server.start());
173 
174  nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
175 
176  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
177  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
178  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop));
179  ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
180  ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
181  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized));
182  ASSERT_FALSE(disc.hasUnknownNodes());
183 
184  /*
185  * Checking the results
186  */
187  ASSERT_TRUE(handler.findNode(get_node_info_server.response.hardware_version.unique_id));
188  ASSERT_EQ(2, handler.findNode(get_node_info_server.response.hardware_version.unique_id)->node_id.get());
189 }
190 
191 
192 TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts)
193 {
194  using namespace uavcan::protocol::dynamic_node_id::server;
195 
196  uavcan::GlobalDataTypeRegistry::instance().reset();
197  uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
198  uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
199 
200  EventTracer tracer;
201  InterlinkedTestNodesWithSysClock nodes;
202  NodeDiscoveryHandler handler;
203 
204  NodeDiscoverer disc(nodes.a, tracer, handler);
205 
206  /*
207  * Initialization
208  */
209  ASSERT_LE(0, disc.init(uavcan::TransferPriority::OneHigherThanLowest));
210 
211  ASSERT_FALSE(disc.hasUnknownNodes());
212 
213  /*
214  * Publishing NodeStatus once to trigger querying
215  * Querying for 2 seconds, no responses will be sent (there's no server)
216  */
217  handler.can_discover = true;
218 
219  uavcan::Publisher<uavcan::protocol::NodeStatus> node_status_pub(nodes.b);
220  ASSERT_LE(0, node_status_pub.init());
221 
222  uavcan::protocol::NodeStatus node_status;
223  node_status.uptime_sec = 10; // Nonzero
224  ASSERT_LE(0, node_status_pub.broadcast(node_status));
225 
226  nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100));
227 
228  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
229  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
230  ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
231  ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
232  ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
233  ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized));
234  ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
235  ASSERT_TRUE(disc.hasUnknownNodes());
236 
237  /*
238  * Emulating node restart
239  */
240  node_status.uptime_sec = 9; // Less than previous
241  ASSERT_LE(0, node_status_pub.broadcast(node_status));
242 
243  nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100));
244 
245  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
246  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
247  ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
248  ASSERT_EQ(7, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
249  ASSERT_EQ(6, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
250  ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized));
251  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
252  ASSERT_TRUE(disc.hasUnknownNodes());
253 
254  /*
255  * Waiting for timeout
256  */
257  nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3100));
258 
259  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
260  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
261  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop));
262  ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
263  ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
264  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized));
265  ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
266  ASSERT_FALSE(disc.hasUnknownNodes());
267 
268  /*
269  * Checking the results
270  */
271  ASSERT_TRUE(handler.findNode(UniqueID()));
272  ASSERT_EQ(2, handler.findNode(UniqueID())->node_id.get());
273 }
274 
275 
276 TEST(dynamic_node_id_server_NodeDiscoverer, Sizes)
277 {
278  using namespace uavcan;
279 
280  std::cout << "BitSet<NodeID::Max + 1>: " << sizeof(BitSet<NodeID::Max + 1>) << std::endl;
281  std::cout << "ServiceClient<protocol::GetNodeInfo>: " << sizeof(ServiceClient<protocol::GetNodeInfo>) << std::endl;
282  std::cout << "protocol::GetNodeInfo::Response: " << sizeof(protocol::GetNodeInfo::Response) << std::endl;
283  std::cout << "Subscriber<protocol::NodeStatus>: " << sizeof(Subscriber<protocol::NodeStatus>) << std::endl;
284 }
uavcan::dynamic_node_id_server::NodeDiscoverer::init
int init(const TransferPriority priority)
Definition: node_discoverer.hpp:312
UAVCAN_NULLPTR
#define UAVCAN_NULLPTR
Definition: libuavcan/libuavcan/include/uavcan/build_config.hpp:51
uavcan::DefaultDataTypeRegistrator
Definition: global_data_type_registry.hpp:186
uavcan::Subscriber
Definition: subscriber.hpp:45
uavcan::Publisher
Definition: publisher.hpp:18
NodeDiscoveryHandler
Definition: node_discoverer.cpp:20
GetNodeInfoMockServer
Definition: get_node_info_mock_server.hpp:13
uavcan::NodeID::get
uint8_t get() const
Definition: transfer.hpp:132
uavcan::NodeID
Definition: transfer.hpp:112
uavcan::DurationBase< MonotonicDuration >::fromMSec
static MonotonicDuration fromMSec(int64_t ms)
Definition: time.hpp:41
NodeDiscoveryHandler::canDiscoverNewNodes
virtual bool canDiscoverNewNodes() const
Definition: node_discoverer.cpp:37
uavcan::dynamic_node_id_server::INodeDiscoveryHandler::NodeAwareness
NodeAwareness
Definition: node_discoverer.hpp:40
uavcan::dynamic_node_id_server::NodeDiscoverer
Definition: node_discoverer.hpp:68
publisher.hpp
NodeDiscoveryHandler::NodeInfo::NodeInfo
NodeInfo()
Definition: node_discoverer.cpp:29
TraceDiscoveryNodeRestartDetected
TraceDiscoveryNodeRestartDetected
Definition: event.hpp:79
get_node_info_mock_server.hpp
NodeDiscoveryHandler::NodeInfo
Definition: node_discoverer.cpp:23
NodeDiscoveryHandler::NodeInfo::unique_id
UniqueID unique_id
Definition: node_discoverer.cpp:25
uavcan::TransferPriority::OneHigherThanLowest
static const TransferPriority OneHigherThanLowest
Definition: transfer.hpp:40
TraceDiscoveryGetNodeInfoRequest
TraceDiscoveryGetNodeInfoRequest
Definition: event.hpp:78
uavcan::dynamic_node_id_server::NodeDiscoverer::hasUnknownNodes
bool hasUnknownNodes() const
Definition: node_discoverer.hpp:336
NodeDiscoveryHandler::NodeInfo::committed
bool committed
Definition: node_discoverer.cpp:27
NodeDiscoveryHandler::NodeDiscoveryHandler
NodeDiscoveryHandler()
Definition: node_discoverer.cpp:35
node_discoverer.hpp
InterlinkedTestNodes
Definition: test_node.hpp:149
TraceDiscoveryTimerStop
TraceDiscoveryTimerStop
Definition: event.hpp:77
uavcan::ServiceClient
Definition: service_client.hpp:217
NodeDiscoveryHandler::findNode
NodeInfo * findNode(const uavcan::NodeID &node_id)
Definition: node_discoverer.cpp:75
TraceDiscoveryNewNodeFound
TraceDiscoveryNewNodeFound
Definition: event.hpp:71
uavcan::BitSet< NodeID::Max+1 >
TraceDiscoveryTimerStart
TraceDiscoveryTimerStart
Definition: event.hpp:75
NodeDiscoveryHandler::findNode
NodeInfo * findNode(const UniqueID &unique_id)
Definition: node_discoverer.cpp:63
uavcan::GenericPublisher< DataType_, DataType_ >::init
int init()
Definition: generic_publisher.hpp:118
NodeDiscoveryHandler::checkNodeAwareness
virtual NodeAwareness checkNodeAwareness(uavcan::NodeID node_id) const
Definition: node_discoverer.cpp:42
NodeDiscoveryHandler::can_discover
bool can_discover
Definition: node_discoverer.cpp:32
InterlinkedTestNodes::a
TestNode a
Definition: test_node.hpp:155
TEST
TEST(dynamic_node_id_server_NodeDiscoverer, Basic)
Definition: node_discoverer.cpp:89
TraceDiscoveryGetNodeInfoFailure
TraceDiscoveryGetNodeInfoFailure
Definition: event.hpp:74
InterlinkedTestNodes::spinBoth
int spinBoth(uavcan::MonotonicDuration duration)
Definition: test_node.hpp:176
TraceDiscoveryNodeFinalized
TraceDiscoveryNodeFinalized
Definition: event.hpp:73
uavcan::dynamic_node_id_server
Definition: abstract_server.hpp:16
NodeDiscoveryHandler::NodeInfo::node_id
uavcan::NodeID node_id
Definition: node_discoverer.cpp:26
uavcan::Publisher::broadcast
int broadcast(const DataType &message)
Definition: publisher.hpp:52
event_tracer.hpp
NodeDiscoveryHandler::handleNewNodeDiscovery
virtual void handleNewNodeDiscovery(const UniqueID *unique_id_or_null, uavcan::NodeID node_id)
Definition: node_discoverer.cpp:52
uavcan::GlobalDataTypeRegistry::instance
static GlobalDataTypeRegistry & instance()
Definition: uc_global_data_type_registry.cpp:128
GetNodeInfoMockServer::start
int start()
Definition: get_node_info_mock_server.hpp:34
uavcan
Definition: libuavcan/libuavcan/include/uavcan/build_config.hpp:204
GetNodeInfoMockServer::response
uavcan::protocol::GetNodeInfo::Response response
Definition: get_node_info_mock_server.hpp:30
InterlinkedTestNodes::b
TestNode b
Definition: test_node.hpp:156
pyuavcan_v0.introspect.info
info
Definition: introspect.py:333
NodeDiscoveryHandler::nodes
std::vector< NodeInfo > nodes
Definition: node_discoverer.cpp:33
uavcan::dynamic_node_id_server::UniqueID
protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id UniqueID
Definition: protocol/dynamic_node_id_server/types.hpp:22
uavcan::dynamic_node_id_server::INodeDiscoveryHandler
Definition: node_discoverer.hpp:29

uavcan_communicator
Author(s):
autogenerated on Fri Dec 13 2024 03:10:02