gimbal.hpp

/****************************************************************************
 *
 * Copyright 2025 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 <Eigen/Eigen>
#include <auterion_sdk/auterion.hpp>
#include <auterion_sdk/system_state/system_state.hpp>
#include <chrono>
#include <iostream>
#include <map>
#include <thread>
#include <vector>
 
namespace auterion {
class SDK;
class GimbalImpl;
 
struct GimbalAttitude {
    Eigen::Quaternionf q_flu;
    Eigen::Quaternionf q_enu;
};
 
struct GimbalMode {
   public:
    enum class AxisMode {
        WorldLock,     
        VehicleFollow  
    };
 
    GimbalMode() = default;
    GimbalMode(AxisMode roll_mode, AxisMode pitch_mode, AxisMode yaw_mode)
        : roll(roll_mode), pitch(pitch_mode), yaw(yaw_mode) {}
 
    std::string toString() const;
 
    AxisMode roll{AxisMode::WorldLock};
    AxisMode pitch{AxisMode::WorldLock};
    AxisMode yaw{AxisMode::VehicleFollow};
};
 
struct GimbalCapabilities {
   public:
    GimbalCapabilities() = default;
 
    bool meetsRequirements(const GimbalCapabilities& requirements) const;
 
    bool supportsMode(const GimbalMode& mode) const;
 
    std::string toString() const;
 
    GimbalCapabilities& canRetract() {
        can_retract = true;
        return *this;
    }
    GimbalCapabilities& canNeutral() {
        can_neutral = true;
        return *this;
    }
    GimbalCapabilities& canRoll() {
        can_roll = true;
        return *this;
    }
    GimbalCapabilities& canRollFollow() {
        can_roll_follow = true;
        return *this;
    }
    GimbalCapabilities& canRollLock() {
        can_roll_lock = true;
        return *this;
    }
    GimbalCapabilities& canPitch() {
        can_pitch = true;
        return *this;
    }
    GimbalCapabilities& canPitchFollow() {
        can_pitch_follow = true;
        return *this;
    }
    GimbalCapabilities& canPitchLock() {
        can_pitch_lock = true;
        return *this;
    }
    GimbalCapabilities& canYaw() {
        can_yaw = true;
        return *this;
    }
    GimbalCapabilities& canYawFollow() {
        can_yaw_follow = true;
        return *this;
    }
    GimbalCapabilities& canYawLock() {
        can_yaw_lock = true;
        return *this;
    }
    GimbalCapabilities& canInfiniteYaw() {
        can_infinite_yaw = true;
        return *this;
    }
    GimbalCapabilities& canPointLocationLocal() {
        can_point_location_local = true;
        return *this;
    }
    GimbalCapabilities& canPointLocationGlobal() {
        can_point_location_global = true;
        return *this;
    }
 
    GimbalCapabilities& RollRangeDeg(float min_deg, float max_deg) {
        roll_min_deg = min_deg;
        roll_max_deg = max_deg;
        return *this;
    }
    GimbalCapabilities& PitchRangeDeg(float min_deg, float max_deg) {
        pitch_min_deg = min_deg;
        pitch_max_deg = max_deg;
        return *this;
    }
    GimbalCapabilities& YawRangeDeg(float min_deg, float max_deg) {
        yaw_min_deg = min_deg;
        yaw_max_deg = max_deg;
        return *this;
    }
 
    bool can_retract{false};
    bool can_neutral{false};
    bool can_roll{false};
    bool can_roll_follow{false};
    bool can_roll_lock{false};
    bool can_pitch{false};
    bool can_pitch_follow{false};
    bool can_pitch_lock{false};
    bool can_yaw{false};
    bool can_yaw_follow{false};
    bool can_yaw_lock{false};
    bool can_infinite_yaw{false};
    bool can_point_location_local{false};
    bool can_point_location_global{false};
 
    float roll_min_deg{0};   
    float roll_max_deg{0};   
    float pitch_min_deg{0};  
    float pitch_max_deg{0};  
    float yaw_min_deg{0};    
    float yaw_max_deg{0};    
};
 
struct GimbalDescriptor {
   public:
    GimbalDescriptor() = default;
    explicit GimbalDescriptor(const GimbalCapabilities& capabilities)
        : capabilities(capabilities) {}
    explicit GimbalDescriptor(const std::string& custom_name, const std::string& vendor_name,
                              const std::string& model_name, const GimbalCapabilities& capabilities)
        : custom_name(custom_name),
          vendor_name(vendor_name),
          model_name(model_name),
          capabilities(capabilities) {}
 
    std::string custom_name{};
    std::string vendor_name{};
    std::string model_name{};
    GimbalCapabilities capabilities{};
 
    std::string toString() const;
};
 
class Gimbal {
   public:
    Gimbal(SDK& sdk, const std::function<bool(const GimbalDescriptor& candidate)>& find_callback);
 
    Gimbal(SDK& sdk, const GimbalDescriptor& descriptor = GimbalDescriptor{})
        : Gimbal(sdk, [&](const GimbalDescriptor& candidate) {
              if (!descriptor.custom_name.empty()) {
                  return descriptor.custom_name == candidate.custom_name;
              }
              if (!descriptor.model_name.empty() || !descriptor.vendor_name.empty()) {
                  return descriptor.model_name == candidate.model_name &&
                         descriptor.vendor_name == candidate.vendor_name;
              }
              return candidate.capabilities.meetsRequirements(descriptor.capabilities);
          }) {}
 
    ~Gimbal() = default;
 
    void setGimbalMode(const GimbalMode& gimbal_mode);
 
    bool takeControl(const bool force_takeover = false);
 
    void releaseControl();
 
    void setRateControl(const float roll_rate_rad_s, const float pitch_rate_rad_s,
                        const float yaw_rate_rad_s) const;
 
    void setAttitudeControl(const Eigen::Quaternionf& q) const;
 
    void setAttitudeControl(const float roll_deg, const float pitch_deg, const float yaw_deg) const;
 
    void setRoiLocation(const auterion::GlobalPosition& position) const;
 
    void setRoiLocationNone() const;
 
    void subscribeAttitude(std::function<void(const GimbalAttitude&)> callback);
 
   private:
    std::shared_ptr<GimbalImpl> _impl;
};
 
}  // namespace auterion