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 <auterion_sdk/exception/exception.hpp>
#include <rclcpp/rclcpp.hpp>
 
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; }
 
    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); }
 
    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>();
    }
 
   private:
    rclcpp::Node::SharedPtr _default_ros_node;
    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);
 
template <>
void SDK::setParameter(const std::string& name, const bool& value);
 
template <>
void SDK::setParameter(const std::string& name, const int64_t& value);
 
template <>
void SDK::setParameter(const std::string& name, const int& value);
 
template <>
void SDK::setParameter(const std::string& name, const float& value);
 
template <>
void SDK::setParameter(const std::string& name, const double& value);
 
template <>
void SDK::setParameter(const std::string& name, const std::string& value);
 
template <>
void SDK::setParameter(const std::string& name, const std::vector<bool>& value);
 
template <>
void SDK::setParameter(const std::string& name, const std::vector<int64_t>& value);
 
template <>
void SDK::setParameter(const std::string& name, const std::vector<int>& value);
 
template <>
void SDK::setParameter(const std::string& name, const std::vector<float>& value);
 
template <>
void SDK::setParameter(const std::string& name, const std::vector<double>& value);
 
template <>
void SDK::setParameter(const std::string& name, const std::vector<std::string>& value);
 
}  // namespace auterion