ddrone_task_manager.cpp

Go to the documentation of this file.

 
#include "ddrone_task_manager.h"
 
// using namespace task_manager;
TaskManager::TaskManager(ros::NodeHandle &nodeHandle) : _nh(nodeHandle)
{
    // initialize all the state
    mUavTaskState = kIdle;
    _points_source_param = 0;
    m_jump = false;
    m_swarm.data = false;
    m_approach = false;
    _points_id = 0;
    yaw_initialized = false;
    ui_points_selected = false;
    // publish way points
    m_pos_cmd_pub = _nh.advertise<geometry_msgs::Point>("/dummy/pos_cmd", 1);
    // publish target
    m_tgt_pub = _nh.advertise<common_msgs::target>("/nndp_cpp/tgt", 1);
    // publish refrence from file
    m_ref_pub =
        _nh.advertise<common_msgs::state>("/task_manager/current_state", 1);
    // trigger swarm
    m_swarm_pub = _nh.advertise<std_msgs::Bool>("/task_manager/swarm", 1);
    // send error to UI
    m_error_state_pub =
        _nh.advertise<std_msgs::Byte>("/task_manager/error_state", 1);
    //
    m_mpc_pos_init_pub = _nh.advertise<geometry_msgs::PoseStamped>(
        "/task_manager/mpc_init_position_nwu", 1);
    swarm_pos_pub = _nh.advertise<geometry_msgs::PoseStamped>(
        "/task_manager/position_nwu", 1);
    // Publish Setpoints to MAVROS
    Position_Setpoint_Pub = _nh.advertise<mavros_msgs::PositionTarget>(
        "/mavros/setpoint_raw/local", 10);
 
    // subscribe refrence
    m_current_state_sub = _nh.subscribe<common_msgs::state>(
        "/rt_ref_gen/current_state", 1, &TaskManager::currentStateCallback,
        this);
    // subscribe mission from UI
    m_mission_sub = _nh.subscribe<std_msgs::Byte>(
        "/mission_from_ui", 1, &TaskManager::webCmdCallback, this);
 
    // subscribe points from UI
    m_ui_points_sub = _nh.subscribe<geometry_msgs::Polygon>(
        "/points_from_ui", 1, &TaskManager::uiPointsCallback, this);
    m_skipCurPoint_sub = _nh.subscribe<std_msgs::Bool>(
        "/mission_skip_point", 1, &TaskManager::jumpWpCallback, this);
    // subscribe diagnostics from mavros
    m_diagnostics_sub = _nh.subscribe<diagnostic_msgs::DiagnosticArray>(
        "/diagnostics", 10, &TaskManager::diagnosticsCallback, this);
 
    // get current uav pose
    m_uav_pose_sub = _nh.subscribe<geometry_msgs::PoseStamped>(
        "/mavros/local_position/pose", 1, &TaskManager::uavPoseCallback, this);
 
    m_approach_cmd_sub = _nh.subscribe<std_msgs::Bool>(
        "/swarm/approach", 1, &TaskManager::approachCallback, this);
    m_online_status_sub = _nh.subscribe<std_msgs::Bool>(
        "/swarm/online", 1, &TaskManager::onlineCallback, this);
 
    m_gps_home_sub = _nh.subscribe<mavros_msgs::HomePosition>(
        "/mavros/home_position/home", 1, &TaskManager::gpsHomeCb, this);
    m_gps_cur_sub = _nh.subscribe<sensor_msgs::NavSatFix>(
        "/mavros/global_position/global", 1, &TaskManager::gpsCurCb, this);
    uav_state_sub = _nh.subscribe<mavros_msgs::State>(
        "/mavros/state", 10, &TaskManager::uavStateCb, this);
 
    // publish file timer
    _nh.param("missionPeriod", _missionPeriod, 0.2);
    std::cout << "missionPeriod: " << _missionPeriod << std::endl;
    m_mission_timer =
        _nh.createTimer(ros::Duration(_missionPeriod),
                        &TaskManager::missionTimer, this, false, false);
    // set waypoints and refrence file location
    _nh.param("simulation", _simulation, false);
    std::cout << "_simulation: " << _simulation << std::endl;
    _nh.param<std::string>("WP_Location", m_points_location, "/home/nvidia");
    std::cout << "WP_Location: " << m_points_location << std::endl;
    _nh.param("pointsSource", _points_source_param, 0);
    std::cout << "pointsSource: " << _points_source_param << std::endl;
    _nh.param("flightHeight", _flightHeight, 1.5);
    std::cout << "flightHeight: " << _flightHeight << std::endl;
    _nh.param("yaw_control", yaw_control_, false);
    std::cout << "yaw_control: " << yaw_control_ << std::endl;
 
    // mPointsFrom =  _points_source_param;
    client_engine0 = _nh.serviceClient<std_srvs::Trigger>("/mavros/engine0");
    client_home =
        _nh.serviceClient<mavros_msgs::CommandHome>("/mavros/cmd/set_home");
    land_client =
        _nh.serviceClient<mavros_msgs::CommandTOL>("/mavros/cmd/land");
    arming_client =
        _nh.serviceClient<mavros_msgs::CommandBool>("/mavros/cmd/arming");
    set_mode_client =
        _nh.serviceClient<mavros_msgs::SetMode>("/mavros/set_mode");
 
    TaskTerminateFlag = false;
    last_in_flight_check = 0;
    last_pre_flight_check = 0;
    inPcmdMode = false;
    ROS_INFO("Task manager is ready!");
}
 
TaskManager::~TaskManager()
{
    // ROS_INFO("Bye task manager~");
}
 
void TaskManager::uavPoseCallback(
    const geometry_msgs::PoseStamped::ConstPtr &msg)
{
    m_uav_pose = *msg;
 
    geometry_msgs::PoseStamped pos_nwu;
    pos_nwu = transform_orientation_enu_nwu(m_uav_pose);
    swarm_pos_pub.publish(pos_nwu);
    m_uav_pose_nwu = pos_nwu;
}
 
void TaskManager::pubTarget(bool controlMode, float tgtX, float tgtY,
                            float tgtZ)
{
    common_msgs::target tgt;
    tgt.controller = controlMode;
    tgt.tgt.x = tgtX;
    tgt.tgt.y = tgtY;
    tgt.tgt.z = tgtZ;
    m_tgt_pub.publish(tgt);
}
 
void TaskManager::pubTargetWYaw(bool controlMode, float tgtX, float tgtY,
                                float tgtZ, float yaw)
{
    common_msgs::target tgt;
    tgt.controller = controlMode;
    tgt.tgt.x = tgtX;
    tgt.tgt.y = tgtY;
    tgt.tgt.z = tgtZ;
    tgt.yaw_tgt = yaw;
    m_tgt_pub.publish(tgt);
}
 
void TaskManager::velYawTgtCallback(const common_msgs::target::ConstPtr &msg)
{
    common_msgs::target tgt = *msg;
    m_tgt_pub.publish(tgt);
}
 
void TaskManager::posYawTgtCallback(const common_msgs::target::ConstPtr &msg)
{
    common_msgs::target tgt = *msg;
    gpsToEnu.GeodeticToEnu(msg->tgt.x, msg->tgt.y, msg->tgt.z,
                           m_gps_home.geo.latitude, m_gps_home.geo.longitude,
                           m_gps_home.geo.altitude);
    gpsToEnuCur.GeodeticToEnu(m_gps_cur.latitude, m_gps_cur.longitude,
                              m_gps_cur.altitude, m_gps_home.geo.latitude,
                              m_gps_home.geo.longitude,
                              m_gps_home.geo.altitude);
    geometry_msgs::Point deltaP;
    deltaP.x = gpsToEnu.yNorth - gpsToEnuCur.yNorth;
    deltaP.y = -(gpsToEnu.xEast - gpsToEnuCur.xEast);
    deltaP.z = gpsToEnu.zUp - gpsToEnuCur.zUp;
 
    tgt.tgt.x = m_uav_pose.pose.position.y + deltaP.x;
    tgt.tgt.y = -m_uav_pose.pose.position.x + deltaP.y;
    tgt.tgt.z = m_uav_pose.pose.position.z + deltaP.z;
    m_tgt_pub.publish(tgt);
}
 
void TaskManager::velTgtCallback(const geometry_msgs::Point::ConstPtr &msg)
{
    pubTarget(VEL_CONTROL, msg->x, msg->y, msg->z);
    // if(inPcmdMode)
    //     inPcmdMode = false;
}
 
void TaskManager::posTgtCallback(const geometry_msgs::Point::ConstPtr &msg)
{
    gpsToEnu.GeodeticToEnu(msg->x, msg->y, msg->z, m_gps_home.geo.latitude,
                           m_gps_home.geo.longitude, m_gps_home.geo.altitude);
    gpsToEnuCur.GeodeticToEnu(m_gps_cur.latitude, m_gps_cur.longitude,
                              m_gps_cur.altitude, m_gps_home.geo.latitude,
                              m_gps_home.geo.longitude,
                              m_gps_home.geo.altitude);
    geometry_msgs::Point deltP;
    deltP.x = gpsToEnu.yNorth - gpsToEnuCur.yNorth;
    deltP.y = -(gpsToEnu.xEast - gpsToEnuCur.xEast);
    deltP.z = gpsToEnu.zUp - gpsToEnuCur.zUp;
 
    pubTarget(POS_CONTROL, m_uav_pose.pose.position.y + deltP.x,
              -m_uav_pose.pose.position.x + deltP.y,
              m_uav_pose.pose.position.z + deltP.z);
}
 
void TaskManager::shutdownSwarm()
{
    // send swarm off
    m_swarm.data = false;
    m_swarm_pub.publish(m_swarm);
    m_vcmd_sub.shutdown();
    m_pcmd_sub.shutdown();
    ROS_INFO("Shut down swarm Algo.");
}
 
void TaskManager::approachCallback(const std_msgs::Bool::ConstPtr &msg)
{
    if (msg->data && !m_approach)
    {
        m_approach = true;
        ROS_WARN("APPROACH");
        shutdownSwarm();
        ROS_INFO("Flying 15m to south.");
        // fly 15m to south
        m_tgt.tgt.x = m_uav_pose_nwu.pose.position.x - 15.0;
        m_tgt.tgt.y = m_uav_pose_nwu.pose.position.y;
        pubTarget(POS_CONTROL, m_tgt.tgt.x, m_tgt.tgt.y,
                  m_uav_pose_nwu.pose.position.z);
        m_mission_timer.start();
    }
}
 
void TaskManager::onlineCallback(const std_msgs::Bool::ConstPtr &msg)
{
    if (!msg->data)
    {
        missionCallback(5);
        ROS_INFO("Landing triggered by offline status.");
    }
}
 
void TaskManager::gpsHomeCb(const mavros_msgs::HomePosition::ConstPtr &msg)
{
    m_gps_home = *msg;
}
 
void TaskManager::gpsCurCb(const sensor_msgs::NavSatFix::ConstPtr &msg)
{
    m_gps_cur = *msg;
}
void TaskManager::uavStateCb(const mavros_msgs::State::ConstPtr &msg)
{
    uav_current_state = *msg;
}
 
// void TaskManager::gcsCmdCallback(const mavros::Cmd::ConstPtr& msg){
 
//     int8_t msg_from_qt_gcs;
//     msg_from_qt_gcs = msg->param1;
//     ROS_INFO("Got command from QT GCS");
//     missionCallback(msg_from_qt_gcs);
 
// }
 
void TaskManager::webCmdCallback(const std_msgs::Byte::ConstPtr &msg)
{
    ROS_INFO("Got command from web GCS");
    missionCallback(msg->data);
}
 
void TaskManager::missionCallback(int8_t msg)
{
    switch (msg)
    {
        // No need for engine0 in 1.8 firmware
 
        // case ENGINE0:
        //     ROS_INFO("ENGINE0 is sent!");
        //     // calculateYaw();
        //     engine0();
        //     break;
 
    case TAKEOFF:
        takeoff();
        mUavTaskState = kTakeOff;
        ROS_WARN("TAKEOFF");
        ROS_INFO("UAV is taking off!");
 
        break;
 
    case MISSION:
        if (mUavTaskState == kTakeOff)
        {
            if (loadMission())
            {
                ROS_INFO("Mission starts!");
                mUavTaskState = kMission;
                m_mission_timer.start();
                ROS_WARN("MISSION");
            }
            else
            {
                ROS_ERROR("Mission Load failed!");
            }
        }
        else
        {
            ROS_ERROR("Please send takeoff command first!");
        }
        break;
 
    case SWARM:
        if (mUavTaskState == kTakeOff || mUavTaskState == kHover)
        {
            clearMission();
            ROS_INFO_STREAM("Switch from " << mUavTaskState << "to SWARM");
            ROS_WARN("SWARM");
            // start swarm
            m_swarm.data = true;
            m_swarm_pub.publish(m_swarm);
            mUavTaskState == kSwarm;
            // get cmd from swarm algo
            if (yaw_control_)
            {
                m_vcmd_sub = _nh.subscribe<common_msgs::target>(
                    "/velTgtFromSwarm", 10, &TaskManager::velYawTgtCallback,
                    this);
                m_pcmd_sub = _nh.subscribe<common_msgs::target>(
                    "/posTgtFromSwarm", 10, &TaskManager::posYawTgtCallback,
                    this);
            }
            else
            {
                m_vcmd_sub = _nh.subscribe<geometry_msgs::Point>(
                    "/velTgtFromSwarm", 10, &TaskManager::velTgtCallback, this);
                m_pcmd_sub = _nh.subscribe<geometry_msgs::Point>(
                    "/posTgtFromSwarm", 10, &TaskManager::posTgtCallback, this);
            }
        }
        else
        {
            ROS_ERROR("Please send takeoff command first!");
        }
        break;
 
    case HOVER:
        ROS_WARN("HOVER");
        mUavTaskState = kHover;
        // take over control
        shutdownSwarm();
        clearMission();
        pubTargetWYaw(POS_CONTROL, m_uav_pose.pose.position.y,
                      -m_uav_pose.pose.position.x, m_uav_pose.pose.position.z,
                      yaw_);
        break;
 
    case LAND:
        mUavTaskState = kHover;
        shutdownSwarm();
        if (landCb())
        {
            mUavTaskState = kLand;
            ROS_WARN("DDrone: LAND");
        }
        break;
 
    default:
        break;
    }
}
 
void TaskManager::takeoff()
{
    ROS_INFO("Initialize Setpoints...");
    calculateYaw();
    ros::Rate rate(20.0);
    ros::Time last_request = ros::Time::now();
    // Make Sure FCU is connected, wait for 5s if not connected.
    while (ros::ok() && !uav_current_state.connected)
    {
        ros::spinOnce();
        rate.sleep();
        if (ros::Time::now() - last_request > ros::Duration(5.0))
        {
            ROS_ERROR("FCU not connect for 5s. Stop takeoff procedure.");
            return;
        }
    }
    ROS_INFO("FCU Connected");
    // send a few setpoints before starting
    mavros_msgs::PositionTarget init_sp;
    init_sp.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED;  // To be converted to NED in setpoint_raw plugin
    init_sp.position.x = m_uav_pose.pose.position.x;
    init_sp.position.y = m_uav_pose.pose.position.y;
    init_sp.position.z = m_uav_pose.pose.position.z;
    init_sp.velocity.x = 0;
    init_sp.velocity.y = 0;
    init_sp.velocity.z = 0;
    init_sp.acceleration_or_force.x = 0;
    init_sp.acceleration_or_force.y = 0;
    init_sp.acceleration_or_force.z = 0;
    init_sp.yaw = yaw_init;
 
    // set the inital position of mission start;
    pos_init.x = m_uav_pose.pose.position.y;
    pos_init.y = -m_uav_pose.pose.position.x;
    pos_init.z = m_uav_pose.pose.position.z;
 
    // set MPC Initial
    // MPC Initial as NWU
    geometry_msgs::PoseStamped mpc_init_pos;
    mpc_init_pos = transform_orientation_enu_nwu(m_uav_pose);
    m_mpc_pos_init_pub.publish(mpc_init_pos);
 
    for (int i = 10; ros::ok() && i > 0; --i)
    {
        // Position_Setpoint_Pub.publish(init_sp);
        ros::spinOnce();
        rate.sleep();
    }
    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";
 
    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;
 
    bool is_mode_ready = false;
    bool is_armed = false;
    last_request = ros::Time::now();
    // Set UAV mode to offboard and arm the vehicle
    if (_simulation)
    {
        is_mode_ready = true;
        is_armed = true;
    }
 
    while (!(is_mode_ready && is_armed))
    {
        if (uav_current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(1.0)))
        {
            ROS_INFO("try set offboard");
            if (set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.mode_sent)
            {
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        }
        else
        {
            if (!uav_current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(1.0)))
            {
                ROS_INFO("try arm");
                if (arming_client.call(arm_cmd) && arm_cmd.response.success)
                {
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
            is_mode_ready = (uav_current_state.mode == "OFFBOARD");
            is_armed = uav_current_state.armed;
        }
        Position_Setpoint_Pub.publish(init_sp);
        ros::spinOnce();
        rate.sleep();
    }
    _relativeHeight = mpc_init_pos.pose.position.z + _flightHeight;
 
    pubTargetWYaw(POS_CONTROL, mpc_init_pos.pose.position.x,
                  mpc_init_pos.pose.position.y, _relativeHeight, yaw_init_nwu);
}
 
bool TaskManager::landCb()
{
    // TODO:
    // 2020-Mar-05： Land cmd request check
    // 2019-Sep-25, only land at current location is supported by flight
    // controller. msg data is only used for data package, all data will be
    // ignored in flight control ROS_INFO("trying to land");
    mavros_msgs::CommandTOL land_cmd;
    land_cmd.request.yaw = 0;
    land_cmd.request.latitude = 0;
    land_cmd.request.longitude = 0;
    land_cmd.request.altitude = 0;
    if (_simulation == 0)
    {
        land_client.call(land_cmd);
        if (land_cmd.response.success)
        {
            ROS_INFO("land command succeed");
            return true;
        }
        else
        {
            ROS_ERROR("LAND Command Failed!");
            return false;
        }
    }
    else
    {
        // in Simulation, using position targte to achieve land
        pubTargetWYaw(POS_CONTROL, m_uav_pose.pose.position.x,
                      m_uav_pose.pose.position.y, -10.0, yaw_);
        return true;
    }
    return false;
}
 
void TaskManager::engine0()
{
    // TODO:
    //      1. move takeoff to set-home
    //      2. remove engine0 func
    std_srvs::Trigger srv;
    if (client_engine0.call(srv))
    {
        if (srv.response.success || _simulation)
        {
            ROS_WARN("ENGINE0");
            mUavTaskState = kReady; // set it here cos the the set home may fail
                                    // in inddor environment
        }
    }
    else
    {
        ROS_ERROR("Failed to activate engine0");
        return;
    }
 
    // call set home in mavros
    mavros_msgs::CommandHome mavros_set_home_srv;
    mavros_set_home_srv.request.current_gps = true;
    if (client_home.call(mavros_set_home_srv))
    {
        if (mavros_set_home_srv.response.success)
        {
            std::string info_string(" Set home succeed!");
            ROS_INFO_STREAM(info_string);
        }
        else
        {
            std::string info_string(" Set home failed!");
            ROS_INFO_STREAM(info_string);
        }
    }
    else
    {
        std::string info_string(" Failed to call service "
                                "mavros_set_home!");
        ROS_INFO_STREAM(info_string);
    }
}
 
// TODO:
// Do we need this function anymore?
void TaskManager::uiPointsCallback(const geometry_msgs::Polygon::ConstPtr &msg)
{
    m_ui_points = *msg;
    ui_points_selected = true;
    ROS_INFO("Got polygon with %d points", int(m_ui_points.points.size()));
}
 
void TaskManager::jumpWpCallback(const std_msgs::Bool::ConstPtr &msg)
{
    m_jump = msg->data;
    ROS_INFO("Skip current point!");
}
 
void TaskManager::calculateYaw()
{
    tf::Quaternion q(
        m_uav_pose.pose.orientation.x, m_uav_pose.pose.orientation.y,
        m_uav_pose.pose.orientation.z, m_uav_pose.pose.orientation.w);
    //std::cout << "=> m_uav_pose Quaternion ={" << m_uav_pose.pose.orientation.x << ", " << m_uav_pose.pose.orientation.y
    //          << ", " << m_uav_pose.pose.orientation.z << ", " << m_uav_pose.pose.orientation.w << "}\n";
    tf::Matrix3x3 m(q);
    double roll, pitch, yaw;
    m.getRPY(roll, pitch, yaw);
    cos_y = cos(yaw);
    sin_y = sin(yaw);
    ROS_INFO("Initialized yaw value %f: ", yaw);
    yaw_initialized = true;
    yaw_init = yaw;
    yaw_init_enu = yaw;
 
    // Calculate yaw in NWU frame for take-off cmd
    tf::Matrix3x3 R_enu2nwu;
    R_enu2nwu.setRPY(0.0, 0.0, -3.1415926 / 2);
    tf::Matrix3x3 R_nwu = R_enu2nwu * m;
    double yaw_nwu;
    R_nwu.getRPY(roll, pitch, yaw_nwu);
    yaw_init_nwu = yaw_nwu;
}
 
void TaskManager::pubGpstgt()
{
    gpsToEnu.GeodeticToEnu(_wp_list[_points_id].x, _wp_list[_points_id].y,
                           _wp_list[_points_id].z, m_gps_home.geo.latitude,
                           m_gps_home.geo.longitude, m_gps_home.geo.altitude);
 
    m_tgt.controller = POS_CONTROL;
    m_tgt.tgt.x = gpsToEnu.yNorth;
    m_tgt.tgt.y = -gpsToEnu.xEast;
    // m_tgt.tgt.z = gpsToEnu.zUp;
    m_tgt.tgt.z = _relativeHeight;
    m_tgt_pub.publish(m_tgt);
}
 
void TaskManager::clearMission()
{
    _wp_list.clear();
    _ref_list.clear();
    _points_id = 0;
    m_tgt.tgt.x = m_uav_pose_nwu.pose.position.x;
    m_tgt.tgt.y = m_uav_pose_nwu.pose.position.y;
    ROS_INFO("Clear Mission");
}
 
bool TaskManager::loadMission()
{
    // //Read in the file for waypoints
    std::ifstream infile(m_points_location);
    std::string line;
    clearMission();
    bool succeed = false;
    switch (_points_source_param)
    {
    case 0:
        ROS_INFO("Loading from local waypoints file");
 
        while (std::getline(infile, line))
        {
            std::istringstream iss(line);
            double _1, _2, _3;
            if (!(iss >> _1 >> _2 >> _3))
            {
                ROS_ERROR("The data size is not correct!");
                return false;
                break;
            } // error
            geometry_msgs::Point p;
            p.x = _1;
            p.y = _2;
            p.z = _3;
            _wp_list.push_back(p);
        }
        if (_wp_list.size() > 0)
        {
            ROS_INFO("Goto wp: 0");
            // m_pos_cmd_pub.publish(_wp_list[_points_id]);
            // pubGpstgt();
            pubTarget(POS_CONTROL, _wp_list[_points_id].x,
                      _wp_list[_points_id].y, _wp_list[_points_id].z);
            // Update m_tgt.tgt (NWU) for checking target reach
            m_tgt.tgt.x = _wp_list[_points_id].x;
            m_tgt.tgt.y = _wp_list[_points_id].y;
            m_tgt.tgt.z = _wp_list[_points_id].z;
            succeed = true;
            std::cout << "   _wp_list[" << _points_id << "] = " << _wp_list[_points_id].x << " " << _wp_list[_points_id].y
                      << " " << _wp_list[_points_id].z << std::endl;
            ROS_INFO("Points have been loaded!");
        }
        break;
 
        // case 1:
        //     ROS_INFO("Loading from refrence waypoints file");
        //     while (std::getline(infile, line))
        //     {
        //         std::istringstream iss(line);
        //         double _1, _2, _3, _4, _5, _6, _7, _8, _9;
        //         if (!(iss >> _1 >> _2 >> _3>> _4 >> _5 >> _6>> _7 >> _8 >>
        //         _9))
        //         {
        //             ROS_ERROR("The data size is not correct!");
        //             return false;
        //             break;
        //         } // error
        //         common_msgs::state p;
        //         p.pos.x = _1;
        //         p.pos.y = _2;
        //         p.pos.z = _3;
        //         p.vel.x = _4;
        //         p.vel.y = _5;
        //         p.vel.z = _6;
        //         p.acc.x = _7;
        //         p.acc.y = _8;
        //         p.acc.z = _9;
        //         _ref_list.push_back(p);
 
        //     }
        //     ROS_INFO("Points have been loaded!");
        //     succeed = true;
        //     break;
        // case 2:
 
        //     if(ui_points_selected)
        //     {
        //         ROS_INFO("Loading from ui selected waypoints");
        //         int p_i = 0;
        //         while (p_i<m_ui_points.points.size())
        //         {
        //             geometry_msgs::Point p;
        //             p.x = m_ui_points.points[p_i].x * cos_y -
        //             m_ui_points.points[p_i].y * sin_y; p.y =
        //             m_ui_points.points[p_i].x * sin_y +
        //             m_ui_points.points[p_i].y * cos_y; p.z =
        //             m_ui_points.points[p_i].z; _wp_list.push_back(p); p_i +=
        //             1;
 
        //         }
 
        //         if (_wp_list.size()>0)
        //         {
        //             ROS_INFO("Goto wp: 0");
        //             m_pos_cmd_pub.publish(_wp_list[_points_id]);
        //             succeed = true;
        //             ROS_INFO("Points have been loaded!");
        //         }
        //     }
        //     else{
        //         ROS_ERROR("Please select points from the UI first!");
        //         succeed = false;
        //     }
        //     break;
 
    case 3:
        ROS_INFO("Loading from velocity points from file");
        while (std::getline(infile, line))
        {
            std::istringstream iss(line);
            double _1, _2, _3, _4;
            if (!(iss >> _1 >> _2 >> _3 >> _4))
            {
                ROS_ERROR("The data size is not correct!");
                return false;
                break;
            } // error
            geometry_msgs::Point p;
            p.x = _2;
            p.y = _3;
            p.z = _4;
            _wp_list.push_back(p);
        }
        ROS_INFO("Points have been loaded!");
        succeed = true;
        break;
 
    default:
        ROS_ERROR("Please choose the correct mission points formation!");
        succeed = false;
        break;
    }
    return succeed;
}
 
void TaskManager::currentStateCallback(const common_msgs::state::ConstPtr &msg)
{
    // // update the current uav position
    _curr.x = msg->pos.x;
    _curr.y = msg->pos.y;
    _curr.z = msg->pos.z;
 
    mavros_msgs::PositionTarget pos_sp;
    // MAVROS pos_sp in ENU frame, msg is in NWU frame
    pos_sp.coordinate_frame = mavros_msgs::PositionTarget::FRAME_LOCAL_NED; // To be converted to NED in setpoint_raw plugin
    pos_sp.position.x = -msg->pos.y;
    pos_sp.position.y = msg->pos.x;
    pos_sp.position.z = msg->pos.z;
    pos_sp.velocity.x = -msg->vel.y;
    pos_sp.velocity.y = msg->vel.x;
    pos_sp.velocity.z = msg->vel.z;
    pos_sp.acceleration_or_force.x = -msg->acc.y;
    pos_sp.acceleration_or_force.y = msg->acc.x;
    pos_sp.acceleration_or_force.z = msg->acc.z;
    double yaw_sp_enu = msg->yaw.x + 1.5708;
    // ensure yaw is in range of (-pi, pi)
    if (yaw_sp_enu > 3.1415926)
    {
        yaw_sp_enu -= 3.1415926 * 2;
    }
    else if (yaw_sp_enu < -3.1415926)
    {
        yaw_sp_enu += 3.1415926 * 2;
    }
    pos_sp.yaw = yaw_sp_enu;
    // typemask 32768 will request rpt controller in px4
    pos_sp.type_mask = 32768;
    Position_Setpoint_Pub.publish(pos_sp);
}
 
void TaskManager::missionTimer(const ros::TimerEvent &)
{
    switch (_points_source_param)
    {
    case 0:
 
    case 2:
        checkReached();
        break;
    case 1:
        if (_points_id < _ref_list.size() - 1)
        {
            m_ref_pub.publish(_ref_list[_points_id]);
            std::cout << "Goto next ref: " << _points_id << std::endl;
            std::cout << _ref_list[_points_id].pos.x << " "
                      << _ref_list[_points_id].pos.y << " "
                      << _ref_list[_points_id].pos.z << std::endl;
            _points_id++;
        }
 
    case 3: // vcmd
        if (_points_id < _wp_list.size() - 1)
        {
            pubTarget(VEL_CONTROL, _wp_list[_points_id].x,
                      _wp_list[_points_id].y, _wp_list[_points_id].z);
            std::cout << "Goto next ref: " << _points_id << std::endl;
            std::cout << _wp_list[_points_id].x << " " << _wp_list[_points_id].y
                      << " " << _wp_list[_points_id].z << std::endl;
            _points_id++;
        }
        break;
    default:
        ROS_ERROR("Please choose the correct mission points formation!");
        break;
    }
}
 
void TaskManager::checkReached()
{
    // calculate the distance between the current position to the wp (NWU)
    geometry_msgs::Point dist;
    dist.x = m_uav_pose_nwu.pose.position.x - m_tgt.tgt.x;
    dist.y = m_uav_pose_nwu.pose.position.y - m_tgt.tgt.y;
    // dist.z = m_uav_pose.pose_nwu.position.z - m_tgt.tgt.z;
 
    // if dist is small enough
    if (dist.x * dist.x + dist.y * dist.y < 2.0 || m_jump)
    {
        // land when approach target reached
        if (m_approach)
        {
            m_mission_timer.stop();
            if (landCb())
            {
                ROS_INFO("UAV is landing!");
            }
        }
        else if (_points_id < _wp_list.size() - 1)
        {
            _points_id++;
            // m_pos_cmd_pub.publish(_wp_list[_points_id]);
            // pubGpstgt();
            pubTarget(POS_CONTROL, _wp_list[_points_id].x,
                      _wp_list[_points_id].y, _wp_list[_points_id].z);
            // Update m_tgt.tgt (NWU) for checking target reach
            m_tgt.tgt.x = _wp_list[_points_id].x;
            m_tgt.tgt.y = _wp_list[_points_id].y;
            m_tgt.tgt.z = _wp_list[_points_id].z;
            std::cout << "Goto wp: " << _points_id << std::endl;
            std::cout << "   _wp_list[" << _points_id << "] = " << _wp_list[_points_id].x << " " << _wp_list[_points_id].y
                      << " " << _wp_list[_points_id].z << std::endl;
        }
        m_jump = false;
    }
}
 
void TaskManager::pubErrorState(std::int8_t val)
{
    std_msgs::Byte uav_error;
    uav_error.data = val;
    m_error_state_pub.publish(uav_error);
}
 
// TODO:
//   revise or remove
void TaskManager::preFlightCheck(uint16_t &pre_flight_check)
{
    for (int position = 0; position < 16; position++)
    {
        bool is_set = (pre_flight_check & (1 << position)) >> position;
        if (is_set)
        {
            pubErrorState(position); // send error to GCS
            switch (position)
            {
            case kAttitudeRollError:
                ROS_ERROR("Attitude Roll Error");
                break;
            case kAttitudePitchError:
                ROS_ERROR("Attitude Pitch Error");
                break;
            case kMagNError:
                ROS_ERROR("Attitude Mag North Error");
                break;
            case kMagEError:
                ROS_ERROR("Attitude Mag East Error");
                break;
            case kMagDError:
                ROS_ERROR("Attitude Mag Z Error");
                break;
            case kAccNError:
                ROS_ERROR("Attitude Acc North Error");
                break;
            case kAccEError:
                ROS_ERROR("Attitude Acc East Error");
                break;
            case kAccDError:
                ROS_ERROR("Attitude Acc Z Error");
                break;
            case kGPSEphError:
                ROS_ERROR("Attitude GPS EPH Error");
                break;
            case kGPSEpvError:
                ROS_ERROR("Attitude GPS EPV Error");
                break;
            case kGPSVelNError:
                ROS_ERROR("Attitude GPS Velocity North Error");
                break;
            case kGPSVelEError:
                ROS_ERROR("Attitude GPS Velocity East Error");
                break;
            case kGPSVelDError:
                ROS_ERROR("Attitude GPS Velocity Z Error");
                break;
            case kGPSTError:
                ROS_ERROR("Attitude GPS Time Error");
                break;
            case kImuError:
                ROS_ERROR("Attitude IMU stuck Error");
                break;
            case kHealth:
                ROS_INFO("All Good!");
                break;
            }
        }
    }
}
 
// TODO:
//  Revise or remove
void TaskManager::inFlightCheck(uint16_t &in_flight_check)
{
    // In flight Error for GCS
    // "Arm Error",     // 16
    // "return home",        // 17
    // "Auto-Land",     // 18
    // "IFC: Flight termination mode triggered.", // 19
    // "IFC: Soft Geo-fence Violated.", // 20
    // "IFC: Hard Geo-fence Violated.", // 21
    // "IFC: Battery Low!",  // 22
    // "IFC: GPS Failed.", // 23
    // "IFC:Large Attitude!"]; //24
 
    uint16_t error_state = 0;
    for (int position = 0; position < 10; position++)
    {
        // handle info stored in bits 1,2, 4 and 6
        if (position == 0 || position == 4)
        {
            continue;
        }
        else if (position == 1)
        {
            error_state = (in_flight_check & 0x03); // get last 2 bits
            switch (int(error_state))
            {
            case 0:
                ROS_ERROR("Arm Error");
                pubErrorState(16);
                break;
            case 1:
                ROS_WARN("Disarmed");
                break;
            case 2:
                ROS_WARN("Armed");
                break;
            case 3:
                ROS_WARN("Pre-Armed");
                break;
            }
            continue; // exit current loop
        }
        else if (position == 5)
        {
            error_state = (in_flight_check & 0x30) >> 4; // get 5th and 6th bits
            switch (int(error_state))
            {
            case 0:
                break;
            case 1:
                // ROS_ERROR("RTL");
                std::cout << ros::Time::now() << " IFC: RTL "
                          << "\n";
                pubErrorState(17);
                TaskTerminateFlag = true;
                break;
            case 2:
                // ROS_ERROR("Auto-Land");
                std::cout << ros::Time::now() << " IFC:Auto-Land "
                          << "\n";
                pubErrorState(18);
                TaskTerminateFlag = true;
                break;
            case 3:
                ROS_ERROR("IFC: Flight termination mode triggered.");
                pubErrorState(19);
                TaskTerminateFlag = true;
                break;
            }
            continue; // exit current loop
        }
        // handle others
        bool is_set = (in_flight_check & (1 << position)) >> position;
        if (is_set)
        {
            switch (position)
            {
            case kSoftGeoViolated:
                ROS_ERROR("IFC: Soft Geo-fence Violated.");
                pubErrorState(20);
                break;
            case kHardGeoViolated:
                ROS_ERROR("IFC: Hard Geo-fence Violated.");
                pubErrorState(21);
                break;
            case kBatteryLow:
                ROS_ERROR("IFC: Battery Low!");
                pubErrorState(22);
                break;
            case kGPSFail:
                ROS_ERROR("IFC: GPS Failed.");
                pubErrorState(23);
                TaskTerminateFlag = true;
                break;
            case kAttituteOutofRange:
                ROS_ERROR("IFC:Large Attitude!");
                pubErrorState(24);
                break;
            }
        }
    }
}
 
void TaskManager::diagnosticsCallback(
    const diagnostic_msgs::DiagnosticArray::ConstPtr &msg)
{
 
    uint16_t pre_flight_check = 0;
    uint16_t in_flight_check = 0;
 
    for (int i = 0; i < msg->status.size(); i++)
    {
        if (msg->status[i].name == "mavros: System")
        {
            for (int val = 0; val < msg->status[i].values.size(); val++)
            {
                if (msg->status[i].values[val].key == "Errors count #3")
                {
                    pre_flight_check =
                        std::stoi(msg->status[i].values[val].value);
                    // ROS_INFO_STREAM("pre_flight_check: " <<
                    // pre_flight_check);
                    if (pre_flight_check &&
                        (pre_flight_check != last_pre_flight_check))
                    {
                        last_pre_flight_check = pre_flight_check;
                        preFlightCheck(pre_flight_check);
                    }
                }
                if (msg->status[i].values[val].key == "Errors count #4")
                {
                    in_flight_check =
                        std::stoi(msg->status[i].values[val].value);
                    // ROS_INFO_STREAM("in_flight_check: " << in_flight_check);
 
                    if (in_flight_check &&
                        (in_flight_check != last_in_flight_check))
                    {
                        last_in_flight_check = in_flight_check;
                        if (int(in_flight_check) <= 3)
                        {
                            TaskTerminateFlag = false;
                        }
                        inFlightCheck(in_flight_check);
                    }
 
                    if ((mUavTaskState != kIdle) && (_simulation == 0) &&
                        TaskTerminateFlag)
                    {
                        ROS_ERROR("Task Termination is Activated!");
                        m_swarm.data = false;
                        m_swarm_pub.publish(m_swarm);
                    }
                }
            }
            break;
        }
    }
}
 
geometry_msgs::PoseStamped
TaskManager::transform_orientation_enu_nwu(geometry_msgs::PoseStamped msg)
{
    tf::Matrix3x3 R_enu2nwu;
    R_enu2nwu.setRPY(0.0, 0.0, -3.1415926 / 2);
    // convert position
    // testing:
    tf::Vector3 pos(msg.pose.position.x, msg.pose.position.y,
                    msg.pose.position.z);
    tf::Vector3 pos_nwu = R_enu2nwu * pos;
    // convert orientation
    tf::Quaternion quat(msg.pose.orientation.x, msg.pose.orientation.y,
                        msg.pose.orientation.z, msg.pose.orientation.w);
    tf::Matrix3x3 R(quat);
    tf::Matrix3x3 R_nwu = R_enu2nwu * R;
    tf::Quaternion quat_nwu;
    R_nwu.getRotation(quat_nwu);
 
    geometry_msgs::PoseStamped msg_nwu;
 
    // data in NWU
    msg_nwu.pose.position.x = pos_nwu[0];
    msg_nwu.pose.position.y = pos_nwu[1];
    msg_nwu.pose.position.z = pos_nwu[2];
    msg_nwu.pose.orientation.x = quat_nwu[0];
    msg_nwu.pose.orientation.y = quat_nwu[1];
    msg_nwu.pose.orientation.z = quat_nwu[2];
    msg_nwu.pose.orientation.w = quat_nwu[3];
 
    return msg_nwu;
}