backend_light.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_LIGHT_H
#define UAV_ABSTRACTION_LAYER_BACKEND_LIGHT_H

#include <thread>
#include <Eigen/Core>

#include <uav_abstraction_layer/backend.h>
#include <ros/ros.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <tf2_ros/static_transform_broadcaster.h>

namespace grvc { namespace ual {

class BackendLight : public Backend {

public:
    BackendLight();

    bool             isReady() const override;
    virtual Pose     pose() override;
    virtual Velocity velocity() const override;
    virtual Odometry odometry() const override;
    virtual Transform transform() const override;

    void    setPose(const geometry_msgs::PoseStamped& _pose) override;

    void    goToWaypoint(const Waypoint& _wp) override;

    void    goToWaypointGeo(const WaypointGeo& _wp) override;
    
    void    takeOff(double _height) override;
    void    land() override;
    void    setVelocity(const Velocity& _vel) override;
    void    recoverFromManual() override {}
    void    setHome(bool set_z) override {}

private:
    void initHomeFrame();
    bool referencePoseReached() const;
    void move();
    Velocity calcVel(Pose _target_pose);

    geometry_msgs::PoseStamped home_pose_;
    geometry_msgs::PoseStamped ref_pose_;
    geometry_msgs::PoseStamped cur_pose_;
    geometry_msgs::PoseStamped cur_pose_noisy_;
    geometry_msgs::PoseStamped gazebo_pose_;
    geometry_msgs::TwistStamped ref_vel_;
    geometry_msgs::TwistStamped cur_vel_;

    //Gazebo animated link
    std::string link_name_;
    ros::Publisher link_state_publisher_;

    //Noise
    std::default_random_engine generator_;
    std::normal_distribution<double>* distribution_;

    //Control
    bool flying_ = false;
    bool control_in_vel_ = false;
    float max_h_vel_;
    float max_v_vel_;
    float max_yaw_vel_;
    float max_pose_error_;
    float max_orient_error_;

    int robot_id_;
    std::string pose_frame_id_;
    std::string uav_home_frame_id_;
    std::string uav_frame_id_;
    tf2_ros::StaticTransformBroadcaster * static_tf_broadcaster_;
    std::map <std::string, geometry_msgs::TransformStamped> cached_transforms_;
    ros::Time last_command_time_;

    std::thread offboard_thread_;
};

}}  // namespace grvc::ual

#endif // UAV_ABSTRACTION_LAYER_BACKEND_LIGHT_H