node_discoverer.hpp

00001 /*
00002  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED
00006 #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED
00007 
00008 #include <uavcan/build_config.hpp>
00009 #include <uavcan/util/map.hpp>
00010 #include <uavcan/util/method_binder.hpp>
00011 #include <uavcan/util/bitset.hpp>
00012 #include <uavcan/node/timer.hpp>
00013 #include <uavcan/node/subscriber.hpp>
00014 #include <uavcan/node/service_client.hpp>
00015 #include <uavcan/protocol/dynamic_node_id_server/types.hpp>
00016 #include <uavcan/protocol/dynamic_node_id_server/event.hpp>
00017 #include <cassert>
00018 // UAVCAN types
00019 #include <uavcan/protocol/NodeStatus.hpp>
00020 #include <uavcan/protocol/GetNodeInfo.hpp>
00021 
00022 namespace uavcan
00023 {
00024 namespace dynamic_node_id_server
00025 {
00026 /**
00027  * The allocator must implement this interface.
00028  */
00029 class INodeDiscoveryHandler
00030 {
00031 public:
00032     /**
00033      * In order to avoid bus congestion, normally only leader can discover new nodes.
00034      */
00035     virtual bool canDiscoverNewNodes() const = 0;
00036 
00037     /**
00038      * These values represent the level of awareness of a certain node by the server.
00039      */
00040     enum NodeAwareness
00041     {
00042         NodeAwarenessUnknown,
00043         NodeAwarenessKnownButNotCommitted,
00044         NodeAwarenessKnownAndCommitted
00045     };
00046 
00047     /**
00048      * It is OK to do full log traversal in this method, because the unique ID collector will cache the
00049      * result when possible.
00050      */
00051     virtual NodeAwareness checkNodeAwareness(NodeID node_id) const = 0;
00052 
00053     /**
00054      * This method will be called when a new node responds to GetNodeInfo request.
00055      * If this method fails to register the node, the node will be queried again later and this method will be
00056      * invoked again.
00057      * Unique ID will be UAVCAN_NULLPTR if the node is assumed to not implement the GetNodeInfo service.
00058      */
00059     virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) = 0;
00060 
00061     virtual ~INodeDiscoveryHandler() { }
00062 };
00063 
00064 /**
00065  * This class listens to NodeStatus messages from other nodes and retrieves their unique ID if they are not
00066  * known to the allocator.
00067  */
00068 class NodeDiscoverer : TimerBase
00069 {
00070     typedef MethodBinder<NodeDiscoverer*, void (NodeDiscoverer::*)(const ServiceCallResult<protocol::GetNodeInfo>&)>
00071         GetNodeInfoResponseCallback;
00072 
00073     typedef MethodBinder<NodeDiscoverer*, void (NodeDiscoverer::*)(const ReceivedDataStructure<protocol::NodeStatus>&)>
00074         NodeStatusCallback;
00075 
00076     struct NodeData
00077     {
00078         uint32_t last_seen_uptime;
00079         uint8_t num_get_node_info_attempts;
00080 
00081         NodeData()
00082             : last_seen_uptime(0)
00083             , num_get_node_info_attempts(0)
00084         { }
00085     };
00086 
00087     typedef Map<NodeID, NodeData>  NodeMap ;
00088 
00089     /**
00090      * When this number of attempts has been made, the discoverer will give up and assume that the node
00091      * does not implement this service.
00092      */
00093     enum { MaxAttemptsToGetNodeInfo = 5 };
00094 
00095     enum { TimerPollIntervalMs = 170 }; // ~ ceil(500 ms service timeout / 3)
00096 
00097     /*
00098      * States
00099      */
00100     INodeDiscoveryHandler& handler_;
00101     IEventTracer& tracer_;
00102 
00103     BitSet<NodeID::Max + 1>  committed_node_mask_;       ///< Nodes that are marked will not be queried
00104     NodeMap  node_map_;
00105 
00106     ServiceClient<protocol::GetNodeInfo, GetNodeInfoResponseCallback> get_node_info_client_;
00107     Subscriber<protocol::NodeStatus, NodeStatusCallback> node_status_sub_;
00108 
00109     /*
00110      * Methods
00111      */
00112     void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); }
00113 
00114     INode& getNode() { return node_status_sub_.getNode(); }
00115 
00116     void removeNode(const NodeID node_id)
00117     {
00118         node_map_.remove(node_id);
00119         trace(TraceDiscoveryNodeRemoved, node_id.get());
00120     }
00121 
00122     NodeID pickNextNodeToQuery() const
00123     {
00124         // This essentially means that we pick first available node. Remember that the map is unordered.
00125         const NodeMap::KVPair* const pair = node_map_.getByIndex(0);
00126         return (pair == UAVCAN_NULLPTR) ? NodeID() : pair->key;
00127     }
00128 
00129     bool needToQuery(NodeID node_id)
00130     {
00131         UAVCAN_ASSERT(node_id.isUnicast());
00132 
00133         /*
00134          * Fast check
00135          */
00136         if (committed_node_mask_[node_id.get()])
00137         {
00138             return false;
00139         }
00140 
00141         /*
00142          * Slow check - may involve full log search
00143          */
00144         const INodeDiscoveryHandler::NodeAwareness awareness = handler_.checkNodeAwareness(node_id);
00145 
00146         if (awareness == INodeDiscoveryHandler::NodeAwarenessUnknown)
00147         {
00148             return true;
00149         }
00150         else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownButNotCommitted)
00151         {
00152             removeNode(node_id);
00153             return false;
00154         }
00155         else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownAndCommitted)
00156         {
00157             trace(TraceDiscoveryCommitCacheUpdated, node_id.get());
00158             committed_node_mask_[node_id.get()] = true;
00159             removeNode(node_id);
00160             return false;
00161         }
00162         else
00163         {
00164             UAVCAN_ASSERT(0);
00165             return false;
00166         }
00167     }
00168 
00169     NodeID pickNextNodeToQueryAndCleanupMap()
00170     {
00171         NodeID node_id;
00172         do
00173         {
00174             node_id = pickNextNodeToQuery();
00175             if (node_id.isUnicast())
00176             {
00177                 if (needToQuery(node_id))
00178                 {
00179                     return node_id;
00180                 }
00181                 else
00182                 {
00183                     removeNode(node_id);
00184                 }
00185             }
00186         }
00187         while (node_id.isUnicast());
00188         return NodeID();
00189     }
00190 
00191     void finalizeNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id)
00192     {
00193         trace(TraceDiscoveryNodeFinalized, node_id.get() | ((unique_id_or_null == UAVCAN_NULLPTR) ? 0U : 0x100U));
00194         removeNode(node_id);
00195         /*
00196          * It is paramount to check if the server is still interested to receive this data.
00197          * Otherwise, if the node appeared in the log while we were waiting for response, we'd end up with
00198          * duplicate node ID in the log.
00199          */
00200         if (needToQuery(node_id))
00201         {
00202             handler_.handleNewNodeDiscovery(unique_id_or_null, node_id);
00203         }
00204     }
00205 
00206     void handleGetNodeInfoResponse(const ServiceCallResult<protocol::GetNodeInfo>& result)
00207     {
00208         if (result.isSuccessful())
00209         {
00210             UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "GetNodeInfo response from %d",
00211                          int(result.getCallID().server_node_id.get()));
00212             finalizeNodeDiscovery(&result.getResponse().hardware_version.unique_id, result.getCallID().server_node_id);
00213         }
00214         else
00215         {
00216             trace(TraceDiscoveryGetNodeInfoFailure, result.getCallID().server_node_id.get());
00217 
00218             NodeData* const data = node_map_.access(result.getCallID().server_node_id);
00219             if (data == UAVCAN_NULLPTR)
00220             {
00221                 return;         // Probably it is a known node now
00222             }
00223 
00224             UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer",
00225                          "GetNodeInfo request to %d has timed out, %d attempts",
00226                          int(result.getCallID().server_node_id.get()), int(data->num_get_node_info_attempts));
00227             data->num_get_node_info_attempts++;
00228             if (data->num_get_node_info_attempts >= MaxAttemptsToGetNodeInfo)
00229             {
00230                 finalizeNodeDiscovery(UAVCAN_NULLPTR, result.getCallID().server_node_id);
00231                 // Warning: data pointer is invalidated now
00232             }
00233         }
00234     }
00235 
00236     void handleTimerEvent(const TimerEvent&)
00237     {
00238         if (get_node_info_client_.hasPendingCalls())
00239         {
00240             return;
00241         }
00242 
00243         const NodeID node_id = pickNextNodeToQueryAndCleanupMap();
00244         if (!node_id.isUnicast())
00245         {
00246             trace(TraceDiscoveryTimerStop, 0);
00247             stop();
00248             return;
00249         }
00250 
00251         if (!handler_.canDiscoverNewNodes())
00252         {
00253             return;     // Timer must continue to run in order to not stuck when it unlocks
00254         }
00255 
00256         trace(TraceDiscoveryGetNodeInfoRequest, node_id.get());
00257 
00258         UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d",
00259                      int(node_id.get()));
00260         const int res = get_node_info_client_.call(node_id, protocol::GetNodeInfo::Request());
00261         if (res < 0)
00262         {
00263             getNode().registerInternalFailure("NodeDiscoverer GetNodeInfo call");
00264         }
00265     }
00266 
00267     void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
00268     {
00269         if (!needToQuery(msg.getSrcNodeID()))
00270         {
00271             return;
00272         }
00273 
00274         NodeData* data = node_map_.access(msg.getSrcNodeID());
00275         if (data == UAVCAN_NULLPTR)
00276         {
00277             trace(TraceDiscoveryNewNodeFound, msg.getSrcNodeID().get());
00278 
00279             data = node_map_.insert(msg.getSrcNodeID(), NodeData());
00280             if (data == UAVCAN_NULLPTR)
00281             {
00282                 getNode().registerInternalFailure("NodeDiscoverer OOM");
00283                 return;
00284             }
00285         }
00286         UAVCAN_ASSERT(data != UAVCAN_NULLPTR);
00287 
00288         if (msg.uptime_sec < data->last_seen_uptime)
00289         {
00290             trace(TraceDiscoveryNodeRestartDetected, msg.getSrcNodeID().get());
00291             data->num_get_node_info_attempts = 0;
00292         }
00293         data->last_seen_uptime = msg.uptime_sec;
00294 
00295         if (!isRunning())
00296         {
00297             startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs));
00298             trace(TraceDiscoveryTimerStart, getPeriod().toUSec());
00299         }
00300     }
00301 
00302 public:
00303     NodeDiscoverer(INode& node, IEventTracer& tracer, INodeDiscoveryHandler& handler)
00304         : TimerBase(node)
00305         , handler_(handler)
00306         , tracer_(tracer)
00307         , node_map_(node.getAllocator())
00308         , get_node_info_client_(node)
00309         , node_status_sub_(node)
00310     { }
00311 
00312     int init(const TransferPriority priority)
00313     {
00314         int res = get_node_info_client_.init(priority);
00315         if (res < 0)
00316         {
00317             return res;
00318         }
00319         get_node_info_client_.setCallback(
00320             GetNodeInfoResponseCallback(this, &NodeDiscoverer::handleGetNodeInfoResponse));
00321 
00322         res = node_status_sub_.start(NodeStatusCallback(this, &NodeDiscoverer::handleNodeStatus));
00323         if (res < 0)
00324         {
00325             return res;
00326         }
00327 
00328         // Note: the timer starts ad-hoc from the node status callback, not here.
00329 
00330         return 0;
00331     }
00332 
00333     /**
00334      * Returns true if there's at least one node with pending GetNodeInfo.
00335      */
00336     bool hasUnknownNodes() const { return !node_map_.isEmpty(); }
00337 
00338     /**
00339      * Returns number of nodes that are being queried at the moment.
00340      * This method is needed for testing and state visualization.
00341      */
00342     uint8_t getNumUnknownNodes() const { return static_cast<uint8_t>(node_map_.getSize()); }
00343 };
00344 
00345 }
00346 }
00347 
00348 #endif // Include guard