diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index bed9212eb1..69d6890077 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -41,6 +41,7 @@ class Dispatcher : Noncopyable bool add(TransferListenerBase* listener, Mode mode); void remove(TransferListenerBase* listener); + bool exists(DataTypeID dtid) const; void cleanup(MonotonicTime ts); void handleFrame(const RxFrame& frame); @@ -79,6 +80,10 @@ public: void unregisterServiceRequestListener(TransferListenerBase* listener); void unregisterServiceResponseListener(TransferListenerBase* listener); + bool hasSubscriber(DataTypeID dtid) const; + bool hasPublisher(DataTypeID dtid) const; + bool hasServer(DataTypeID dtid) const; + int getNumMessageListeners() const { return lmsg_.getNumEntries(); } int getNumServiceRequestListeners() const { return lsrv_req_.getNumEntries(); } int getNumServiceResponseListeners() const { return lsrv_resp_.getNumEntries(); } diff --git a/libuavcan/src/transport/dispatcher.cpp b/libuavcan/src/transport/dispatcher.cpp index 9851dc8fb9..a8e4ee035e 100644 --- a/libuavcan/src/transport/dispatcher.cpp +++ b/libuavcan/src/transport/dispatcher.cpp @@ -33,6 +33,18 @@ void Dispatcher::ListenerRegister::remove(TransferListenerBase* listener) list_.remove(listener); } +bool Dispatcher::ListenerRegister::exists(DataTypeID dtid) const +{ + TransferListenerBase* p = list_.get(); + while (p) + { + if (p->getDataTypeDescriptor().getID() == dtid) + return true; + p = p->getNextListNode(); + } + return false; +} + void Dispatcher::ListenerRegister::cleanup(MonotonicTime ts) { TransferListenerBase* p = list_.get(); @@ -188,6 +200,22 @@ void Dispatcher::unregisterServiceResponseListener(TransferListenerBase* listene lsrv_resp_.remove(listener); } +bool Dispatcher::hasSubscriber(DataTypeID dtid) const +{ + return lmsg_.exists(dtid); +} + +bool Dispatcher::hasPublisher(DataTypeID dtid) const +{ + return outgoing_transfer_reg_.exists(dtid, TransferTypeMessageBroadcast) + || outgoing_transfer_reg_.exists(dtid, TransferTypeMessageUnicast); +} + +bool Dispatcher::hasServer(DataTypeID dtid) const +{ + return lsrv_req_.exists(dtid); +} + bool Dispatcher::setNodeID(NodeID nid) { if (nid.isUnicast() && !self_node_id_.isValid()) diff --git a/libuavcan/test/transport/dispatcher.cpp b/libuavcan/test/transport/dispatcher.cpp index 700111e360..fb862a0319 100644 --- a/libuavcan/test/transport/dispatcher.cpp +++ b/libuavcan/test/transport/dispatcher.cpp @@ -108,8 +108,15 @@ TEST(Dispatcher, Reception) }; /* - * Sending the transfers + * Registration */ + for (int i = 0; i < NUM_SUBSCRIBERS; i++) + { + ASSERT_FALSE(dispatcher.hasSubscriber(subscribers[i]->getDataTypeDescriptor().getID())); + ASSERT_FALSE(dispatcher.hasPublisher(subscribers[i]->getDataTypeDescriptor().getID())); + ASSERT_FALSE(dispatcher.hasServer(subscribers[i]->getDataTypeDescriptor().getID())); + } + ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[0].get())); ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[1].get())); ASSERT_TRUE(dispatcher.registerMessageListener(subscribers[2].get())); @@ -117,6 +124,20 @@ TEST(Dispatcher, Reception) ASSERT_TRUE(dispatcher.registerServiceResponseListener(subscribers[4].get())); ASSERT_TRUE(dispatcher.registerServiceResponseListener(subscribers[5].get())); + for (int i = 0; i < NUM_SUBSCRIBERS; i++) + ASSERT_FALSE(dispatcher.hasPublisher(subscribers[i]->getDataTypeDescriptor().getID())); + + // Subscribers + ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[0]->getDataTypeDescriptor().getID())); + ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[1]->getDataTypeDescriptor().getID())); + ASSERT_TRUE(dispatcher.hasSubscriber(subscribers[2]->getDataTypeDescriptor().getID())); + + // Servers + ASSERT_TRUE(dispatcher.hasServer(subscribers[3]->getDataTypeDescriptor().getID())); + + /* + * Sending the transfers + */ // Multiple service request listeners are not allowed ASSERT_FALSE(dispatcher.registerServiceRequestListener(subscribers[3].get())); @@ -209,6 +230,13 @@ TEST(Dispatcher, Transmission) uavcan::Frame frame(123, uavcan::TransferTypeMessageUnicast, SELF_NODE_ID, 2, 0, 0, true); frame.setPayload(reinterpret_cast("123"), 3); + ASSERT_FALSE(dispatcher.hasPublisher(123)); + ASSERT_FALSE(dispatcher.hasPublisher(456)); + const uavcan::OutgoingTransferRegistryKey otr_key(123, uavcan::TransferTypeMessageUnicast, 2); + ASSERT_TRUE(out_trans_reg.accessOrCreate(otr_key, uavcan::MonotonicTime::fromMSec(1000000))); + ASSERT_TRUE(dispatcher.hasPublisher(123)); + ASSERT_FALSE(dispatcher.hasPublisher(456)); + ASSERT_EQ(2, dispatcher.send(frame, TX_DEADLINE, tsMono(0), uavcan::CanTxQueue::Volatile)); /*