backend.h

//----------------------------------------------------------------------------------------------------------------------
// GRVC UAL
//----------------------------------------------------------------------------------------------------------------------
// The MIT License (MIT)
// 
// Copyright (c) 2016 GRVC University of Seville
// 
// Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated
// documentation files (the "Software"), to deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to
// permit persons to whom the Software is furnished to do so, subject to the following conditions:
// 
// The above copyright notice and this permission notice shall be included in all copies or substantial portions of the
// Software.
// 
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE
// WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS
// OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR
// OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
//----------------------------------------------------------------------------------------------------------------------
#ifndef UAV_ABSTRACTION_LAYER_BACKEND_H
#define UAV_ABSTRACTION_LAYER_BACKEND_H

#include <thread>
#include <atomic>
#include <mutex>
#include <std_msgs/Float32.h>
#include <geometry_msgs/Vector3Stamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <sensor_msgs/NavSatFix.h>
#include <nav_msgs/Odometry.h>
// #include <nav_msgs/Path.h>

namespace grvc { namespace ual {

typedef geometry_msgs::PoseStamped      Pose;
typedef geometry_msgs::PoseStamped      Waypoint;
typedef sensor_msgs::NavSatFix          WaypointGeo;
typedef geometry_msgs::TwistStamped     Velocity;
typedef nav_msgs::Odometry              Odometry;
typedef geometry_msgs::TransformStamped Transform;

class Backend {
public:

    template <typename Callable, typename ... Args>
    inline bool threadSafeCall(Callable&& _fn, Args&& ... _args) {
        // Only one thread can lock
        if (running_mutex_.try_lock()) {
            running_task_ = true;  // set running after locking
            std::bind(_fn, this, std::forward<Args>(_args)...)();
            running_mutex_.unlock();
            running_task_ = false;  // reset it after unlocking
            return true;  // Call succeeded
        } else {
            return false;  // Call failed
        }
    }

    enum State {
        UNINITIALIZED,
        LANDED_DISARMED,
        LANDED_ARMED,
        TAKING_OFF,
        FLYING_AUTO,
        FLYING_MANUAL,
        LANDING
    };

    Backend();

    virtual bool     isReady() const = 0;
    bool             isIdle();
    virtual Pose     pose() = 0;
    virtual Velocity velocity() const = 0;
    virtual Odometry odometry() const = 0;
    virtual Transform transform() const = 0;
    inline State state() { return this->state_; }

    virtual void    setPose(const geometry_msgs::PoseStamped& _pose) = 0;

    virtual void    goToWaypoint(const Waypoint& _wp) = 0;

    virtual void    goToWaypointGeo(const WaypointGeo& _wp) = 0;

    // virtual void trackPath(const Path& _path) = 0;
    virtual void    takeOff(double _height) = 0;
    virtual void    land() = 0;
    virtual void    setVelocity(const Velocity& _vel) = 0;
    virtual void    recoverFromManual() = 0;
    virtual void    setHome(bool set_z) = 0;

    void            abort(bool _freeze = true);

    virtual ~Backend() = default; // Ensure proper destructor calling for derived classes

    static Backend* createBackend();

protected:
    std::atomic<bool> abort_ = {false};

    std::atomic<bool> freeze_ = {false};

    std::mutex running_mutex_;
    std::atomic<bool> running_task_ = {false};

    // Ros spinning thread
    std::thread spin_thread_;

    std::atomic<State> state_ = {UNINITIALIZED};
};

}}  // namespace grvc::ual

#endif // UAV_ABSTRACTION_LAYER_BACKEND_H