Skip to content

Instantly share code, notes, and snippets.

@CuriousG102
Created August 2, 2021 00:35
Show Gist options
  • Save CuriousG102/d4ca439d6d6acb6da6bc2cb91357f4f6 to your computer and use it in GitHub Desktop.
Save CuriousG102/d4ca439d6d6acb6da6bc2cb91357f4f6 to your computer and use it in GitHub Desktop.
Script for flying drone in Airsim
// Copyright (c) Microsoft Corporation. All rights reserved.
// Licensed under the MIT License.
#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>
#include <windows.h>
#include <stdio.h>
HANDLE hStdin;
DWORD fdwSaveOldMode;
constexpr auto KEY_UP = 'i';
constexpr auto KEY_DOWN = 'k';
constexpr auto KEY_LEFT = 'j';
constexpr auto KEY_RIGHT = 'l';
constexpr auto KEY_GAIN = 'w';
constexpr auto KEY_LOSE = 's';
constexpr auto KEY_ROT_LEFT = 'a';
constexpr auto KEY_ROT_RIGHT = 'd';
VOID ErrorExit(LPSTR);
void UpdateKey(char, bool*, KEY_EVENT_RECORD);
int main()
{
using namespace msr::airlib;
msr::airlib::MultirotorRpcLibClient client;
DWORD cNumRead, fdwMode, i;
INPUT_RECORD irInBuf[128];
hStdin = GetStdHandle(STD_INPUT_HANDLE);
if (hStdin == INVALID_HANDLE_VALUE)
ErrorExit("GetStdHandle");
if (!GetConsoleMode(hStdin, &fdwSaveOldMode))
ErrorExit("GetConsoleMode");
fdwMode = ENABLE_WINDOW_INPUT | ENABLE_MOUSE_INPUT;
if (!SetConsoleMode(hStdin, fdwMode))
ErrorExit("SetConsoleMode");
try {
client.confirmConnection();
client.enableApiControl(true);
client.armDisarm(true);
float takeoff_timeout = 5;
client.takeoffAsync(takeoff_timeout)->waitOnLastTask();
// switch to explicit hover mode so that this is the fall back when
// move* commands are finished.
std::this_thread::sleep_for(std::chrono::duration<double>(5));
client.hoverAsync()->waitOnLastTask();
// moveByVelocityZ is an offboard operation, so we need to set offboard mode.
client.enableApiControl(true);
auto position = client.getMultirotorState().getPosition();
const float size = 10.0f;
DrivetrainType drivetrain = DrivetrainType::ForwardOnly;
const float SPEED = 9.0f;
const float ROTATIONAL_SPEED = 60.f;
const float DURATION = 1.0f / 15.0f;
const float VERTICAL_SPEED = 6.f;
bool leftKey = false;
bool rightKey = false;
bool upKey = false;
bool downKey = false;
bool rKey = false;
bool fKey = false;
bool rotLeftKey = false;
bool rotRightKey = false;
DWORD numberOfEvents;
while (true) {
if (!GetNumberOfConsoleInputEvents(hStdin, &numberOfEvents))
ErrorExit("GetNumberOfConsoleInputEvents");
if (numberOfEvents > 0) {
if (!ReadConsoleInput(
hStdin, // input buffer handle
irInBuf, // buffer to read into
128, // size of read buffer
&cNumRead)) // number of records read
ErrorExit("ReadConsoleInput");
for (i = 0; i < cNumRead; i++) {
if (irInBuf[i].EventType != KEY_EVENT)
continue;
UpdateKey(KEY_LEFT, &leftKey, irInBuf[i].Event.KeyEvent);
UpdateKey(KEY_RIGHT, &rightKey, irInBuf[i].Event.KeyEvent);
UpdateKey(KEY_UP, &upKey, irInBuf[i].Event.KeyEvent);
UpdateKey(KEY_DOWN, &downKey, irInBuf[i].Event.KeyEvent);
UpdateKey(KEY_GAIN, &rKey, irInBuf[i].Event.KeyEvent);
UpdateKey(KEY_LOSE, &fKey, irInBuf[i].Event.KeyEvent);
UpdateKey(KEY_ROT_LEFT, &rotLeftKey, irInBuf[i].Event.KeyEvent);
UpdateKey(KEY_ROT_RIGHT, &rotRightKey, irInBuf[i].Event.KeyEvent);
}
}
YawMode yaw_mode(true, rotRightKey * ROTATIONAL_SPEED - rotLeftKey * ROTATIONAL_SPEED);
const float vY = rightKey * SPEED - leftKey * SPEED;
const float vX = upKey* SPEED - downKey * SPEED;
const float vZ = fKey * VERTICAL_SPEED - rKey * VERTICAL_SPEED;
std::cout << "moveByVelocity(" << vX << "," << vY << "," << vZ << "," << DURATION << ")" << std::endl;
client.moveByVelocityBodyFrameAsync(vX, vY, vZ, DURATION, drivetrain, yaw_mode);
std::this_thread::sleep_for(std::chrono::duration<double>(DURATION));
}
client.hoverAsync()->waitOnLastTask();
std::cout << "Press Enter to land" << std::endl; std::cin.get();
client.landAsync()->waitOnLastTask();
std::cout << "Press Enter to disarm" << std::endl; std::cin.get();
client.armDisarm(false);
}
catch (rpc::rpc_error& e) {
std::string msg = e.get_error().as<std::string>();
std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl;
}
return 0;
}
void UpdateKey(char key, bool* keyState, KEY_EVENT_RECORD ker) {
if (ker.uChar.AsciiChar != key)
return;
*keyState = ker.bKeyDown;
}
VOID ErrorExit(LPSTR lpszMessage)
{
fprintf(stderr, "%s\n", lpszMessage);
// Restore input mode on exit.
SetConsoleMode(hStdin, fdwSaveOldMode);
ExitProcess(0);
}
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment