node_info_retriever.hpp

00001 /*
00002  * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #ifndef UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED
00006 #define UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED
00007 
00008 #include <uavcan/build_config.hpp>
00009 #include <uavcan/debug.hpp>
00010 #include <uavcan/util/multiset.hpp>
00011 #include <uavcan/node/service_client.hpp>
00012 #include <uavcan/node/timer.hpp>
00013 #include <uavcan/protocol/node_status_monitor.hpp>
00014 #include <uavcan/protocol/GetNodeInfo.hpp>
00015 
00016 namespace uavcan
00017 {
00018 /**
00019  * Classes that need to receive GetNodeInfo responses should implement this interface.
00020  */
00021 class UAVCAN_EXPORT INodeInfoListener
00022 {
00023 public:
00024     /**
00025      * Called when a response to GetNodeInfo request is received. This happens shortly after the node restarts or
00026      * becomes online for the first time.
00027      * @param node_id   Node ID of the node
00028      * @param response  Node info struct
00029      */
00030     virtual void handleNodeInfoRetrieved(NodeID node_id, const protocol::GetNodeInfo::Response& node_info) = 0;
00031 
00032     /**
00033      * Called when the retriever decides that the node does not support the GetNodeInfo service.
00034      * This method will never be called if the number of attempts is unlimited.
00035      */
00036     virtual void handleNodeInfoUnavailable(NodeID node_id) = 0;
00037 
00038     /**
00039      * This call is routed directly from @ref NodeStatusMonitor.
00040      * Default implementation does nothing.
00041      * @param event     Node status change event
00042      */
00043     virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event)
00044     {
00045         (void)event;
00046     }
00047 
00048     /**
00049      * This call is routed directly from @ref NodeStatusMonitor.
00050      * Default implementation does nothing.
00051      * @param msg       Node status message
00052      */
00053     virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
00054     {
00055         (void)msg;
00056     }
00057 
00058     virtual ~INodeInfoListener() { }
00059 };
00060 
00061 /**
00062  * This class automatically retrieves a response to GetNodeInfo once a node appears online or restarts.
00063  * It does a number of attempts in case if there's a communication failure before assuming that the node does not
00064  * implement the GetNodeInfo service. All parameters are pre-configured with sensible default values that should fit
00065  * virtually any use case, but they can be overriden if needed - refer to the setter methods below for details.
00066  *
00067  * Defaults are pre-configured so that the class is able to query 123 nodes (node ID 1..125, where 1 is our local
00068  * node and 1 is one node that implements GetNodeInfo service, hence 123) of which none implements GetNodeInfo
00069  * service in under 5 seconds. The 5 second limitation is imposed by UAVCAN-compatible bootloaders, which are
00070  * unlikely to wait for more than that before continuing to boot. In case if this default value is not appropriate
00071  * for the end application, the request interval can be overriden via @ref setRequestInterval().
00072  *
00073  * Following the above explained requirements, the default request interval is defined as follows:
00074  *      request interval [ms] = floor(5000 [ms] bootloader timeout / 123 nodes)
00075  * Which yields 40 ms.
00076  *
00077  * Given default service timeout 1000 ms and the defined above request frequency 40 ms, the maximum number of
00078  * concurrent requests will be:
00079  *      max concurrent requests = ceil(1000 [ms] timeout / 40 [ms] request interval)
00080  * Which yields 25 requests.
00081  *
00082  * Keep the above equations in mind when changing the default request interval.
00083  *
00084  * Obviously, if all calls are completing in under (request interval), the number of concurrent requests will never
00085  * exceed one. This is actually the most likely scenario.
00086  *
00087  * Note that all nodes are queried in a round-robin fashion, regardless of their uptime, number of requests made, etc.
00088  *
00089  * Events from this class can be routed to many listeners, @ref INodeInfoListener.
00090  */
00091 class UAVCAN_EXPORT NodeInfoRetriever : public NodeStatusMonitor
00092                                       , TimerBase
00093 {
00094 public:
00095     enum { MaxNumRequestAttempts = 254 };
00096     enum { UnlimitedRequestAttempts = 0 };
00097 
00098 private:
00099     typedef MethodBinder<NodeInfoRetriever*,
00100                          void (NodeInfoRetriever::*)(const ServiceCallResult<protocol::GetNodeInfo>&)>
00101             GetNodeInfoResponseCallback;
00102 
00103     struct Entry
00104     {
00105         uint32_t uptime_sec;
00106         uint8_t num_attempts_made;
00107         bool request_needed;                    ///< Always false for unknown nodes
00108         bool updated_since_last_attempt;        ///< Always false for unknown nodes
00109 
00110         Entry()
00111             : uptime_sec(0)
00112             , num_attempts_made(0)
00113             , request_needed(false)
00114             , updated_since_last_attempt(false)
00115         {
00116 #if UAVCAN_DEBUG
00117             StaticAssert<sizeof(Entry) <= 8>::check();
00118 #endif
00119         }
00120     };
00121 
00122     struct NodeInfoRetrievedHandlerCaller
00123     {
00124         const NodeID node_id;
00125         const protocol::GetNodeInfo::Response& node_info;
00126 
00127         NodeInfoRetrievedHandlerCaller(NodeID arg_node_id, const protocol::GetNodeInfo::Response& arg_node_info)
00128             : node_id(arg_node_id)
00129             , node_info(arg_node_info)
00130         { }
00131 
00132         bool operator()(INodeInfoListener* key)
00133         {
00134             UAVCAN_ASSERT(key != UAVCAN_NULLPTR);
00135             key->handleNodeInfoRetrieved(node_id, node_info);
00136             return false;
00137         }
00138     };
00139 
00140     template <typename Event>
00141     struct GenericHandlerCaller
00142     {
00143         void (INodeInfoListener::* const method)(Event);
00144         Event event;
00145 
00146         GenericHandlerCaller(void (INodeInfoListener::*arg_method)(Event), Event arg_event)
00147             : method(arg_method)
00148             , event(arg_event)
00149         { }
00150 
00151         bool operator()(INodeInfoListener* key)
00152         {
00153             UAVCAN_ASSERT(key != UAVCAN_NULLPTR);
00154             (key->*method)(event);
00155             return false;
00156         }
00157     };
00158 
00159     enum { DefaultNumRequestAttempts = 16 };
00160     enum { DefaultTimerIntervalMSec = 40 };  ///< Read explanation in the class documentation
00161 
00162     /*
00163      * State
00164      */
00165     Entry entries_[NodeID::Max];  // [1, NodeID::Max]
00166 
00167     Multiset<INodeInfoListener*>  listeners_;
00168 
00169     ServiceClient<protocol::GetNodeInfo, GetNodeInfoResponseCallback> get_node_info_client_;
00170 
00171     MonotonicDuration request_interval_;
00172 
00173     mutable uint8_t last_picked_node_;
00174 
00175     uint8_t num_attempts_;
00176 
00177     /*
00178      * Methods
00179      */
00180     const Entry& getEntry(NodeID node_id) const { return const_cast<NodeInfoRetriever*>(this)->getEntry(node_id); }
00181     Entry&       getEntry(NodeID node_id)
00182     {
00183         if (node_id.get() < 1 || node_id.get() > NodeID::Max)
00184         {
00185             handleFatalError("NodeInfoRetriever NodeID");
00186         }
00187         return entries_[node_id.get() - 1];
00188     }
00189 
00190     void startTimerIfNotRunning()
00191     {
00192         if (!TimerBase::isRunning())
00193         {
00194             TimerBase::startPeriodic(request_interval_);
00195             UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %s sec", request_interval_.toString().c_str());
00196         }
00197     }
00198 
00199     NodeID pickNextNodeToQuery(bool& out_at_least_one_request_needed) const
00200     {
00201         out_at_least_one_request_needed = false;
00202 
00203         for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin
00204         {
00205             last_picked_node_++;
00206             if (last_picked_node_ > NodeID::Max)
00207             {
00208                 last_picked_node_ = 1;
00209             }
00210             UAVCAN_ASSERT((last_picked_node_ >= 1) &&
00211                           (last_picked_node_ <= NodeID::Max));
00212 
00213             const Entry& entry = getEntry(last_picked_node_);
00214 
00215             if (entry.request_needed)
00216             {
00217                 out_at_least_one_request_needed = true;
00218 
00219                 if (entry.updated_since_last_attempt &&
00220                     !get_node_info_client_.hasPendingCallToServer(last_picked_node_))
00221                 {
00222                     UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_));
00223                     return NodeID(last_picked_node_);
00224                 }
00225             }
00226         }
00227 
00228         return NodeID();        // No node could be found
00229     }
00230 
00231     virtual void handleTimerEvent(const TimerEvent&)
00232     {
00233         bool at_least_one_request_needed = false;
00234         const NodeID next = pickNextNodeToQuery(at_least_one_request_needed);
00235 
00236         if (next.isUnicast())
00237         {
00238             UAVCAN_ASSERT(at_least_one_request_needed);
00239             getEntry(next).updated_since_last_attempt = false;
00240             const int res = get_node_info_client_.call(next, protocol::GetNodeInfo::Request());
00241             if (res < 0)
00242             {
00243                 get_node_info_client_.getNode().registerInternalFailure("NodeInfoRetriever GetNodeInfo call");
00244             }
00245         }
00246         else
00247         {
00248             if (!at_least_one_request_needed)
00249             {
00250                 TimerBase::stop();
00251                 UAVCAN_TRACE("NodeInfoRetriever", "Timer stopped");
00252             }
00253         }
00254     }
00255 
00256     virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event)
00257     {
00258         const bool was_offline = !event.was_known ||
00259                                  (event.old_status.mode == protocol::NodeStatus::MODE_OFFLINE);
00260 
00261         const bool offline_now = event.status.mode == protocol::NodeStatus::MODE_OFFLINE;
00262 
00263         if (was_offline || offline_now)
00264         {
00265             Entry& entry = getEntry(event.node_id);
00266 
00267             entry.request_needed = !offline_now;
00268             entry.num_attempts_made = 0;
00269 
00270             UAVCAN_TRACE("NodeInfoRetriever", "Offline status change: node ID %d, request needed: %d",
00271                          int(event.node_id.get()), int(entry.request_needed));
00272 
00273             if (entry.request_needed)
00274             {
00275                 startTimerIfNotRunning();
00276             }
00277         }
00278 
00279         listeners_.forEach(
00280             GenericHandlerCaller<const NodeStatusChangeEvent&>(&INodeInfoListener::handleNodeStatusChange, event));
00281     }
00282 
00283     virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
00284     {
00285         Entry& entry = getEntry(msg.getSrcNodeID());
00286 
00287         if (msg.uptime_sec < entry.uptime_sec)
00288         {
00289             entry.request_needed = true;
00290             entry.num_attempts_made = 0;
00291 
00292             startTimerIfNotRunning();
00293         }
00294         entry.uptime_sec = msg.uptime_sec;
00295         entry.updated_since_last_attempt = true;
00296 
00297         listeners_.forEach(GenericHandlerCaller<const ReceivedDataStructure<protocol::NodeStatus>&>(
00298             &INodeInfoListener::handleNodeStatusMessage, msg));
00299     }
00300 
00301     void handleGetNodeInfoResponse(const ServiceCallResult<protocol::GetNodeInfo>& result)
00302     {
00303         Entry& entry = getEntry(result.getCallID().server_node_id);
00304 
00305         if (result.isSuccessful())
00306         {
00307             /*
00308              * Updating the uptime here allows to properly handle a corner case where the service response arrives
00309              * after the device has restarted and published its new NodeStatus (although it's unlikely to happen).
00310              */
00311             entry.uptime_sec = result.getResponse().status.uptime_sec;
00312             entry.request_needed = false;
00313             listeners_.forEach(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id,
00314                                                               result.getResponse()));
00315         }
00316         else
00317         {
00318             if (num_attempts_ != UnlimitedRequestAttempts)
00319             {
00320                 entry.num_attempts_made++;
00321                 if (entry.num_attempts_made >= num_attempts_)
00322                 {
00323                     entry.request_needed = false;
00324                     listeners_.forEach(GenericHandlerCaller<NodeID>(&INodeInfoListener::handleNodeInfoUnavailable,
00325                                                                     result.getCallID().server_node_id));
00326                 }
00327             }
00328         }
00329     }
00330 
00331 public:
00332     NodeInfoRetriever(INode& node)
00333         : NodeStatusMonitor(node)
00334         , TimerBase(node)
00335         , listeners_(node.getAllocator())
00336         , get_node_info_client_(node)
00337         , request_interval_(MonotonicDuration::fromMSec(DefaultTimerIntervalMSec))
00338         , last_picked_node_(1)
00339         , num_attempts_(DefaultNumRequestAttempts)
00340     { }
00341 
00342     /**
00343      * Starts the retriever.
00344      * Destroy the object to stop it.
00345      * Returns negative error code.
00346      */
00347     int start(const TransferPriority priority = TransferPriority::OneHigherThanLowest)
00348     {
00349         int res = NodeStatusMonitor::start();
00350         if (res < 0)
00351         {
00352             return res;
00353         }
00354 
00355         res = get_node_info_client_.init(priority);
00356         if (res < 0)
00357         {
00358             return res;
00359         }
00360         get_node_info_client_.setCallback(GetNodeInfoResponseCallback(this,
00361                                                                       &NodeInfoRetriever::handleGetNodeInfoResponse));
00362         // Note: the timer will be started ad-hoc
00363         return 0;
00364     }
00365 
00366     /**
00367      * This method forces the class to re-request uavcan.protocol.GetNodeInfo from all nodes as if they
00368      * have just appeared in the network.
00369      */
00370     void invalidateAll()
00371     {
00372         NodeStatusMonitor::forgetAllNodes();
00373         get_node_info_client_.cancelAllCalls();
00374 
00375         for (unsigned i = 0; i < (sizeof(entries_) / sizeof(entries_[0])); i++)
00376         {
00377             entries_[i] = Entry();
00378         }
00379         // It is not necessary to reset the last picked node index
00380     }
00381 
00382     /**
00383      * Adds one listener. Does nothing if such listener already exists.
00384      * May return -ErrMemory if there's no space to add the listener.
00385      */
00386     int addListener(INodeInfoListener* listener)
00387     {
00388         if (listener != UAVCAN_NULLPTR)
00389         {
00390             removeListener(listener);
00391             return (UAVCAN_NULLPTR == listeners_.emplace(listener)) ? -ErrMemory : 0;
00392         }
00393         else
00394         {
00395             return -ErrInvalidParam;
00396         }
00397     }
00398 
00399     /**
00400      * Removes the listener.
00401      * If the listener was not registered, nothing will be done.
00402      */
00403     void removeListener(INodeInfoListener* listener)
00404     {
00405         if (listener != UAVCAN_NULLPTR)
00406         {
00407             listeners_.removeAll(listener);
00408         }
00409         else
00410         {
00411             UAVCAN_ASSERT(0);
00412         }
00413     }
00414 
00415     unsigned getNumListeners() const { return listeners_.getSize(); }
00416 
00417     /**
00418      * Number of attempts to retrieve GetNodeInfo response before giving up on the assumption that the service is
00419      * not implemented.
00420      * Zero is a special value that can be used to set unlimited number of attempts, @ref UnlimitedRequestAttempts.
00421      */
00422     uint8_t getNumRequestAttempts() const { return num_attempts_; }
00423     void setNumRequestAttempts(const uint8_t num)
00424     {
00425         num_attempts_ = min(static_cast<uint8_t>(MaxNumRequestAttempts), num);
00426     }
00427 
00428     /**
00429      * Request interval also implicitly defines the maximum number of concurrent requests.
00430      * Read the class documentation for details.
00431      */
00432     MonotonicDuration getRequestInterval() const { return request_interval_; }
00433     void setRequestInterval(const MonotonicDuration interval)
00434     {
00435         if (interval.isPositive())
00436         {
00437             request_interval_ = interval;
00438             if (TimerBase::isRunning())
00439             {
00440                 TimerBase::startPeriodic(request_interval_);
00441             }
00442         }
00443         else
00444         {
00445             UAVCAN_ASSERT(0);
00446         }
00447     }
00448 
00449     /**
00450      * These methods are needed mostly for testing.
00451      */
00452     bool isRetrievingInProgress() const { return TimerBase::isRunning(); }
00453 
00454     uint8_t getNumPendingRequests() const
00455     {
00456         const unsigned num = get_node_info_client_.getNumPendingCalls();
00457         UAVCAN_ASSERT(num <= 0xFF);
00458         return static_cast<uint8_t>(num);
00459     }
00460 };
00461 
00462 }
00463 
00464 #endif // Include guard