ddrone_task_manager.h

Go to the documentation of this file.

/*
 *  Copyright 2019 Yu ZHOU
 *
 *  Created on: Jun 9, 2019
 *      Author: Yu ZHOU
 */
#ifndef DDRONE_TASK_MANAGER_H
#define DDRONE_TASK_MANAGER_H
 
#include "gpsUtils.h"
#include <common_msgs/state.h>
#include <common_msgs/target.h>
#include <diagnostic_msgs/DiagnosticArray.h>
#include <diagnostic_msgs/DiagnosticStatus.h>
#include <fstream>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Polygon.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/TransformStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/CommandHome.h>
#include <mavros_msgs/CommandTOL.h>
#include <mavros_msgs/HomePosition.h>
#include <mavros_msgs/PositionTarget.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>
#include <ros/ros.h>
#include <sensor_msgs/NavSatFix.h>
#include <sstream>
#include <std_msgs/Bool.h>
#include <std_msgs/Byte.h>
#include <std_srvs/Empty.h>
#include <std_srvs/Trigger.h>
#include <string>
#include <tf/tf.h>
#define ENGINE0 0
#define TAKEOFF 1
#define MISSION 2
#define SWARM 3
#define HOVER 4
#define LAND 5
#define POS_CONTROL 0
#define VEL_CONTROL 1
 
 
enum UavTaskState
{
    kIdle,
    kReady,
    kTakeOff,
    kHover,
    kMission,
    kSwarm,
    kLand,
};
 
enum PreFlightCheck
{
    kAttitudeRollError, 
    kAttitudePitchError,
    kMagNError, 
    kMagEError,
    kMagDError,
    kAccNError, 
    kAccEError,
    kAccDError,
    kGPSEphError, 
    kGPSEpvError,
    kGPSVelNError,
    kGPSVelEError,
    kGPSVelDError,
    kGPSTError,
    kImuError, 
    kHealth,   
};
 
enum InFlightCheck
{
 
    kDisarmed, 
    kArmed,    
    kSoftGeoViolated,
    kHardGeoViolated,
    kReturnHomeActivated, 
    kLandActivated,       
    // kFlightTerminationActivated, // 110000 Flight Terminate
    kReserved,
    kBatteryLow,
    kGPSFail,
    kAttituteOutofRange,
};
 
// errors_count4: inflight check  (By digits)
// enum InFlightCheck
// {
//   kReturnHomeActivated = 1,         // 1xx
//   kNonGpsLandingActivated = 2,     // 2xx
//   kFlightTerminationActivated = 3, //3xx
//   kSoftGeoViolence = 1,            //x1x
//   kHardGeoViolence = 2,            //x2x
//   kDisarmError = 1,            // xx1
//   kArmError = 2,               // xx2
//   kPrearmError = 3,            // xx3
//   kUnexpectedError = 4,        // xx4
// };
 
// enum PointsType
// {
//   waypoints,
//   refrence,
//   ui_selection
// };
 
class TaskManager
{
public:
    TaskManager(ros::NodeHandle &nodeHandle);
    ~TaskManager();
    // void initiswarm_pos_pubNodeHandle &nh);
    
 
    bool loadMission();
 
    void jumpWp();
    void engine0();
 
    void currentStateCallback(const common_msgs::state::ConstPtr &msg);
 
    void checkReached();
 
    void missionTimer(const ros::TimerEvent &);
 
    void missionCallback(int8_t msg);
    
    void webCmdCallback(const std_msgs::Byte::ConstPtr &msg);
    // void gcsCmdCallback(const mavros::Cmd::ConstPtr& msg);
    void uiPointsCallback(const geometry_msgs::Polygon::ConstPtr &msg);
    void uavPoseCallback(const geometry_msgs::PoseStamped::ConstPtr &msg);
    void velTgtCallback(const geometry_msgs::Point::ConstPtr &msg);
    void posTgtCallback(const geometry_msgs::Point::ConstPtr &msg);
    void velYawTgtCallback(const common_msgs::target::ConstPtr &msg);
    void posYawTgtCallback(const common_msgs::target::ConstPtr &msg);
    void approachCallback(const std_msgs::Bool::ConstPtr &msg);
    void onlineCallback(const std_msgs::Bool::ConstPtr &msg);
    void
    diagnosticsCallback(const diagnostic_msgs::DiagnosticArray::ConstPtr &msg);
    void gpsHomeCb(const mavros_msgs::HomePosition::ConstPtr &msg);
    void gpsCurCb(const sensor_msgs::NavSatFix::ConstPtr &msg);
    void uavStateCb(const mavros_msgs::State::ConstPtr &msg);
    void calculateYaw();
    void jumpWpCallback(const std_msgs::Bool::ConstPtr &msg);
    void pubGpstgt();
    void pubTarget(bool controlMode, float tgtX, float tgtY, float tgtZ);
    void pubTargetWYaw(bool controlMode, float tgtX, float tgtY, float tgtZ,
                       float yaw);
    void pubErrorState(std::int8_t val);
    void shutdownSwarm();
    void clearMission();
    void preFlightCheck(uint16_t &pre_flight_check);
    void inFlightCheck(uint16_t &in_flight_check);
    bool landCb();
    void takeoff();
 
private:
    ros::NodeHandle _nh; 
    ros::Publisher m_pos_cmd_pub; 
    ros::Publisher m_ref_pub;
    ros::Publisher m_tgt_pub;
    ros::Publisher m_engage_pub;
    ros::Publisher m_swarm_pub;
    ros::Publisher m_error_state_pub;
    ros::Publisher m_mpc_pos_init_pub;
    ros::Publisher Position_Setpoint_Pub;
    ros::Publisher MPC_Position_Initial_Pub;
    ros::Publisher swarm_pos_pub;
 
    ros::Subscriber m_current_state_sub;
    ros::Subscriber m_mission_sub;
    ros::Subscriber m_ui_points_sub;
    ros::Subscriber m_uav_pose_sub;
    ros::Subscriber m_skipCurPoint_sub;
    ros::Subscriber m_vcmd_sub;
    ros::Subscriber m_pcmd_sub;
    ros::Subscriber m_gps_home_sub;
    ros::Subscriber m_gps_cur_sub;
    ros::Subscriber m_approach_cmd_sub;
    ros::Subscriber m_diagnostics_sub;
    ros::Subscriber m_gcs_cmd_sub;
    ros::Subscriber m_online_status_sub;
    ros::Subscriber uav_state_sub;
 
    std::vector<geometry_msgs::Point> _wp_list;
    std::vector<common_msgs::state> _ref_list;
    geometry_msgs::Point _curr;
    // temp keep for switching variable
    geometry_msgs::PoseStamped m_uav_pose;
 
    geometry_msgs::PoseStamped m_uav_pose_enu;
    geometry_msgs::PoseStamped m_uav_pose_nwu;
 
    common_msgs::target m_tgt;
    double _flightHeight;
    double _relativeHeight;
 
    UavTaskState mUavTaskState;
    // PointsType mPointsFrom;
 
    bool m_jump;
    ros::Timer m_mission_timer;
    double _missionPeriod;
    int _points_id;
    int _points_source_param;
    std::string m_points_location;
    ros::ServiceClient client_engine0;
    ros::ServiceClient client_home;
    ros::ServiceClient land_client;
    ros::ServiceClient arming_client;
    ros::ServiceClient set_mode_client;
    std_msgs::Bool engage_msg;
    geometry_msgs::Polygon m_ui_points;
    mavros_msgs::HomePosition m_gps_home;
    sensor_msgs::NavSatFix m_gps_cur;
    double cos_y, sin_y, yaw_;
    double yaw_init_nwu;
    double yaw_init_enu;
    double yaw_init;
    geometry_msgs::Point pos_init;
    bool yaw_initialized;
    bool ui_points_selected;
    bool _simulation;
    std_msgs::Bool m_swarm;
    bool m_approach;
    GpsUtils gpsToEnu;
    GpsUtils gpsToEnuCur;
    bool TaskTerminateFlag;
    bool inPcmdMode;
    uint16_t last_in_flight_check;
    uint16_t last_pre_flight_check;
    bool yaw_control_;
    mavros_msgs::State uav_current_state;
 
    geometry_msgs::PoseStamped transform_orientation_enu_nwu(geometry_msgs::PoseStamped);
};
 
#endif