diff --git a/HelloSpawnedDrones/HelloSpawnedDrones.cpp b/HelloSpawnedDrones/HelloSpawnedDrones.cpp index 9bfd4e4b6c..a1398d3a2d 100644 --- a/HelloSpawnedDrones/HelloSpawnedDrones.cpp +++ b/HelloSpawnedDrones/HelloSpawnedDrones.cpp @@ -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(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(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(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(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(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(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(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(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(duration)); + std::this_thread::sleep_for(std::chrono::duration(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(5)); + std::cout << "Landing..." << std::endl; + client->landAsync(timeout, vehicle_name)->waitOnLastTask(); + std::this_thread::sleep_for(std::chrono::duration(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(50)); - } - catch (rpc::rpc_error& e) - { - std::string msg = e.get_error().as(); - std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl; - } + std::this_thread::sleep_for(std::chrono::duration(50)); + } + catch (rpc::rpc_error& e) + { + std::string msg = e.get_error().as(); + 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(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 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(0.1)); - } - - for (auto &toJoin : clientThreads) - { - toJoin.join(); - } - } - catch (rpc::rpc_error& e) - { - std::string msg = e.get_error().as(); - 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(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 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(0.1)); + } + + for (auto &toJoin : clientThreads) + { + toJoin.join(); + } + } + catch (rpc::rpc_error& e) + { + std::string msg = e.get_error().as(); + std::cout << "Exception raised by the API, something went wrong." << std::endl << msg << std::endl; + } + + return 0; }