connection: added a more intelligent callback API

Auterion · Mar 28, 2024 · b4d95ec · b4d95ec
1 parent 141060b
commit b4d95ec
Show file tree

Hide file tree

Showing 2 changed files with 44 additions and 5 deletions.
diff --git a/include/mav/Connection.h b/include/mav/Connection.h
@@ -192,18 +192,41 @@ namespace mav {
             return !_underlying_network_fault && (millis() - _last_received_ms < CONNECTION_TIMEOUT);
         }
 
-        template<typename T, typename E>
-        CallbackHandle addMessageCallback(const T &on_message, const E &on_error) {
+        CallbackHandle addMessageCallback(const std::function<void(const mav::Message&)> &on_message,
+                                          const std::function<void(const std::exception_ptr&)> on_error) {
             std::scoped_lock<std::mutex> lock(_message_callback_mtx);
             CallbackHandle handle = _next_handle;
             _message_callbacks[handle] = FunctionCallback{on_message, on_error};
             _next_handle++;
             return handle;
         }
 
-        template<typename T>
-        CallbackHandle addMessageCallback(const T &on_message) {
-            return addMessageCallback(on_message, nullptr);
+        CallbackHandle addMessageCallback(const std::function<void(const mav::Message&)> &on_message) {
+            return addMessageCallback(on_message, std::function<void(const std::exception_ptr&)>{});
+        }
+
+        CallbackHandle addMessageCallback(const std::function<bool(const mav::Message&)> &selector,
+                                          const std::function<void(const mav::Message&)> &on_message,
+                                          const std::function<void(const std::exception_ptr&)> &on_error) {
+            return addMessageCallback([selector, on_message](const Message &message) {
+                if (selector(message)) {
+                    on_message(message);
+                }
+            }, on_error);
+        }
+
+        CallbackHandle addMessageCallback(int message_id, std::function<void(const mav::Message&)> on_message,
+                                          int source_id=mav::ANY_ID, int component_id=mav::ANY_ID) {
+            return addMessageCallback([message_id, source_id, component_id](const Message &message) {
+                return message.id() == message_id &&
+                    (source_id == mav::ANY_ID || message.header().systemId() == source_id) &&
+                    (component_id == mav::ANY_ID || message.header().componentId() == component_id);
+            }, on_message, std::function<void(const std::exception_ptr&)>{});
+        }
+
+        CallbackHandle addMessageCallback(const std::string &message_name, std::function<void(const mav::Message&)> on_message,
+                                          int source_id=mav::ANY_ID, int component_id=mav::ANY_ID) {
+            return addMessageCallback(_message_set.idForMessage(message_name), on_message, source_id, component_id);
         }
 
         void removeMessageCallback(CallbackHandle handle) {

diff --git a/tests/Network.cpp b/tests/Network.cpp
@@ -308,4 +308,20 @@ TEST_CASE("Create network runtime") {
         connection->receive("HEARTBEAT");
         CHECK_EQ(connection->callbackCount(), 0);
     }
+
+    SUBCASE("Message callback for specific message is called when message arrives") {
+        interface.reset();
+        std::promise<void> callback_called_promise;
+        auto callback_called_future = callback_called_promise.get_future();
+
+        connection->addMessageCallback("TEST_MESSAGE", [&callback_called_promise](const Message &message) {
+            if (message.name() == "TEST_MESSAGE") {
+                callback_called_promise.set_value();
+            }
+        });
+
+        interface.addToReceiveQueue("\xfd\x10\x00\x00\x01\x61\x61\xbc\x26\x00\x2a\x00\x00\x00\x48\x65\x6c\x6c\x6f\x20\x57\x6f\x72\x6c\x64\x21\x53\xd9"s, interface_partner);
+        CHECK((callback_called_future.wait_for(std::chrono::seconds(2)) != std::future_status::timeout));
+        connection->removeAllCallbacks();
+    }
 }