From d16dbe5940d064f9c8101f6c6b30f2366cfcbbc6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 29 May 2021 11:37:16 +0200 Subject: [PATCH] examples: pass through fly_mission example --- examples/fly_mission/fly_mission.cpp | 257 ++++++++++----------------- 1 file changed, 93 insertions(+), 164 deletions(-) diff --git a/examples/fly_mission/fly_mission.cpp b/examples/fly_mission/fly_mission.cpp index f4de36e5af..d0ee1c988d 100644 --- a/examples/fly_mission/fly_mission.cpp +++ b/examples/fly_mission/fly_mission.cpp @@ -1,17 +1,6 @@ -/** - * @file fly_mission.cpp - * - * @brief Demonstrates how to Add & Fly Waypoint missions using the MAVSDK. - * The example is summarised below: - * 1. Adds mission items. - * 2. Starts mission from first mission item. - * 3. Illustrates Pause/Resume mission item. - * 4. Exits after the mission is accomplished. - * - * @author Julian Oes , - * Shakthi Prashanth M - * @date 2017-09-06 - */ +// +// Demonstrates how to add and fly Waypoint missions using the MAVSDK. +// #include #include @@ -24,22 +13,10 @@ #include #include -#define ERROR_CONSOLE_TEXT "\033[31m" // Turn text on console red -#define TELEMETRY_CONSOLE_TEXT "\033[34m" // Turn text on console blue -#define NORMAL_CONSOLE_TEXT "\033[0m" // Restore normal console colour - using namespace mavsdk; using std::chrono::seconds; -using std::chrono::milliseconds; using std::this_thread::sleep_for; -// Handles Action's result -inline void handle_action_err_exit(Action::Result result, const std::string& message); -// Handles Mission's result -inline void handle_mission_err_exit(Mission::Result result, const std::string& message); -// Handles Connection result -inline void handle_connection_err_exit(ConnectionResult result, const std::string& message); - static Mission::MissionItem make_mission_item( double latitude_deg, double longitude_deg, @@ -50,70 +27,78 @@ static Mission::MissionItem make_mission_item( float gimbal_yaw_deg, Mission::MissionItem::CameraAction camera_action); -void usage(std::string bin_name) +void usage(const std::string& bin_name) { - std::cout << NORMAL_CONSOLE_TEXT << "Usage : " << bin_name << " " << std::endl - << "Connection URL format should be :" << std::endl - << " For TCP : tcp://[server_host][:server_port]" << std::endl - << " For UDP : udp://[bind_host][:bind_port]" << std::endl - << " For Serial : serial:///path/to/serial/dev[:baudrate]" << std::endl - << "For example, to connect to the simulator use URL: udp://:14540" << std::endl; + std::cerr << "Usage : " << bin_name << " \n" + << "Connection URL format should be :\n" + << " For TCP : tcp://[server_host][:server_port]\n" + << " For UDP : udp://[bind_host][:bind_port]\n" + << " For Serial : serial:///path/to/serial/dev[:baudrate]\n" + << "For example, to connect to the simulator use URL: udp://:14540\n"; } -int main(int argc, char** argv) +std::shared_ptr get_system(Mavsdk& mavsdk) { - Mavsdk mavsdk; + std::cout << "Waiting to discover system...\n"; + auto prom = std::promise>{}; + auto fut = prom.get_future(); - { - auto prom = std::promise{}; - auto future_result = prom.get_future(); - - std::cout << "Waiting to discover system..." << std::endl; - mavsdk.subscribe_on_new_system([&mavsdk, &prom]() { - const auto system = mavsdk.systems().at(0); - - if (system->is_connected()) { - std::cout << "Discovered system" << std::endl; - prom.set_value(); - } else { - std::cout << "System timed out" << std::endl; - std::cout << "Exiting." << std::endl; - exit(0); - } - }); - - std::string connection_url; - ConnectionResult connection_result; - - if (argc == 2) { - connection_url = argv[1]; - connection_result = mavsdk.add_any_connection(connection_url); - } else { - usage(argv[0]); - return 1; - } + // We wait for new systems to be discovered, once we find one that has an + // autopilot, we decide to use it. + mavsdk.subscribe_on_new_system([&mavsdk, &prom]() { + auto system = mavsdk.systems().back(); - if (connection_result != ConnectionResult::Success) { - std::cout << ERROR_CONSOLE_TEXT << "Connection failed: " << connection_result - << NORMAL_CONSOLE_TEXT << std::endl; - return 1; + if (system->has_autopilot()) { + std::cout << "Discovered autopilot\n"; + + // Unsubscribe again as we only want to find one system. + mavsdk.subscribe_on_new_system(nullptr); + prom.set_value(system); } + }); + + // We usually receive heartbeats at 1Hz, therefore we should find a + // system after around 3 seconds max, surely. + if (fut.wait_for(seconds(3)) == std::future_status::timeout) { + std::cerr << "No autopilot found.\n"; + return {}; + } + + // Get discovered system now. + return fut.get(); +} + +int main(int argc, char** argv) +{ + if (argc != 2) { + usage(argv[0]); + return 1; + } + + Mavsdk mavsdk; + ConnectionResult connection_result = mavsdk.add_any_connection(argv[1]); + + if (connection_result != ConnectionResult::Success) { + std::cerr << "Connection failed: " << connection_result << '\n'; + return 1; + } - future_result.get(); + auto system = get_system(mavsdk); + if (!system) { + return 1; } - auto system = mavsdk.systems().at(0); auto action = Action{system}; auto mission = Mission{system}; auto telemetry = Telemetry{system}; while (!telemetry.health_all_ok()) { - std::cout << "Waiting for system to be ready" << std::endl; + std::cout << "Waiting for system to be ready\n"; sleep_for(seconds(1)); } - std::cout << "System ready" << std::endl; - std::cout << "Creating and uploading mission" << std::endl; + std::cout << "System ready\n"; + std::cout << "Creating and uploading mission\n"; std::vector mission_items; @@ -177,35 +162,29 @@ int main(int argc, char** argv) 0.0f, Mission::MissionItem::CameraAction::StopPhotoInterval)); - { - std::cout << "Uploading mission..." << std::endl; - // We only have the upload_mission function asynchronous for now, so we wrap it using - // std::future. - auto prom = std::promise{}; - auto future_result = prom.get_future(); - Mission::MissionPlan mission_plan{}; - mission_plan.mission_items = mission_items; - mission.upload_mission_async( - mission_plan, [&prom](Mission::Result result) { prom.set_value(result); }); - - const Mission::Result result = future_result.get(); - if (result != Mission::Result::Success) { - std::cout << "Mission upload failed (" << result << "), exiting." << std::endl; - return 1; - } - std::cout << "Mission uploaded." << std::endl; + std::cout << "Uploading mission...\n"; + Mission::MissionPlan mission_plan{}; + mission_plan.mission_items = mission_items; + const Mission::Result upload_result = mission.upload_mission(mission_plan); + + if (upload_result != Mission::Result::Success) { + std::cerr << "Mission upload failed: " << upload_result << ", exiting.\n"; + return 1; } - std::cout << "Arming..." << std::endl; + std::cout << "Arming...\n"; const Action::Result arm_result = action.arm(); - handle_action_err_exit(arm_result, "Arm failed: "); - std::cout << "Armed." << std::endl; + if (arm_result != Action::Result::Success) { + std::cerr << "Arming failed: " << arm_result << '\n'; + return 1; + } + std::cout << "Armed.\n"; std::atomic want_to_pause{false}; // Before starting the mission, we want to be sure to subscribe to the mission progress. mission.subscribe_mission_progress([&want_to_pause](Mission::MissionProgress mission_progress) { std::cout << "Mission status update: " << mission_progress.current << " / " - << mission_progress.total << std::endl; + << mission_progress.total << '\n'; if (mission_progress.current >= 2) { // We can only set a flag here. If we do more request inside the callback, @@ -214,71 +193,46 @@ int main(int argc, char** argv) } }); - { - std::cout << "Starting mission." << std::endl; - auto prom = std::promise{}; - auto future_result = prom.get_future(); - mission.start_mission_async([&prom](Mission::Result result) { - prom.set_value(result); - std::cout << "Started mission." << std::endl; - }); - - const Mission::Result result = future_result.get(); - handle_mission_err_exit(result, "Mission start failed: "); + Mission::Result start_mission_result = mission.start_mission(); + if (start_mission_result != Mission::Result::Success) { + std::cerr << "Starting mission failed: " << start_mission_result << '\n'; + return 1; } while (!want_to_pause) { sleep_for(seconds(1)); } - { - auto prom = std::promise{}; - auto future_result = prom.get_future(); + std::cout << "Pausing mission...\n"; + const Mission::Result pause_mission_result = mission.pause_mission(); - std::cout << "Pausing mission..." << std::endl; - mission.pause_mission_async([&prom](Mission::Result result) { prom.set_value(result); }); - - const Mission::Result result = future_result.get(); - if (result != Mission::Result::Success) { - std::cout << "Failed to pause mission (" << result << ")" << std::endl; - } else { - std::cout << "Mission paused." << std::endl; - } + if (pause_mission_result != Mission::Result::Success) { + std::cerr << "Failed to pause mission:" << pause_mission_result << '\n'; } + std::cout << "Mission paused.\n"; // Pause for 5 seconds. sleep_for(seconds(5)); // Then continue. - { - auto prom = std::promise{}; - auto future_result = prom.get_future(); - - std::cout << "Resuming mission..." << std::endl; - mission.start_mission_async([&prom](Mission::Result result) { prom.set_value(result); }); - - const Mission::Result result = future_result.get(); - if (result != Mission::Result::Success) { - std::cout << "Failed to resume mission (" << result << ")" << std::endl; - } else { - std::cout << "Resumed mission." << std::endl; - } + Mission::Result start_mission_again_result = mission.start_mission(); + if (start_mission_again_result != Mission::Result::Success) { + std::cerr << "Starting mission again failed: " << start_mission_again_result << '\n'; + return 1; } while (!mission.is_mission_finished().second) { sleep_for(seconds(1)); } - { - // We are done, and can do RTL to go home. - std::cout << "Commanding RTL..." << std::endl; - const Action::Result result = action.return_to_launch(); - if (result != Action::Result::Success) { - std::cout << "Failed to command RTL (" << result << ")" << std::endl; - } else { - std::cout << "Commanded RTL." << std::endl; - } + // We are done, and can do RTL to go home. + std::cout << "Commanding RTL...\n"; + const Action::Result rtl_result = action.return_to_launch(); + if (rtl_result != Action::Result::Success) { + std::cout << "Failed to command RTL: " << rtl_result << '\n'; + return 1; } + std::cout << "Commanded RTL.\n"; // We need to wait a bit, otherwise the armed state might not be correct yet. sleep_for(seconds(2)); @@ -287,7 +241,7 @@ int main(int argc, char** argv) // Wait until we're done. sleep_for(seconds(1)); } - std::cout << "Disarmed, exiting." << std::endl; + std::cout << "Disarmed, exiting.\n"; } Mission::MissionItem make_mission_item( @@ -311,28 +265,3 @@ Mission::MissionItem make_mission_item( new_item.camera_action = camera_action; return new_item; } - -inline void handle_action_err_exit(Action::Result result, const std::string& message) -{ - if (result != Action::Result::Success) { - std::cerr << ERROR_CONSOLE_TEXT << message << result << NORMAL_CONSOLE_TEXT << std::endl; - exit(EXIT_FAILURE); - } -} - -inline void handle_mission_err_exit(Mission::Result result, const std::string& message) -{ - if (result != Mission::Result::Success) { - std::cerr << ERROR_CONSOLE_TEXT << message << result << NORMAL_CONSOLE_TEXT << std::endl; - exit(EXIT_FAILURE); - } -} - -// Handles connection result -inline void handle_connection_err_exit(ConnectionResult result, const std::string& message) -{ - if (result != ConnectionResult::Success) { - std::cerr << ERROR_CONSOLE_TEXT << message << result << NORMAL_CONSOLE_TEXT << std::endl; - exit(EXIT_FAILURE); - } -}