mode.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 <Eigen/Eigen>
#include <auterion_sdk/auterion.hpp>
#include <auterion_sdk/control/control.hpp>
#include <functional>
#include <memory>
#include <variant>
 
namespace px4_ros2 {
class HealthAndArmingCheckReporter;
}
 
namespace auterion {
class ModeConfiguration {
   public:
    struct PX4Mode {
        using ID = uint8_t;
        static const ID kModeIDInvalid;  
 
        static const ID kModeIDPosctl;         
        static const ID kModeIDTakeoff;        
        static const ID kModeIDDescend;        
        static const ID kModeIDLand;           
        static const ID kModeIDRtl;            
        static const ID kModeIDPrecisionLand;  
    };
 
    PX4Mode::ID replace_px4_mode{
        PX4Mode::kModeIDInvalid};  
 
    bool needs_angular_velocity{false};
    bool needs_attitude{false};
    bool needs_local_altitude{false};
    bool needs_local_position{false};
    bool needs_local_position_relaxed{false};
    bool needs_global_position{false};
    bool needs_valid_mission{false};
    bool needs_valid_home{false};
    bool needs_manual_control_input{false};
    bool can_prevent_arming{false};
    bool user_selectable{true};
 
    ModeConfiguration merge(const ModeConfiguration& other) const {
        ModeConfiguration config = *this;
        config.needs_angular_velocity =
            config.needs_angular_velocity || other.needs_angular_velocity;
        config.needs_attitude = config.needs_attitude || other.needs_attitude;
        config.needs_local_altitude = config.needs_local_altitude || other.needs_local_altitude;
        config.needs_local_position = config.needs_local_position || other.needs_local_position;
        config.needs_local_position_relaxed =
            config.needs_local_position_relaxed || other.needs_local_position_relaxed;
        config.needs_global_position = config.needs_global_position || other.needs_global_position;
        config.needs_valid_mission = config.needs_valid_mission || other.needs_valid_mission;
        config.needs_valid_home = config.needs_valid_home || other.needs_valid_home;
        config.needs_manual_control_input =
            config.needs_manual_control_input || other.needs_manual_control_input;
        config.can_prevent_arming = config.can_prevent_arming || other.can_prevent_arming;
        config.user_selectable = config.user_selectable || other.user_selectable;
        return config;
    }
 
    ModeConfiguration& replacesPX4Mode(PX4Mode::ID px4_mode_id) {
        replace_px4_mode = px4_mode_id;
        return *this;
    }
 
    ModeConfiguration& needsAngularVelocity(bool needs = true) {
        needs_angular_velocity = needs;
        return *this;
    }
 
    ModeConfiguration& needsAttitude(bool needs = true) {
        needs_attitude = needs;
        return *this;
    }
 
    ModeConfiguration& needsLocalAltitude(bool needs = true) {
        needs_local_altitude = needs;
        return *this;
    }
 
    ModeConfiguration& needsLocalPosition(bool needs = true) {
        needs_local_position = needs;
        return *this;
    }
 
    ModeConfiguration& needsLocalPositionRelaxed(bool needs = true) {
        needs_local_position_relaxed = needs;
        return *this;
    }
 
    ModeConfiguration& needsGlobalPosition(bool needs = true) {
        needs_global_position = needs;
        return *this;
    }
 
    ModeConfiguration& needsValidMission(bool needs = true) {
        needs_valid_mission = needs;
        return *this;
    }
 
    ModeConfiguration& needsValidHome(bool needs = true) {
        needs_valid_home = needs;
        return *this;
    }
 
    ModeConfiguration& needsManualControlInput(bool needs = true) {
        needs_manual_control_input = needs;
        return *this;
    }
 
    ModeConfiguration& canPreventArming(bool can = true) {
        can_prevent_arming = can;
        return *this;
    }
 
    ModeConfiguration& userSelectable(bool selectable = true) {
        user_selectable = selectable;
        return *this;
    }
};
 
enum class ModeFailureReason {
    None = 0,           
    NoTrackingService,  
    InvalidTarget,      
    Other,              
};
 
class FailureReporter {
   public:
    void fail(ModeFailureReason reason);
 
   private:
    friend class ModeImpl;
    explicit FailureReporter(px4_ros2::HealthAndArmingCheckReporter& reporter)
        : _reporter(reporter) {}
 
    px4_ros2::HealthAndArmingCheckReporter& _reporter;
};
 
enum class Result {
    Success = 0,
    Rejected,
    Interrupted,
    Timeout,
    Deactivated,
    ModeFailureOther = 100,
};
 
class ModeImpl;
 
struct ManualControlInput {
    float roll_input_normalized;
    float pitch_input_normalized;
    float yaw_input_normalized;
    float throttle_input_normalized;
 
    float aux1_input_normalized;
    float aux2_input_normalized;
    float aux3_input_normalized;
    float aux4_input_normalized;
    float aux5_input_normalized;
    float aux6_input_normalized;
 
    bool valid;
};
 
class Mode {
   private:
    std::shared_ptr<ModeImpl> _impl;
 
   public:
    Mode(SDK& sdk, const std::string& mode_name, const std::vector<ModalityConfig>& controls,
         ModeConfiguration config = ModeConfiguration{});
 
    ~Mode() = default;
 
    void onActivate(const std::function<void()>& callback);
 
    void onDeactivate(const std::function<void()>& callback);
 
    void onUpdateSetpoint(const std::function<Setpoint(float)>& callback);
 
    void canRun(const std::function<void(FailureReporter&)>& callback);
 
    [[nodiscard]] auterion::ManualControlInput lastManualControlInput() const;
 
    [[nodiscard]] ModeConfiguration::PX4Mode::ID id() const;
 
    void completed(Result result);
};
 
}  // namespace auterion