Fix #547 Add callback for components discovered.

core/include/system.h

@@ -1,9 +1,20 @@
 #pragma once
 
 #include <memory>
+#include <functional>
 
 namespace dronecode_sdk {
 
+/**
+ * @brief Component Types
+ */
+enum ComponentType { UNKNOWN = 0, AUTOPILOT, CAMERA, GIMBAL };
+
+/**
+ * @brief type for component discovery callback
+ */
+typedef std::function<void(ComponentType)> discover_callback_t;
+
 class SystemImpl;
 class DronecodeSDKImpl;
 class PluginImplBase;
@@ -76,6 +87,14 @@ class System {
      */
     uint64_t get_uuid() const;
 
+    /**
+     * @brief Register a callback to be called when a component is discovered.
+     *
+     * @param callback a function of type void(ComponentType) which will be called with the
+     * component type of the new component.
+     */
+    void register_component_discovered_callback(discover_callback_t callback) const;
+
     /**
      * @brief Copy constructor (object is not copyable).
      */

core/system.cpp

@@ -53,4 +53,9 @@ uint64_t System::get_uuid() const
     return _system_impl->get_uuid();
 }
 
+void System::register_component_discovered_callback(discover_callback_t callback) const
+{
+    return _system_impl->register_component_discovered_callback(callback);
+}
+
 } // namespace dronecode_sdk

core/system_impl.cpp

@@ -25,6 +25,8 @@ SystemImpl::SystemImpl(DronecodeSDKImpl &parent, uint8_t system_id, uint8_t comp
     _call_every_handler(_time),
     _thread_pool(3)
 {
+    component_discovered_callback = nullptr;
+
     _system_thread = new std::thread(&SystemImpl::system_thread, this);
 
     register_mavlink_message_handler(
@@ -328,10 +330,33 @@ std::string SystemImpl::component_name(uint8_t component_id)
     }
 }
 
+ComponentType SystemImpl::component_type(uint8_t component_id)
+{
+    switch (component_id) {
+        case MAV_COMP_ID_AUTOPILOT1:
+            return AUTOPILOT;
+        case MAV_COMP_ID_CAMERA:
+        case MAV_COMP_ID_CAMERA2:
+        case MAV_COMP_ID_CAMERA3:
+        case MAV_COMP_ID_CAMERA4:
+        case MAV_COMP_ID_CAMERA5:
+        case MAV_COMP_ID_CAMERA6:
+            return CAMERA;
+        case MAV_COMP_ID_GIMBAL:
+            return GIMBAL;
+        default:
+            return UNKNOWN;
+    }
+}
+
 void SystemImpl::add_new_component(uint8_t component_id)
 {
     auto res_pair = _components.insert(component_id);
     if (res_pair.second) {
+        if (component_discovered_callback != nullptr) {
+            const ComponentType type = component_type(component_id);
+            call_user_callback([this, type]() { component_discovered_callback(type); });
+        }
         LogDebug() << "Component " << component_name(component_id) << " added.";
     }
 }
@@ -341,6 +366,18 @@ size_t SystemImpl::total_components() const
     return _components.size();
 }
 
+void SystemImpl::register_component_discovered_callback(discover_callback_t callback)
+{
+    component_discovered_callback = callback;
+
+    if (total_components() > 0) {
+        for (const auto &elem : _components) {
+            const ComponentType type = component_type(elem);
+            call_user_callback([this, type]() { component_discovered_callback(type); });
+        }
+    }
+}
+
 bool SystemImpl::is_standalone() const
 {
     return !has_autopilot();

core/system_impl.h

@@ -7,6 +7,7 @@
 #include "timeout_handler.h"
 #include "call_every_handler.h"
 #include "thread_pool.h"
+#include "system.h"
 #include <cstdint>
 #include <functional>
 #include <atomic>
@@ -97,6 +98,9 @@ class SystemImpl {
     void add_new_component(uint8_t component_id);
     size_t total_components() const;
 
+    void register_component_discovered_callback(discover_callback_t callback);
+    discover_callback_t component_discovered_callback;
+
     uint8_t get_autopilot_id() const;
     std::vector<uint8_t> get_camera_ids() const;
     uint8_t get_gimbal_id() const;
@@ -190,6 +194,7 @@ class SystemImpl {
     void set_disconnected();
 
     static std::string component_name(uint8_t component_id);
+    static ComponentType component_type(uint8_t component_id);
 
     void system_thread();
     void send_heartbeat();

example/takeoff_land/takeoff_and_land.cpp

@@ -5,6 +5,7 @@
 
 #include <chrono>
 #include <cstdint>
+#include <dronecode_sdk/system.h>
 #include <dronecode_sdk/action.h>
 #include <dronecode_sdk/dronecode_sdk.h>
 #include <dronecode_sdk/telemetry.h>
@@ -29,6 +30,12 @@ void usage(std::string bin_name)
               << "For example, to connect to the simulator use URL: udp://:14540" << std::endl;
 }
 
+void component_discovered(ComponentType component_type)
+{
+    std::cout << NORMAL_CONSOLE_TEXT << "Discovered a component with type "
+              << unsigned(component_type) << std::endl;
+}
+
 int main(int argc, char **argv)
 {
     DronecodeSDK dc;
@@ -51,6 +58,11 @@ int main(int argc, char **argv)
         return 1;
     }
 
+    // We don't need to specify the UUID if it's only one system anyway.
+    // If there were multiple, we could specify it with:
+    // dc.system(uint64_t uuid);
+    System &system = dc.system();
+
     std::cout << "Waiting to discover system..." << std::endl;
     dc.register_on_discover([&discovered_system](uint64_t uuid) {
         std::cout << "Discovered system with UUID: " << uuid << std::endl;
@@ -67,10 +79,9 @@ int main(int argc, char **argv)
         return 1;
     }
 
-    // We don't need to specify the UUID if it's only one system anyway.
-    // If there were multiple, we could specify it with:
-    // dc.system(uint64_t uuid);
-    System &system = dc.system();
+    // Register a callback so we get told when components (camera, gimbal) etc
+    // are found.
+    system.register_component_discovered_callback(component_discovered);
 
     auto telemetry = std::make_shared<Telemetry>(system);
     auto action = std::make_shared<Action>(system);