subscriber.cpp

00001 /*
00002  * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
00003  */
00004 
00005 #include <gtest/gtest.h>
00006 #include <uavcan/node/subscriber.hpp>
00007 #include <uavcan/util/method_binder.hpp>
00008 #include <root_ns_a/EmptyMessage.hpp>
00009 #include <root_ns_a/MavlinkMessage.hpp>
00010 #include "../clock.hpp"
00011 #include "../transport/can/can.hpp"
00012 #include "test_node.hpp"
00013 
00014 
00015 template <typename DataType>
00016 struct SubscriptionListener
00017 {
00018     typedef uavcan::ReceivedDataStructure<DataType> ReceivedDataStructure;
00019 
00020     struct ReceivedDataStructureCopy
00021     {
00022         uavcan::MonotonicTime ts_monotonic;
00023         uavcan::UtcTime ts_utc;
00024         uavcan::TransferType transfer_type;
00025         uavcan::TransferID transfer_id;
00026         uavcan::NodeID src_node_id;
00027         uavcan::uint8_t iface_index;
00028         DataType msg;
00029 
00030         ReceivedDataStructureCopy(const ReceivedDataStructure& s)
00031             : ts_monotonic(s.getMonotonicTimestamp())
00032             , ts_utc(s.getUtcTimestamp())
00033             , transfer_type(s.getTransferType())
00034             , transfer_id(s.getTransferID())
00035             , src_node_id(s.getSrcNodeID())
00036             , iface_index(s.getIfaceIndex())
00037             , msg(s)
00038         { }
00039     };
00040 
00041     std::vector<DataType> simple;
00042     std::vector<ReceivedDataStructureCopy> extended;
00043 
00044     void receiveExtended(ReceivedDataStructure& msg)
00045     {
00046         extended.push_back(msg);
00047     }
00048 
00049     void receiveSimple(DataType& msg)
00050     {
00051         simple.push_back(msg);
00052     }
00053 
00054     typedef SubscriptionListener<DataType> SelfType;
00055     typedef uavcan::MethodBinder<SelfType*, void (SelfType::*)(ReceivedDataStructure&)> ExtendedBinder;
00056     typedef uavcan::MethodBinder<SelfType*, void (SelfType::*)(DataType&)> SimpleBinder;
00057 
00058     ExtendedBinder bindExtended() { return ExtendedBinder(this, &SelfType::receiveExtended); }
00059     SimpleBinder bindSimple() { return SimpleBinder(this, &SelfType::receiveSimple); }
00060 };
00061 
00062 
00063 TEST(Subscriber, Basic)
00064 {
00065     // Manual type registration - we can't rely on the GDTR state
00066     uavcan::GlobalDataTypeRegistry::instance().reset();
00067     uavcan::DefaultDataTypeRegistrator<root_ns_a::MavlinkMessage> _registrator;
00068 
00069     SystemClockDriver clock_driver;
00070     CanDriverMock can_driver(2, clock_driver);
00071     TestNode node(can_driver, clock_driver, 1);
00072 
00073     typedef SubscriptionListener<root_ns_a::MavlinkMessage> Listener;
00074 
00075     uavcan::Subscriber<root_ns_a::MavlinkMessage, Listener::ExtendedBinder> sub_extended(node);
00076     uavcan::Subscriber<root_ns_a::MavlinkMessage, Listener::ExtendedBinder> sub_extended2(node); // Not used
00077     uavcan::Subscriber<root_ns_a::MavlinkMessage, Listener::SimpleBinder> sub_simple(node);
00078     uavcan::Subscriber<root_ns_a::MavlinkMessage, Listener::SimpleBinder> sub_simple2(node);     // Not used
00079 
00080     std::cout <<
00081         "sizeof(uavcan::Subscriber<root_ns_a::MavlinkMessage, Listener::ExtendedBinder>): " <<
00082         sizeof(uavcan::Subscriber<root_ns_a::MavlinkMessage, Listener::ExtendedBinder>) << std::endl;
00083 
00084     // Null binder - will fail
00085     ASSERT_EQ(-uavcan::ErrInvalidParam, sub_extended.start(Listener::ExtendedBinder(UAVCAN_NULLPTR, UAVCAN_NULLPTR)));
00086 
00087     Listener listener;
00088 
00089     /*
00090      * Message layout:
00091      * uint8 seq
00092      * uint8 sysid
00093      * uint8 compid
00094      * uint8 msgid
00095      * uint8[<256] payload
00096      */
00097     root_ns_a::MavlinkMessage expected_msg;
00098     expected_msg.seq = 0x42;
00099     expected_msg.sysid = 0x72;
00100     expected_msg.compid = 0x08;
00101     expected_msg.msgid = 0xa5;
00102     expected_msg.payload = "Msg";
00103 
00104     const uint8_t transfer_payload[] = {0x42, 0x72, 0x08, 0xa5, 'M', 's', 'g'};
00105 
00106     /*
00107      * RxFrame generation
00108      */
00109     std::vector<uavcan::RxFrame> rx_frames;
00110     for (uint8_t i = 0; i < 4; i++)
00111     {
00112         uavcan::TransferType tt = uavcan::TransferTypeMessageBroadcast;
00113         uavcan::NodeID dni = (tt == uavcan::TransferTypeMessageBroadcast) ?
00114                              uavcan::NodeID::Broadcast : node.getDispatcher().getNodeID();
00115         // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
00116         // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
00117         uavcan::Frame frame(root_ns_a::MavlinkMessage::DefaultDataTypeID, tt, uavcan::NodeID(uint8_t(i + 100)),
00118                             dni, i);
00119         frame.setStartOfTransfer(true);
00120         frame.setEndOfTransfer(true);
00121         frame.setPayload(transfer_payload, 7);
00122         uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
00123         rx_frames.push_back(rx_frame);
00124     }
00125 
00126     /*
00127      * Reception
00128      */
00129     ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners());
00130 
00131     ASSERT_EQ(0, sub_extended.start(listener.bindExtended()));
00132     ASSERT_EQ(0, sub_extended2.start(listener.bindExtended()));
00133     ASSERT_EQ(0, sub_simple.start(listener.bindSimple()));
00134     ASSERT_EQ(0, sub_simple2.start(listener.bindSimple()));
00135 
00136     ASSERT_EQ(4, node.getDispatcher().getNumMessageListeners());
00137 
00138     sub_extended2.stop();  // These are not used - making sure they aren't receiving anything
00139     sub_simple2.stop();
00140 
00141     ASSERT_EQ(2, node.getDispatcher().getNumMessageListeners());
00142 
00143     for (unsigned i = 0; i < rx_frames.size(); i++)
00144     {
00145         can_driver.ifaces[0].pushRx(rx_frames[i]);
00146         can_driver.ifaces[1].pushRx(rx_frames[i]);
00147     }
00148 
00149     ASSERT_LE(0, node.spin(clock_driver.getMonotonic() + durMono(10000)));
00150 
00151     /*
00152      * Validation
00153      */
00154     ASSERT_EQ(listener.extended.size(), rx_frames.size());
00155     for (unsigned i = 0; i < rx_frames.size(); i++)
00156     {
00157         const Listener::ReceivedDataStructureCopy s = listener.extended.at(i);
00158         ASSERT_TRUE(s.msg == expected_msg);
00159         ASSERT_EQ(rx_frames[i].getSrcNodeID(), s.src_node_id);
00160         ASSERT_EQ(rx_frames[i].getTransferID(), s.transfer_id);
00161         ASSERT_EQ(rx_frames[i].getTransferType(), s.transfer_type);
00162         ASSERT_EQ(rx_frames[i].getMonotonicTimestamp(), s.ts_monotonic);
00163         ASSERT_EQ(rx_frames[i].getIfaceIndex(), s.iface_index);
00164     }
00165 
00166     ASSERT_EQ(listener.simple.size(), rx_frames.size());
00167     for (unsigned i = 0; i < rx_frames.size(); i++)
00168     {
00169         ASSERT_TRUE(listener.simple.at(i) == expected_msg);
00170     }
00171 
00172     ASSERT_EQ(0, sub_extended.getFailureCount());
00173     ASSERT_EQ(0, sub_simple.getFailureCount());
00174 
00175     /*
00176      * Unregistration
00177      */
00178     ASSERT_EQ(2, node.getDispatcher().getNumMessageListeners());
00179 
00180     sub_extended.stop();
00181     sub_extended2.stop();
00182     sub_simple.stop();
00183     sub_simple2.stop();
00184 
00185     ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners());
00186 }
00187 
00188 
00189 static void panickingSink(const uavcan::ReceivedDataStructure<root_ns_a::MavlinkMessage>&)
00190 {
00191     FAIL() << "I just went mad";
00192 }
00193 
00194 
00195 TEST(Subscriber, FailureCount)
00196 {
00197     // Manual type registration - we can't rely on the GDTR state
00198     uavcan::GlobalDataTypeRegistry::instance().reset();
00199     uavcan::DefaultDataTypeRegistrator<root_ns_a::MavlinkMessage> _registrator;
00200 
00201     SystemClockDriver clock_driver;
00202     CanDriverMock can_driver(2, clock_driver);
00203     TestNode node(can_driver, clock_driver, 1);
00204 
00205     {
00206         uavcan::Subscriber<root_ns_a::MavlinkMessage> sub(node);
00207         ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners());
00208         sub.start(panickingSink);
00209         ASSERT_EQ(1, node.getDispatcher().getNumMessageListeners());
00210 
00211         ASSERT_EQ(0, sub.getFailureCount());
00212 
00213         for (uint8_t i = 0; i < 4; i++)
00214         {
00215             // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
00216             // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
00217             uavcan::Frame frame(root_ns_a::MavlinkMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
00218                                 uavcan::NodeID(uint8_t(i + 100)), uavcan::NodeID::Broadcast, i);
00219             frame.setStartOfTransfer(true);
00220             frame.setEndOfTransfer(true);
00221             // No payload - broken transfer
00222             uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
00223             can_driver.ifaces[0].pushRx(rx_frame);
00224             can_driver.ifaces[1].pushRx(rx_frame);
00225         }
00226 
00227         ASSERT_LE(0, node.spin(clock_driver.getMonotonic() + durMono(10000)));
00228 
00229         ASSERT_EQ(4, sub.getFailureCount());
00230 
00231         ASSERT_EQ(1, node.getDispatcher().getNumMessageListeners()); // Still there
00232     }
00233     ASSERT_EQ(0, node.getDispatcher().getNumMessageListeners());     // Removed
00234 }
00235 
00236 
00237 TEST(Subscriber, SingleFrameTransfer)
00238 {
00239     // Manual type registration - we can't rely on the GDTR state
00240     uavcan::GlobalDataTypeRegistry::instance().reset();
00241     uavcan::DefaultDataTypeRegistrator<root_ns_a::EmptyMessage> _registrator;
00242 
00243     SystemClockDriver clock_driver;
00244     CanDriverMock can_driver(2, clock_driver);
00245     TestNode node(can_driver, clock_driver, 1);
00246 
00247     typedef SubscriptionListener<root_ns_a::EmptyMessage> Listener;
00248 
00249     uavcan::Subscriber<root_ns_a::EmptyMessage, Listener::SimpleBinder> sub(node);
00250 
00251     std::cout <<
00252         "sizeof(uavcan::Subscriber<root_ns_a::EmptyMessage, Listener::SimpleBinder>): " <<
00253         sizeof(uavcan::Subscriber<root_ns_a::EmptyMessage, Listener::SimpleBinder>) << std::endl;
00254 
00255     Listener listener;
00256 
00257     sub.start(listener.bindSimple());
00258 
00259     for (uint8_t i = 0; i < 4; i++)
00260     {
00261         // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id,
00262         // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame
00263         uavcan::Frame frame(root_ns_a::EmptyMessage::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
00264                             uavcan::NodeID(uint8_t(i + 100)), uavcan::NodeID::Broadcast, i);
00265         frame.setStartOfTransfer(true);
00266         frame.setEndOfTransfer(true);
00267         // No payload - message is empty
00268         uavcan::RxFrame rx_frame(frame, clock_driver.getMonotonic(), clock_driver.getUtc(), 0);
00269         can_driver.ifaces[0].pushRx(rx_frame);
00270         can_driver.ifaces[1].pushRx(rx_frame);
00271     }
00272 
00273     ASSERT_LE(0, node.spin(clock_driver.getMonotonic() + durMono(10000)));
00274 
00275     ASSERT_EQ(0, sub.getFailureCount());
00276 
00277     ASSERT_EQ(4, listener.simple.size());
00278     for (unsigned i = 0; i < 4; i++)
00279     {
00280         ASSERT_TRUE(listener.simple.at(i) == root_ns_a::EmptyMessage());
00281     }
00282 }