Script to control quadrotor through checkpoints

gistfile1.txt

/* 
UNIT TESTS FOR AIRSIM DRONE CONTROL API CALLS
 */
#include "common/common_utils/StrictMode.hpp"
STRICT_MODE_OFF
#ifndef RPCLIB_MSGPACK
#define RPCLIB_MSGPACK clmdep_msgpack
#endif // !RPCLIB_MSGPACK
#include "rpc/rpc_error.h"
STRICT_MODE_ON

#include "vehicles/multirotor/api/MultirotorRpcLibClient.hpp"
#include "common/common_utils/FileSystem.hpp"
#include <iostream>
#include <chrono>

int main()
{
    using namespace msr::airlib;

    msr::airlib::MultirotorRpcLibClient client;
    typedef ImageCaptureBase::ImageRequest ImageRequest;
    typedef ImageCaptureBase::ImageResponse ImageResponse;
    typedef ImageCaptureBase::ImageType ImageType;
    typedef common_utils::FileSystem FileSystem;

    float client_timeout_sec_;
    float client_lookahead_;
    float client_adaptive_lookahead_ ;
    float client_margin_;
    Eigen::MatrixXd actions_; actions_.resize(3, 7);
    actions_ << 1.15, 2.12, 2.77, 3.0, 2.77, 2.12, 1.15,
            -2.772, -2.12, -1.15, 0, 1.15, 2.12, 2.772, 
            -1.18, -0.7854, -0.393, 0, 0.393, 0.7854, 1.18;
    double yaw_rate_ = 2.36;
    double yaw_duration_ = 0.;
    try {
        std::string drone_name = "Drone1";
        
        client.confirmConnection();
        client.enableApiControl(true, drone_name);    
        client.armDisarm(true);

        client_timeout_sec_ = (3.4028235E38F);
        msr::airlib::YawMode client_default_yaw_mode_ = msr::airlib::YawMode();
        client_lookahead_ = (-1.0F);
        client_adaptive_lookahead_ = (1.0F);
        client_margin_ = (5.0F);
        DrivetrainType client_drivetrain_ = msr::airlib::DrivetrainType::MaxDegreeOfFreedom;


        // std::cout << "Press Enter to takeoff" << std::endl;
        // std::cin.get();
        float takeoff_timeout = 5;
        // client.takeoffAsync(takeoff_timeout, drone_name)->waitOnLastTask();
        client.moveToZAsync(-40, 5., takeoff_timeout, client_default_yaw_mode_, client_lookahead_, client_adaptive_lookahead_, drone_name)->waitOnLastTask();

        // switch to explicit hover mode so that this is the fall back when
        // move* commands are finished.
        client.hoverAsync(drone_name)->waitOnLastTask();

        // std::cout << "Press Enter to fly in a 10m box pattern at 3 m/s velocity" << std::endl;
        // std::cin.get();
        // moveByVelocityZ is an offboard operation, so we need to set offboard mode.
        // client.enableApiControl(true,drone_name);

        auto position = client.getMultirotorState(drone_name).getPosition();
        float z = position.z(); // current position (NED coordinate system).
        constexpr float speed = 3.0f;
        constexpr float size = 10.0f;
        constexpr float duration = size / speed;
        DrivetrainType drivetrain = DrivetrainType::ForwardOnly;
        // DrivetrainType drivetrain = DrivetrainType::MaxDegreeOfFreedom;
        YawMode yaw_mode(true, 0); //is_rate, yaw_or_rate

        // test1
/*         std::cout << "moveByVelocityZ(" << speed << ", 0, " << z << "," << duration << ")" << std::endl;
        client.moveByVelocityZAsync(speed, 0, z, duration, drivetrain, yaw_mode, drone_name);
        std::this_thread::sleep_for(std::chrono::duration<double>(duration));

        std::cout << "moveByVelocityZ(0, " << speed << "," << z << "," << duration << ")" << std::endl;
        client.moveByVelocityZAsync(0, speed, z, duration, drivetrain, yaw_mode, drone_name);
        std::this_thread::sleep_for(std::chrono::duration<double>(duration));

        std::cout << "moveByVelocityZ(" << -speed << ", 0, " << z << "," << duration << ")" << std::endl;
        client.moveByVelocityZAsync(-speed, 0, z, duration, drivetrain, yaw_mode, drone_name);
        std::this_thread::sleep_for(std::chrono::duration<double>(duration));

        std::cout << "moveByVelocityZ(0, " << -speed << "," << z << "," << duration << ")" << std::endl;
        client.moveByVelocityZAsync(0, -speed, z, duration, drivetrain, yaw_mode, drone_name);
         */std::this_thread::sleep_for(std::chrono::duration<double>(duration));

        client.hoverAsync(drone_name)->waitOnLastTask();
        std::vector<int> indices = {0,6,4,5,1,3,2};

        // angle rate pid gains
        const float kP = 100.0f;
        const float kI = 0.01f;
        const float kD = 1.0f;
        msr::airlib::vector<float> kpvec = {kP, kP, kP, 1.0f};
        msr::airlib::vector<float> kivec = {kI, kI, kI, 1.0f};
        msr::airlib::vector<float> kdvec = {kD, kD, kD, 1.0f};
        // AngleLevelControllerGains()

        // our flight pattern
        for (int i = 0; i < indices.size(); i++)
        {
            int ind;
            std::cout << "Enter control action index" << std::endl;
            // std::cin >> ind;
            ind = indices.at(i);
            if (ind == -1)
            {
                break;
            }
            
            Eigen::Matrix<double, 3, 1> selected_action_displacement = actions_.block(0, ind, 3, 1);
            
            float yaw_rate = selected_action_displacement(2) / 1.0;
            if (yaw_rate < 0.)
            {
                yaw_rate = -1.*yaw_rate_;
                yaw_duration_ = abs(selected_action_displacement(2) /yaw_rate_);
            }
            else if (yaw_rate > 0.)
            {
                yaw_rate = yaw_rate_;
                yaw_duration_ = abs(selected_action_displacement(2) /yaw_rate_);
            }
            else
            {
            yaw_rate=0.;
            yaw_duration_ = 0.;
            }

            // bool is_rate = false;
            // double yaw_or_rate = yaw_rate;

            bool is_rate = false;
            double yaw_or_rate = selected_action_displacement(2);

            // bool is_rate = true;
            // double yaw_or_rate = selected_action_displacement(2) / 2.36;

            YawMode testyawmode = msr::airlib::YawMode(is_rate, yaw_or_rate);
            // client.moveByVelocityZBodyFrameAsync(1.5 * 3.0, 0, -40., 15, drivetrain, testyawmode, drone_name);
            client.setVelocityControllerGains(kpvec, kivec, kdvec, drone_name);
            client.moveByVelocityZBodyFrameAsync(1.5 *selected_action_displacement(0), 1.5*selected_action_displacement(1), -40., 3., drivetrain, testyawmode, drone_name)->waitOnLastTask();
            // client.hoverAsync(drone_name)->waitOnLastTask();
        }
        
        // std::cout << "Press Enter to disarm" << std::endl;
        // std::cin.get();
        client.armDisarm(false, drone_name);
    }


    catch (rpc::rpc_error& e) {
        const auto msg = e.get_error().as<std::string>();
        std::cout << "Exception raised by the API, something went wrong." << std::endl
                  << msg << std::endl;
    }

    return 0;
}