auterion.hpp

/****************************************************************************
 *
 * Copyright 2023 Auterion AG. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *
 * 1. Redistributions of source code must retain the above copyright notice, this
 * list of conditions and the following disclaimer.
 *
 * 2. Redistributions in binary form must reproduce the above copyright notice,
 * this list of conditions and the following disclaimer in the documentation
 * and/or other materials provided with the distribution.
 *
 * 3. Neither the name of the copyright holder nor the names of its contributors
 * may be used to endorse or promote products derived from this software without
 * specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
 
#pragma once
 
#include <atomic>
#include <auterion_sdk/exception/exception.hpp>
#include <functional>
#include <rclcpp/rclcpp.hpp>
#include <string>
#include <thread>
#include <variant>
#include <vector>
 
namespace auterion {
class ParameterManager;
 
struct RunOptions {
    RunOptions& withSkipFlightControllerConnectionCheck(bool skip) {
        skip_flight_controller_connection_check = skip;
        return *this;
    }
 
    RunOptions& withSkipMessageCompatibilityCheck(bool skip) {
        skip_message_compatibility_check = skip;
        return *this;
    }
 
    RunOptions& withFlightControllerConnectionTimeout(std::chrono::seconds timeout) {
        if (timeout.count() <= 0) {
            throw InvalidArgumentException{
                "flight_controller_connection_timeout must be positive (got: " +
                std::to_string(timeout.count()) + "s)."};
        }
        flight_controller_connection_timeout = timeout;
        return *this;
    }
 
    void validate() const {
        if (skip_flight_controller_connection_check && !skip_message_compatibility_check) {
            throw InvalidArgumentException(
                "Cannot check message compatibility if connection check is skipped. "
                "Enable flight controller connection check, or disable message compatibility "
                "check.");
        }
    }
 
    bool skip_flight_controller_connection_check{false};
    bool skip_message_compatibility_check{true};
    std::chrono::seconds flight_controller_connection_timeout{15};
};
 
;
 
class SDK {
   public:
    SDK(int argc, char** argv, const std::string& node_name);
 
    ~SDK();
 
    [[deprecated("Use run(RunOptions) instead, this method will be removed in a future release.")]]
    void
    setSkipFlightControllerConnectionCheck(bool skip) {
        _skip_flight_controller_connection_check = skip;
    }
 
    void run(RunOptions options = RunOptions());
 
    rclcpp::Node::SharedPtr defaultRosHandle() const { return _default_ros_node; }
 
    bool canWaitForData() const {
        const std::thread::id executor_thread = _executor_thread.load();
        return executor_thread != std::thread::id() &&
               executor_thread != std::this_thread::get_id();
    }
 
    rclcpp::Time now() const { return _default_ros_node->now(); }
 
    void addNode(rclcpp::Node::SharedPtr node) { _executor->add_node(node); }
    void removeNode(rclcpp::Node::SharedPtr node) { _executor->remove_node(node); }
 
    using ShutdownCallbackHandle = rclcpp::PreShutdownCallbackHandle;
 
    ShutdownCallbackHandle addShutdownCallback(std::function<void()> callback);
    void removeShutdownCallback(const ShutdownCallbackHandle& handle);
 
    void requestShutdown(const std::string& reason);
 
    const rclcpp::executors::SingleThreadedExecutor::UniquePtr& executor() const {
        return _executor;
    }
 
    // Function to verify supported parameter types at compile time
    template <typename T>
    static constexpr void verify_param() {
        static_assert(std::is_same<T, bool>::value || std::is_same<T, int64_t>::value ||
                          std::is_same<T, int>::value || std::is_same<T, float>::value ||
                          std::is_same<T, double>::value || std::is_same<T, std::string>::value ||
                          std::is_same<T, std::vector<bool>>::value ||
                          std::is_same<T, std::vector<int64_t>>::value ||
                          std::is_same<T, std::vector<int>>::value ||
                          std::is_same<T, std::vector<float>>::value ||
                          std::is_same<T, std::vector<double>>::value ||
                          std::is_same<T, std::vector<std::string>>::value,
                      "Unsupported parameter data type");
    }
 
    template <typename T>
    T getParameter(const std::string& name,
                   std::function<void(const T&)> on_change_callback = nullptr) {
        verify_param<T>();
    }
 
    template <typename T>
    void setParameter(const std::string& name, const T& value) {
        verify_param<T>();
        setParameterValue(name, value, ParameterScope::App);
    }
 
    template <typename T>
    void setSystemParameter(const std::string& full_name, const T& value) {
        verify_param<T>();
        setParameterValue(full_name, value, ParameterScope::System);
    }
 
   private:
    using ParameterValue =
        std::variant<bool, int64_t, int, float, double, std::string, std::vector<bool>,
                     std::vector<int64_t>, std::vector<int>, std::vector<float>,
                     std::vector<double>, std::vector<std::string>>;
    enum class ParameterScope { App, System };
    void setParameterValue(const std::string& name, const ParameterValue& value,
                           ParameterScope scope);
 
    rclcpp::Node::SharedPtr _default_ros_node;
    std::atomic<std::thread::id> _executor_thread{};  
    rclcpp::executors::SingleThreadedExecutor::UniquePtr _executor;
    bool _skip_flight_controller_connection_check{false};  // deprecating, use RunOptions
    std::unique_ptr<ParameterManager> _parameter_manager;
};
 
template <>
bool SDK::getParameter(const std::string& name,
                       std::function<void(const bool&)> on_change_callback);
 
template <>
int64_t SDK::getParameter(const std::string& name,
                          std::function<void(const int64_t&)> on_change_callback);
 
template <>
int SDK::getParameter(const std::string& name, std::function<void(const int&)> on_change_callback);
 
template <>
float SDK::getParameter(const std::string& name,
                        std::function<void(const float&)> on_change_callback);
 
template <>
double SDK::getParameter(const std::string& name,
                         std::function<void(const double&)> on_change_callback);
 
template <>
std::string SDK::getParameter(const std::string& name,
                              std::function<void(const std::string&)> on_change_callback);
 
template <>
std::vector<bool> SDK::getParameter(
    const std::string& name, std::function<void(const std::vector<bool>&)> on_change_callback);
 
template <>
std::vector<int64_t> SDK::getParameter(
    const std::string& name, std::function<void(const std::vector<int64_t>&)> on_change_callback);
 
template <>
std::vector<int> SDK::getParameter(const std::string& name,
                                   std::function<void(const std::vector<int>&)> on_change_callback);
 
template <>
std::vector<float> SDK::getParameter(
    const std::string& name, std::function<void(const std::vector<float>&)> on_change_callback);
 
template <>
std::vector<double> SDK::getParameter(
    const std::string& name, std::function<void(const std::vector<double>&)> on_change_callback);
 
template <>
std::vector<std::string> SDK::getParameter(
    const std::string& name,
    std::function<void(const std::vector<std::string>&)> on_change_callback);
 
}  // namespace auterion