node_discoverer.hpp
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
3  */
4 
5 #ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED
6 #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED
7 
9 #include <uavcan/util/map.hpp>
11 #include <uavcan/util/bitset.hpp>
12 #include <uavcan/node/timer.hpp>
17 #include <cassert>
18 // UAVCAN types
19 #include <uavcan/protocol/NodeStatus.hpp>
20 #include <uavcan/protocol/GetNodeInfo.hpp>
21 
22 namespace uavcan
23 {
24 namespace dynamic_node_id_server
25 {
30 {
31 public:
35  virtual bool canDiscoverNewNodes() const = 0;
36 
41  {
45  };
46 
51  virtual NodeAwareness checkNodeAwareness(NodeID node_id) const = 0;
52 
59  virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) = 0;
60 
61  virtual ~INodeDiscoveryHandler() { }
62 };
63 
69 {
72 
75 
76  struct NodeData
77  {
80 
82  : last_seen_uptime(0)
83  , num_get_node_info_attempts(0)
84  { }
85  };
86 
88 
93  enum { MaxAttemptsToGetNodeInfo = 5 };
94 
95  enum { TimerPollIntervalMs = 170 }; // ~ ceil(500 ms service timeout / 3)
96 
97  /*
98  * States
99  */
102 
104  NodeMap node_map_;
105 
108 
109  /*
110  * Methods
111  */
112  void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); }
113 
114  INode& getNode() { return node_status_sub_.getNode(); }
115 
116  void removeNode(const NodeID node_id)
117  {
118  node_map_.remove(node_id);
119  trace(TraceDiscoveryNodeRemoved, node_id.get());
120  }
121 
123  {
124  // This essentially means that we pick first available node. Remember that the map is unordered.
125  const NodeMap::KVPair* const pair = node_map_.getByIndex(0);
126  return (pair == UAVCAN_NULLPTR) ? NodeID() : pair->key;
127  }
128 
129  bool needToQuery(NodeID node_id)
130  {
131  UAVCAN_ASSERT(node_id.isUnicast());
132 
133  /*
134  * Fast check
135  */
136  if (committed_node_mask_[node_id.get()])
137  {
138  return false;
139  }
140 
141  /*
142  * Slow check - may involve full log search
143  */
144  const INodeDiscoveryHandler::NodeAwareness awareness = handler_.checkNodeAwareness(node_id);
145 
147  {
148  return true;
149  }
151  {
152  removeNode(node_id);
153  return false;
154  }
156  {
157  trace(TraceDiscoveryCommitCacheUpdated, node_id.get());
158  committed_node_mask_[node_id.get()] = true;
159  removeNode(node_id);
160  return false;
161  }
162  else
163  {
164  UAVCAN_ASSERT(0);
165  return false;
166  }
167  }
168 
170  {
171  NodeID node_id;
172  do
173  {
174  node_id = pickNextNodeToQuery();
175  if (node_id.isUnicast())
176  {
177  if (needToQuery(node_id))
178  {
179  return node_id;
180  }
181  else
182  {
183  removeNode(node_id);
184  }
185  }
186  }
187  while (node_id.isUnicast());
188  return NodeID();
189  }
190 
191  void finalizeNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id)
192  {
193  trace(TraceDiscoveryNodeFinalized, node_id.get() | ((unique_id_or_null == UAVCAN_NULLPTR) ? 0U : 0x100U));
194  removeNode(node_id);
195  /*
196  * It is paramount to check if the server is still interested to receive this data.
197  * Otherwise, if the node appeared in the log while we were waiting for response, we'd end up with
198  * duplicate node ID in the log.
199  */
200  if (needToQuery(node_id))
201  {
202  handler_.handleNewNodeDiscovery(unique_id_or_null, node_id);
203  }
204  }
205 
207  {
208  if (result.isSuccessful())
209  {
210  UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "GetNodeInfo response from %d",
211  int(result.getCallID().server_node_id.get()));
212  finalizeNodeDiscovery(&result.getResponse().hardware_version.unique_id, result.getCallID().server_node_id);
213  }
214  else
215  {
216  trace(TraceDiscoveryGetNodeInfoFailure, result.getCallID().server_node_id.get());
217 
218  NodeData* const data = node_map_.access(result.getCallID().server_node_id);
219  if (data == UAVCAN_NULLPTR)
220  {
221  return; // Probably it is a known node now
222  }
223 
224  UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer",
225  "GetNodeInfo request to %d has timed out, %d attempts",
226  int(result.getCallID().server_node_id.get()), int(data->num_get_node_info_attempts));
227  data->num_get_node_info_attempts++;
228  if (data->num_get_node_info_attempts >= MaxAttemptsToGetNodeInfo)
229  {
230  finalizeNodeDiscovery(UAVCAN_NULLPTR, result.getCallID().server_node_id);
231  // Warning: data pointer is invalidated now
232  }
233  }
234  }
235 
237  {
238  if (get_node_info_client_.hasPendingCalls())
239  {
240  return;
241  }
242 
243  const NodeID node_id = pickNextNodeToQueryAndCleanupMap();
244  if (!node_id.isUnicast())
245  {
246  trace(TraceDiscoveryTimerStop, 0);
247  stop();
248  return;
249  }
250 
251  if (!handler_.canDiscoverNewNodes())
252  {
253  return; // Timer must continue to run in order to not stuck when it unlocks
254  }
255 
256  trace(TraceDiscoveryGetNodeInfoRequest, node_id.get());
257 
258  UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d",
259  int(node_id.get()));
260  const int res = get_node_info_client_.call(node_id, protocol::GetNodeInfo::Request());
261  if (res < 0)
262  {
263  getNode().registerInternalFailure("NodeDiscoverer GetNodeInfo call");
264  }
265  }
266 
268  {
269  if (!needToQuery(msg.getSrcNodeID()))
270  {
271  return;
272  }
273 
274  NodeData* data = node_map_.access(msg.getSrcNodeID());
275  if (data == UAVCAN_NULLPTR)
276  {
277  trace(TraceDiscoveryNewNodeFound, msg.getSrcNodeID().get());
278 
279  data = node_map_.insert(msg.getSrcNodeID(), NodeData());
280  if (data == UAVCAN_NULLPTR)
281  {
282  getNode().registerInternalFailure("NodeDiscoverer OOM");
283  return;
284  }
285  }
286  UAVCAN_ASSERT(data != UAVCAN_NULLPTR);
287 
288  if (msg.uptime_sec < data->last_seen_uptime)
289  {
291  data->num_get_node_info_attempts = 0;
292  }
293  data->last_seen_uptime = msg.uptime_sec;
294 
295  if (!isRunning())
296  {
297  startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs));
298  trace(TraceDiscoveryTimerStart, getPeriod().toUSec());
299  }
300  }
301 
302 public:
304  : TimerBase(node)
305  , handler_(handler)
306  , tracer_(tracer)
307  , node_map_(node.getAllocator())
308  , get_node_info_client_(node)
309  , node_status_sub_(node)
310  { }
311 
312  int init(const TransferPriority priority)
313  {
314  int res = get_node_info_client_.init(priority);
315  if (res < 0)
316  {
317  return res;
318  }
319  get_node_info_client_.setCallback(
321 
322  res = node_status_sub_.start(NodeStatusCallback(this, &NodeDiscoverer::handleNodeStatus));
323  if (res < 0)
324  {
325  return res;
326  }
327 
328  // Note: the timer starts ad-hoc from the node status callback, not here.
329 
330  return 0;
331  }
332 
336  bool hasUnknownNodes() const { return !node_map_.isEmpty(); }
337 
342  uint8_t getNumUnknownNodes() const { return static_cast<uint8_t>(node_map_.getSize()); }
343 };
344 
345 }
346 }
347 
348 #endif // Include guard
TraceDiscoveryNewNodeFound
Definition: event.hpp:23
void remove(const Key &key)
Definition: map.hpp:256
std::uint8_t uint8_t
Definition: std.hpp:24
TraceDiscoveryNodeRemoved
Definition: event.hpp:23
bool isEmpty() const
Definition: map.hpp:170
ServiceClient< protocol::GetNodeInfo, GetNodeInfoResponseCallback > get_node_info_client_
ServiceCallID getCallID() const
bool isUnicast() const
Definition: transfer.hpp:136
Value * insert(const Key &key, const Value &value)
Definition: map.hpp:233
virtual void handleNewNodeDiscovery(const UniqueID *unique_id_or_null, NodeID node_id)=0
TraceDiscoveryNodeRestartDetected
Definition: event.hpp:23
TraceDiscoveryNodeFinalized
Definition: event.hpp:23
virtual NodeAwareness checkNodeAwareness(NodeID node_id) const =0
virtual void onEvent(TraceCode event_code, int64_t event_argument)=0
bool hasPendingCalls() const
void handleGetNodeInfoResponse(const ServiceCallResult< protocol::GetNodeInfo > &result)
TraceDiscoveryCommitCacheUpdated
Definition: event.hpp:23
uint8_t get() const
Definition: transfer.hpp:132
unsigned getSize() const
Definition: map.hpp:367
protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id UniqueID
void trace(TraceCode code, int64_t argument)
NodeDiscoverer(INode &node, IEventTracer &tracer, INodeDiscoveryHandler &handler)
std::uint32_t uint32_t
Definition: std.hpp:26
TraceDiscoveryTimerStart
Definition: event.hpp:23
void finalizeNodeDiscovery(const UniqueID *unique_id_or_null, NodeID node_id)
int start(const Callback &callback)
Definition: subscriber.hpp:83
TraceDiscoveryGetNodeInfoRequest
Definition: event.hpp:23
const ResponseFieldType & getResponse() const
BitSet< NodeID::Max+1 > committed_node_mask_
Nodes that are marked will not be queried.
void handleNodeStatus(const ReceivedDataStructure< protocol::NodeStatus > &msg)
void setCallback(const Callback &cb)
std::int64_t int64_t
Definition: std.hpp:32
int init(const TransferPriority priority)
TraceDiscoveryTimerStop
Definition: event.hpp:23
Value * access(const Key &key)
Definition: map.hpp:225
static MonotonicDuration fromMSec(int64_t ms)
Definition: time.hpp:41
Subscriber< protocol::NodeStatus, NodeStatusCallback > node_status_sub_
int call(NodeID server_node_id, const RequestType &request)
TraceDiscoveryGetNodeInfoFailure
Definition: event.hpp:23
KVPair * getByIndex(unsigned index)
Definition: map.hpp:333


uavcan_communicator
Author(s):
autogenerated on Wed Jan 11 2023 03:59:39