node_status_monitor.hpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #ifndef UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED
00006 #define UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED
00007 
00008 #include <uavcan/debug.hpp>
00009 #include <uavcan/util/method_binder.hpp>
00010 #include <uavcan/node/subscriber.hpp>
00011 #include <uavcan/node/timer.hpp>
00012 #include <uavcan/protocol/NodeStatus.hpp>
00013 #include <cassert>
00014 #include <cstdlib>
00015 
00016 namespace uavcan
00017 {
00018 /**
00019  * This class implements the core functionality of a network monitor.
00020  * It can be extended by inheritance to add more complex logic, or used directly as is.
00021  */
00022 class UAVCAN_EXPORT NodeStatusMonitor
00023 {
00024 public:
00025     struct NodeStatus
00026     {
00027         uint8_t health   : 2;
00028         uint8_t mode     : 3;
00029         uint8_t sub_mode : 3;
00030 
00031         NodeStatus() :
00032             health(protocol::NodeStatus::HEALTH_CRITICAL),
00033             mode(protocol::NodeStatus::MODE_OFFLINE),
00034             sub_mode(0)
00035         {
00036             StaticAssert<protocol::NodeStatus::FieldTypes::health::BitLen   == 2>::check();
00037             StaticAssert<protocol::NodeStatus::FieldTypes::mode::BitLen     == 3>::check();
00038             StaticAssert<protocol::NodeStatus::FieldTypes::sub_mode::BitLen == 3>::check();
00039         }
00040 
00041         bool operator!=(const NodeStatus rhs) const { return !operator==(rhs); }
00042         bool operator==(const NodeStatus rhs) const
00043         {
00044             return std::memcmp(this, &rhs, sizeof(NodeStatus)) == 0;
00045         }
00046 
00047 #if UAVCAN_TOSTRING
00048         std::string toString() const
00049         {
00050             char buf[40];
00051             (void)snprintf(buf, sizeof(buf), "health=%d mode=%d sub_mode=%d", int(health), int(mode), int(sub_mode));
00052             return std::string(buf);
00053         }
00054 #endif
00055     };
00056 
00057     struct NodeStatusChangeEvent
00058     {
00059         NodeID node_id;
00060         NodeStatus status;
00061         NodeStatus old_status;
00062         bool was_known;
00063 
00064         NodeStatusChangeEvent() :
00065             was_known(false)
00066         { }
00067     };
00068 
00069 private:
00070     enum { TimerPeriodMs100 = 2 };
00071 
00072     typedef MethodBinder<NodeStatusMonitor*,
00073                          void (NodeStatusMonitor::*)(const ReceivedDataStructure<protocol::NodeStatus>&)>
00074             NodeStatusCallback;
00075 
00076     typedef MethodBinder<NodeStatusMonitor*, void (NodeStatusMonitor::*)(const TimerEvent&)> TimerCallback;
00077 
00078     Subscriber<protocol::NodeStatus, NodeStatusCallback> sub_;
00079 
00080     TimerEventForwarder<TimerCallback>  timer_;
00081 
00082     struct Entry
00083     {
00084         NodeStatus status;
00085         int8_t time_since_last_update_ms100;
00086         Entry() :
00087             time_since_last_update_ms100(-1)
00088         { }
00089     };
00090 
00091     mutable Entry entries_[NodeID::Max];  // [1, NodeID::Max]
00092 
00093     Entry& getEntry(NodeID node_id) const
00094     {
00095         if (node_id.get() < 1 || node_id.get() > NodeID::Max)
00096         {
00097             handleFatalError("NodeStatusMonitor NodeID");
00098         }
00099         return entries_[node_id.get() - 1];
00100     }
00101 
00102     void changeNodeStatus(const NodeID node_id, const Entry new_entry_value)
00103     {
00104         Entry& entry = getEntry(node_id);
00105         if (entry.status != new_entry_value.status)
00106         {
00107             NodeStatusChangeEvent event;
00108             event.node_id    = node_id;
00109             event.old_status = entry.status;
00110             event.status     = new_entry_value.status;
00111             event.was_known  = entry.time_since_last_update_ms100 >= 0;
00112 
00113             UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: [%s] --> [%s]", int(node_id.get()),
00114                          (event.was_known ? "known" : "new"),
00115                          event.old_status.toString().c_str(), event.status.toString().c_str());
00116 
00117             handleNodeStatusChange(event);
00118         }
00119         entry = new_entry_value;
00120     }
00121 
00122     void handleNodeStatus(const ReceivedDataStructure<protocol::NodeStatus>& msg)
00123     {
00124         Entry new_entry;
00125         new_entry.time_since_last_update_ms100 = 0;
00126         new_entry.status.health   = msg.health   & ((1 << protocol::NodeStatus::FieldTypes::health::BitLen) - 1);
00127         new_entry.status.mode     = msg.mode     & ((1 << protocol::NodeStatus::FieldTypes::mode::BitLen) - 1);
00128         new_entry.status.sub_mode = msg.sub_mode & ((1 << protocol::NodeStatus::FieldTypes::sub_mode::BitLen) - 1);
00129 
00130         changeNodeStatus(msg.getSrcNodeID(), new_entry);
00131 
00132         handleNodeStatusMessage(msg);
00133     }
00134 
00135     void handleTimerEvent(const TimerEvent&)
00136     {
00137         const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100;
00138 
00139         for (uint8_t i = 1; i <= NodeID::Max; i++)
00140         {
00141             Entry& entry = getEntry(i);
00142             if (entry.time_since_last_update_ms100 >= 0 &&
00143                 entry.status.mode != protocol::NodeStatus::MODE_OFFLINE)
00144             {
00145                 entry.time_since_last_update_ms100 =
00146                     int8_t(entry.time_since_last_update_ms100 + int8_t(TimerPeriodMs100));
00147 
00148                 if (entry.time_since_last_update_ms100 > OfflineTimeoutMs100)
00149                 {
00150                     Entry new_entry_value = entry;
00151                     new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100;
00152                     new_entry_value.status.mode = protocol::NodeStatus::MODE_OFFLINE;
00153                     changeNodeStatus(i, new_entry_value);
00154                 }
00155             }
00156         }
00157     }
00158 
00159 protected:
00160     /**
00161      * Called when a node becomes online, changes status or goes offline.
00162      * Refer to uavcan.protocol.NodeStatus for the offline timeout value.
00163      * Overriding is not required.
00164      */
00165     virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event)
00166     {
00167         (void)event;
00168     }
00169 
00170     /**
00171      * Called for every received message uavcan.protocol.NodeStatus after handleNodeStatusChange(), even
00172      * if the status code did not change.
00173      * Overriding is not required.
00174      */
00175     virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
00176     {
00177         (void)msg;
00178     }
00179 
00180 public:
00181     explicit NodeStatusMonitor(INode& node)
00182         : sub_(node)
00183         , timer_(node)
00184     { }
00185 
00186     virtual ~NodeStatusMonitor() { }
00187 
00188     /**
00189      * Starts the monitor.
00190      * Destroy the object to stop it.
00191      * Returns negative error code.
00192      */
00193     int start()
00194     {
00195         const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus));
00196         if (res >= 0)
00197         {
00198             timer_.setCallback(TimerCallback(this, &NodeStatusMonitor::handleTimerEvent));
00199             timer_.startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100));
00200         }
00201         return res;
00202     }
00203 
00204     /**
00205      * Make the node unknown.
00206      */
00207     void forgetNode(NodeID node_id)
00208     {
00209         if (node_id.isValid())
00210         {
00211             Entry& entry = getEntry(node_id);
00212             entry = Entry();
00213         }
00214         else
00215         {
00216             UAVCAN_ASSERT(0);
00217         }
00218     }
00219 
00220     /**
00221      * Make all nodes unknown.
00222      */
00223     void forgetAllNodes()
00224     {
00225         for (unsigned i = 0; i < (sizeof(entries_) / sizeof(entries_[0])); i++)
00226         {
00227             entries_[i] = Entry();
00228         }
00229     }
00230 
00231     /**
00232      * Returns status of a given node.
00233      * Unknown nodes are considered offline.
00234      * Complexity O(1).
00235      */
00236     NodeStatus getNodeStatus(NodeID node_id) const
00237     {
00238         if (!node_id.isValid())
00239         {
00240             UAVCAN_ASSERT(0);
00241             return NodeStatus();
00242         }
00243 
00244         const Entry& entry = getEntry(node_id);
00245         if (entry.time_since_last_update_ms100 >= 0)
00246         {
00247             return entry.status;
00248         }
00249         else
00250         {
00251             return NodeStatus();
00252         }
00253     }
00254 
00255     /**
00256      * Whether the class has observed this node at least once since initialization.
00257      * Complexity O(1).
00258      */
00259     bool isNodeKnown(NodeID node_id) const
00260     {
00261         if (!node_id.isValid())
00262         {
00263             UAVCAN_ASSERT(0);
00264             return false;
00265         }
00266         return getEntry(node_id).time_since_last_update_ms100 >= 0;
00267     }
00268 
00269     /**
00270      * This helper method allows to quickly estimate the overall network health.
00271      * Health of the local node is not considered.
00272      * Returns an invalid Node ID value if there's no known nodes in the network.
00273      */
00274     NodeID findNodeWithWorstHealth() const
00275     {
00276         NodeID nid_with_worst_health;
00277         uint8_t worst_health = protocol::NodeStatus::HEALTH_OK;
00278 
00279         for (uint8_t i = 1; i <= NodeID::Max; i++)
00280         {
00281             const NodeID nid(i);
00282             UAVCAN_ASSERT(nid.isUnicast());
00283             const Entry& entry = getEntry(nid);
00284             if (entry.time_since_last_update_ms100 >= 0)
00285             {
00286                 if (entry.status.health > worst_health || !nid_with_worst_health.isValid())
00287                 {
00288                     nid_with_worst_health = nid;
00289                     worst_health = entry.status.health;
00290                 }
00291             }
00292         }
00293 
00294         return nid_with_worst_health;
00295     }
00296 
00297     /**
00298      * Calls the operator for every known node.
00299      * Operator signature:
00300      *   void (NodeID, NodeStatus)
00301      */
00302     template <typename Operator>
00303     void forEachNode(Operator op) const
00304     {
00305         for (uint8_t i = 1; i <= NodeID::Max; i++)
00306         {
00307             const NodeID nid(i);
00308             UAVCAN_ASSERT(nid.isUnicast());
00309             const Entry& entry = getEntry(nid);
00310             if (entry.time_since_last_update_ms100 >= 0)
00311             {
00312                 op(nid, entry.status);
00313             }
00314         }
00315     }
00316 };
00317 
00318 }
00319 
00320 #endif // UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED