Created
February 3, 2024 07:35
-
-
Save sandilyasg/1966c156f40ddcdf3b6715f55e607f5d to your computer and use it in GitHub Desktop.
Script to control quadrotor through checkpoints
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/* | |
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; | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment