node_status_monitor.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <gtest/gtest.h>
00006 #include <uavcan/protocol/node_status_monitor.hpp>
00007 #include <uavcan/protocol/node_status_provider.hpp>
00008 #include "helpers.hpp"
00009 
00010 static void publishNodeStatus(CanDriverMock& can, uavcan::NodeID node_id,
00011                               uavcan::uint8_t health, uavcan::uint8_t mode,
00012                               uavcan::uint32_t uptime_sec, uavcan::TransferID tid)
00013 {
00014     uavcan::protocol::NodeStatus msg;
00015     msg.health     = health;
00016     msg.mode       = mode;
00017     msg.uptime_sec = uptime_sec;
00018     emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid);
00019 }
00020 
00021 
00022 static void shortSpin(TestNode& node)
00023 {
00024     ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
00025 }
00026 
00027 
00028 TEST(NodeStatusMonitor, Basic)
00029 {
00030     using uavcan::protocol::NodeStatus;
00031     using uavcan::NodeID;
00032 
00033     SystemClockMock clock_mock(100);
00034     clock_mock.monotonic_auto_advance = 1000;
00035 
00036     CanDriverMock can(2, clock_mock);
00037 
00038     TestNode node(can, clock_mock, 64);
00039 
00040     uavcan::GlobalDataTypeRegistry::instance().reset();
00041     uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
00042 
00043     uavcan::NodeStatusMonitor nsm(node);
00044     ASSERT_LE(0, nsm.start());
00045 
00046     ASSERT_LE(0, node.spin(uavcan::MonotonicDuration::fromMSec(10)));
00047 
00048     /*
00049      * Empty NSM, no nodes were registered yet
00050      */
00051     ASSERT_FALSE(nsm.findNodeWithWorstHealth().isValid());
00052 
00053     ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(123)));
00054     ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(123)).mode);
00055 
00056     /*
00057      * Some new status messages
00058      */
00059     publishNodeStatus(can, 10, NodeStatus::HEALTH_OK, NodeStatus::MODE_OPERATIONAL, 12, 0);
00060     shortSpin(node);
00061     ASSERT_EQ(NodeID(10), nsm.findNodeWithWorstHealth());
00062 
00063     publishNodeStatus(can, 9,  NodeStatus::HEALTH_WARNING, NodeStatus::MODE_INITIALIZATION, 0, 0);
00064     shortSpin(node);
00065     ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstHealth());
00066 
00067     publishNodeStatus(can, 11, NodeStatus::HEALTH_CRITICAL, NodeStatus::MODE_MAINTENANCE, 999, 0);
00068     shortSpin(node);
00069     ASSERT_EQ(NodeID(11), nsm.findNodeWithWorstHealth());
00070 
00071     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
00072     ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
00073     ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
00074 
00075     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
00076     ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
00077     ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
00078 
00079     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
00080     ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
00081     ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health);
00082 
00083     /*
00084      * Timeout
00085      */
00086     std::cout << "Starting timeout test, current monotime is " << clock_mock.monotonic << std::endl;
00087 
00088     clock_mock.advance(500000);
00089     shortSpin(node);
00090     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
00091     ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
00092     ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
00093 
00094     clock_mock.advance(500000);
00095     shortSpin(node);
00096     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
00097     ASSERT_EQ(NodeStatus::MODE_INITIALIZATION, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
00098     ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
00099 
00100     clock_mock.advance(500000);
00101     shortSpin(node);
00102     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
00103     ASSERT_EQ(NodeStatus::MODE_MAINTENANCE, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
00104     ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health);
00105 
00106     /*
00107      * Will timeout now
00108      */
00109     clock_mock.advance(4000000);
00110     shortSpin(node);
00111 
00112     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
00113     ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
00114     ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
00115 
00116     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
00117     ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
00118     ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
00119 
00120     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
00121     ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
00122     ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(11)).health);
00123 
00124     /*
00125      * Recovering one node, adding two extra
00126      */
00127     publishNodeStatus(can, 11, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_OPERATIONAL, 999, 1);
00128     shortSpin(node);
00129 
00130     publishNodeStatus(can, 127, NodeStatus::HEALTH_WARNING, NodeStatus::MODE_OPERATIONAL, 9999, 1);
00131     shortSpin(node);
00132 
00133     publishNodeStatus(can, 1, NodeStatus::HEALTH_OK, NodeStatus::MODE_OPERATIONAL, 1234, 1);
00134     shortSpin(node);
00135 
00136     /*
00137      * Making sure OFFLINE is still worst status
00138      */
00139     ASSERT_EQ(NodeID(9), nsm.findNodeWithWorstHealth());
00140 
00141     /*
00142      * Final validation
00143      */
00144     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(10)));
00145     ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(10)).mode);
00146     ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(10)).health);
00147 
00148     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(9)));
00149     ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
00150     ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(9)).health);
00151 
00152     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(11)));
00153     ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(11)).mode);
00154     ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(11)).health);
00155 
00156     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(127)));
00157     ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(127)).mode);
00158     ASSERT_EQ(NodeStatus::HEALTH_WARNING, nsm.getNodeStatus(uavcan::NodeID(127)).health);
00159 
00160     ASSERT_TRUE(nsm.isNodeKnown(uavcan::NodeID(1)));
00161     ASSERT_EQ(NodeStatus::MODE_OPERATIONAL, nsm.getNodeStatus(uavcan::NodeID(1)).mode);
00162     ASSERT_EQ(NodeStatus::HEALTH_OK, nsm.getNodeStatus(uavcan::NodeID(1)).health);
00163 
00164     /*
00165      * Forgetting
00166      */
00167     nsm.forgetNode(127);
00168     ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(127)));
00169     ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(127)).mode);
00170     ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(127)).health);
00171 
00172     nsm.forgetNode(9);
00173     ASSERT_FALSE(nsm.isNodeKnown(uavcan::NodeID(9)));
00174     ASSERT_EQ(NodeStatus::MODE_OFFLINE, nsm.getNodeStatus(uavcan::NodeID(9)).mode);
00175     ASSERT_EQ(NodeStatus::HEALTH_CRITICAL, nsm.getNodeStatus(uavcan::NodeID(9)).health);
00176 }