Skip to content

Commit

Permalink
Fix HelloSpawnedDrones C++ file to use new API, naming convention, co…
Browse files Browse the repository at this point in the history 
…nvert to spaces
  • Loading branch information
@rajat2004
rajat2004 committed Feb 8, 2021
1 parent daefd7f commit 7dbba6c
Showing 1 changed file with 123 additions and 121 deletions.
244 changes: 123 additions & 121 deletions HelloSpawnedDrones/HelloSpawnedDrones.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -19,154 +19,156 @@ STRICT_MODE_ON

void runSingleClient(uint16_t port, int ordinal)
{
using namespace msr::airlib;
const char host[] = "localhost";
float timeout_s = 60;
using namespace msr::airlib;
const char host[] = "localhost";
float timeout_s = 60;

try
{
msr::airlib::MultirotorRpcLibClient *client = new msr::airlib::MultirotorRpcLibClient(host, port, timeout_s);
std::cout << "Confirming connections..." << std::endl;
client->confirmConnection();
try
{
MultirotorRpcLibClient *client = new MultirotorRpcLibClient(host, port, timeout_s);
std::cout << "Confirming connections..." << std::endl;
client->confirmConnection();

std::string vehicleName = "UAV_" + std::to_string(ordinal);
std::cout << "Vehicle name:" << vehicleName << std::endl;
std::string vehicle_name = "UAV_" + std::to_string(ordinal);
std::cout << "Vehicle name:" << vehicle_name << std::endl;

client->simAddVehicle(vehicleName, "simpleflight", "", 0, 5.0f * (ordinal + 1), 0);
Pose pose(Vector3r(0, 5.0f * (ordinal + 1), 0), Quaternionr(0, 0, 0, 0));
client->simAddVehicle(vehicle_name, "simpleflight", pose, "");

// This is a bit crude, but give it a moment to settle on the ground, else takeoff will fail
std::this_thread::sleep_for(std::chrono::duration<double>(2));
// This is a bit crude, but give it a moment to settle on the ground, else takeoff will fail
std::this_thread::sleep_for(std::chrono::duration<double>(2));

// moveByVelocityZ is an offboard operation, so we need to set offboard mode.
client->enableApiControl(true, vehicleName);
client->armDisarm(true, vehicleName);
// moveByVelocityZ is an offboard operation, so we need to set offboard mode.
client->enableApiControl(true, vehicle_name);
client->armDisarm(true, vehicle_name);

auto groundPosition = client->getMultirotorState(vehicleName).getPosition();
float groundZ = groundPosition.z(); // current position (NED coordinate system).
auto groundPosition = client->getMultirotorState(vehicle_name).getPosition();
float groundZ = groundPosition.z(); // current position (NED coordinate system).

float takeoffTimeout = 5;
std::cout << "Initiating takeoff for " << vehicleName << "..." << std::endl;
client->takeoffAsync(takeoffTimeout, vehicleName)->waitOnLastTask();
std::cout << "Completed takeoff for " << vehicleName << "..." << std::endl;
float takeoffTimeout = 5;
std::cout << "Initiating takeoff for " << vehicle_name << "..." << std::endl;
client->takeoffAsync(takeoffTimeout, vehicle_name)->waitOnLastTask();
std::cout << "Completed takeoff for " << vehicle_name << "..." << std::endl;

const float speed = 3.0f;
const float speed = 3.0f;

// switch to explicit hover mode so that this is the fallback when
// move* commands are finished.
std::cout << "Initiating hover for " << vehicleName << "..." << std::endl;
client->hoverAsync(vehicleName)->waitOnLastTask();
std::cout << "Completed hover for " << vehicleName << "..." << std::endl;
// switch to explicit hover mode so that this is the fallback when
// move* commands are finished.
std::cout << "Initiating hover for " << vehicle_name << "..." << std::endl;
client->hoverAsync(vehicle_name)->waitOnLastTask();
std::cout << "Completed hover for " << vehicle_name << "..." << std::endl;

auto position = client->getMultirotorState(vehicleName).getPosition();
float duration = 1;
float z = position.z(); // current position (NED coordinate system).
auto position = client->getMultirotorState(vehicle_name).getPosition();
float duration = 1;
float z = position.z(); // current position (NED coordinate system).

// Altitude difference between each platform, in meters
const float altitudeDelta = 1.0f;
// Altitude difference between each platform, in meters
const float altitudeDelta = 1.0f;

z -= ordinal * altitudeDelta;
float timeout = 10.0f;
client->moveToZAsync(z, speed, timeout, msr::airlib::YawMode(), -1.0f, 1.0f, vehicleName)->waitOnLastTask();
z -= ordinal * altitudeDelta;
float timeout = 10.0f;
client->moveToZAsync(z, speed, timeout, YawMode(), -1.0f, 1.0f, vehicle_name)->waitOnLastTask();

std::cout << "Completed move to z " << z << " for " << vehicleName << "..." << std::endl;
std::cout << "Flying in a 10m box pattern at 3 m/s velocity" << std::endl;
std::cout << "Completed move to z " << z << " for " << vehicle_name << "..." << std::endl;
std::cout << "Flying in a 10m box pattern at 3 m/s velocity" << std::endl;

const float size = 5.0f;
duration = size / speed;
DrivetrainType driveTrain = DrivetrainType::ForwardOnly;
YawMode yaw_mode(true, 0);
const float size = 5.0f;
duration = size / speed;
DrivetrainType driveTrain = DrivetrainType::ForwardOnly;
YawMode yaw_mode(true, 0);

position = client->getMultirotorState(vehicleName).getPosition();
std::cout << "Position of " << port << ": " << position << std::endl;
z = position.z(); // current position (NED coordinate system).
position = client->getMultirotorState(vehicle_name).getPosition();
std::cout << "Position of " << port << ": " << position << std::endl;
z = position.z(); // current position (NED coordinate system).

std::cout << "moveByVelocityZ(" << speed << ", 0, " << z << "," << duration << ")" << std::endl;
client->moveByVelocityZAsync(speed, 0, z, duration, driveTrain, yaw_mode, vehicleName);
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, vehicleName);
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, vehicleName);
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, vehicleName);
std::cout << "moveByVelocityZ(" << speed << ", 0, " << z << "," << duration << ")" << std::endl;
client->moveByVelocityZAsync(speed, 0, z, duration, driveTrain, yaw_mode, vehicle_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, vehicle_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, vehicle_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, vehicle_name);

std::this_thread::sleep_for(std::chrono::duration<double>(duration));
std::this_thread::sleep_for(std::chrono::duration<double>(duration));

client->moveToZAsync(groundZ - 0.5f, speed, timeout, msr::airlib::YawMode(), -1.0f, 1.0f, vehicleName)->waitOnLastTask();
client->moveToZAsync(groundZ - 0.5f, speed, timeout, YawMode(), -1.0f, 1.0f, vehicle_name)->waitOnLastTask();

std::cout << "Hovering..." << std::endl;
client->hoverAsync(vehicleName)->waitOnLastTask();
std::cout << "Hovering..." << std::endl;
client->hoverAsync(vehicle_name)->waitOnLastTask();

client->enableApiControl(true, vehicleName);
client->enableApiControl(true, vehicle_name);

std::cout << "Landing..." << std::endl;
client->landAsync(timeout, vehicleName)->waitOnLastTask();
std::this_thread::sleep_for(std::chrono::duration<double>(5));
std::cout << "Landing..." << std::endl;
client->landAsync(timeout, vehicle_name)->waitOnLastTask();
std::this_thread::sleep_for(std::chrono::duration<double>(5));

std::cout << "Disarming..." << std::endl;
client->armDisarm(false, vehicleName);
std::cout << "Disarming..." << std::endl;
client->armDisarm(false, vehicle_name);

std::cout << "Done!..." << std::endl;
std::cout << "Done!..." << std::endl;

delete client;
delete client;

std::this_thread::sleep_for(std::chrono::duration<double>(50));
}
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;
}
std::this_thread::sleep_for(std::chrono::duration<double>(50));
}
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;
}
}

int main(int argc, char *argv[])
{
using namespace msr::airlib;

uint16_t rpcPort = 41451;
int numPlatforms = 1;

std::cout << "argc is " << argc << std::endl;
if (argc > 1)
{
std::cout << "Num plats string: " << argv[1] << std::endl;
int numPlatsInt = atoi(argv[1]);
numPlatforms = static_cast<uint16_t>(numPlatsInt);
}

std::cout << "First port is " << rpcPort << std::endl;
std::cout << "Num platforms: " << numPlatforms << std::endl;
std::cout << "Making clients..." << std::endl;


try
{
std::cout << "Press Enter to begin..." << std::endl; std::cin.get();

std::vector<std::thread> clientThreads;

// Count down, so the first one can easily go the highest (without knowing count)
int clientOrdinal = numPlatforms - 1;
for (int i = 0; i < numPlatforms; i++)
{
clientThreads.push_back(std::thread(runSingleClient, rpcPort, clientOrdinal));
clientOrdinal--;

std::this_thread::sleep_for(std::chrono::duration<double>(0.1));
}

for (auto &toJoin : clientThreads)
{
toJoin.join();
}
}
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;
using namespace msr::airlib;

uint16_t rpc_port = 41451;
int num_platforms = 1;

std::cout << "argc is " << argc << std::endl;
if (argc > 1)
{
std::cout << "Num plats string: " << argv[1] << std::endl;
int numPlatsInt = atoi(argv[1]);
num_platforms = static_cast<uint16_t>(numPlatsInt);
}

std::cout << "First port is " << rpc_port << std::endl;
std::cout << "Num platforms: " << num_platforms << std::endl;
std::cout << "Making clients..." << std::endl;


try
{
std::cout << "Press Enter to begin..." << std::endl;
std::cin.get();

std::vector<std::thread> clientThreads;

// Count down, so the first one can easily go the highest (without knowing count)
int clientOrdinal = num_platforms - 1;
for (int i = 0; i < num_platforms; i++)
{
clientThreads.push_back(std::thread(runSingleClient, rpc_port, clientOrdinal));
clientOrdinal--;

std::this_thread::sleep_for(std::chrono::duration<double>(0.1));
}

for (auto &toJoin : clientThreads)
{
toJoin.join();
}
}
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;
}

0 comments on commit 7dbba6c

Please sign in to comment.