Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 20:28:37 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

mavsdk_tests: add header and fix style

Browse Source
This commit is contained in:
Julian Oes
2020-03-17 16:15:34 +01:00
committed by Nuno Marques
parent 9b61ce1006
commit b5048a3414
6 changed files with 477 additions and 287 deletions
Download Patch File Download Diff File Expand all files Collapse all files
test/mavsdk_tests/autopilot_tester.cpp
+48 -11
View File
@@ -1,3 +1,36 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include "autopilot_tester.h"
#include <iostream>
#include <future>
@@ -13,7 +46,7 @@ void AutopilotTester::connect(const std::string uri)
REQUIRE(poll_condition_with_timeout(
[this]() { return _mavsdk.is_connected(); }, std::chrono::seconds(25)));
auto& system = _mavsdk.system();
auto &system = _mavsdk.system();
_telemetry.reset(new Telemetry(system));
_action.reset(new Action(system));
@@ -109,7 +142,8 @@ void AutopilotTester::prepare_square_mission(MissionOptions mission_options)
std::vector<std::shared_ptr<MissionItem>> mission_items {};
mission_items.push_back(create_mission_item({mission_options.leg_length_m, 0.}, mission_options, ct));
mission_items.push_back(create_mission_item({mission_options.leg_length_m, mission_options.leg_length_m}, mission_options, ct));
mission_items.push_back(create_mission_item({mission_options.leg_length_m, mission_options.leg_length_m},
mission_options, ct));
mission_items.push_back(create_mission_item({0., mission_options.leg_length_m}, mission_options, ct));
_mission->set_return_to_launch_after_mission(mission_options.rtl_at_end);
@@ -152,9 +186,9 @@ CoordinateTransformation AutopilotTester::get_coordinate_transformation()
}
std::shared_ptr<MissionItem> AutopilotTester::create_mission_item(
const CoordinateTransformation::LocalCoordinate& local_coordinate,
const MissionOptions& mission_options,
const CoordinateTransformation& ct)
const CoordinateTransformation::LocalCoordinate &local_coordinate,
const MissionOptions &mission_options,
const CoordinateTransformation &ct)
{
auto mission_item = std::make_shared<MissionItem>();
const auto pos_north = ct.global_from_local(local_coordinate);
@@ -168,12 +202,13 @@ void AutopilotTester::execute_rtl()
REQUIRE(Action::Result::SUCCESS == _action->return_to_launch());
}
void AutopilotTester::offboard_goto(const Offboard::PositionNEDYaw& target, float acceptance_radius_m, std::chrono::seconds timeout_duration)
void AutopilotTester::offboard_goto(const Offboard::PositionNEDYaw &target, float acceptance_radius_m,
std::chrono::seconds timeout_duration)
{
_offboard->set_position_ned(target);
REQUIRE(_offboard->start() == Offboard::Result::SUCCESS);
CHECK(poll_condition_with_timeout(
[=]() { return estimated_position_close_to(target, acceptance_radius_m); }, timeout_duration));
[ = ]() { return estimated_position_close_to(target, acceptance_radius_m); }, timeout_duration));
std::cout << "Target position reached" << std::endl;
}
@@ -187,7 +222,7 @@ void AutopilotTester::offboard_land()
_offboard->set_velocity_ned(land_velocity);
}
bool AutopilotTester::estimated_position_close_to(const Offboard::PositionNEDYaw& target_pos, float acceptance_radius_m)
bool AutopilotTester::estimated_position_close_to(const Offboard::PositionNEDYaw &target_pos, float acceptance_radius_m)
{
Telemetry::PositionNED est_pos = _telemetry->position_velocity_ned().position;
return sq(est_pos.north_m - target_pos.north_m) +
@@ -195,7 +230,8 @@ bool AutopilotTester::estimated_position_close_to(const Offboard::PositionNEDYaw
sq(est_pos.down_m - target_pos.down_m) < sq(acceptance_radius_m);
}
bool AutopilotTester::estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw& target_pos, float acceptance_radius_m)
bool AutopilotTester::estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw &target_pos,
float acceptance_radius_m)
{
Telemetry::PositionNED est_pos = _telemetry->position_velocity_ned().position;
return sq(est_pos.north_m - target_pos.north_m) +
@@ -213,7 +249,8 @@ Telemetry::GroundTruth AutopilotTester::get_ground_truth_position()
return _telemetry->ground_truth();
}
bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth& target_pos, float acceptance_radius_m)
bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos,
float acceptance_radius_m)
{
CHECK(std::isfinite(target_pos.latitude_deg));
CHECK(std::isfinite(target_pos.longitude_deg));
@@ -224,7 +261,7 @@ bool AutopilotTester::ground_truth_horizontal_position_close_to(const Telemetry:
Telemetry::GroundTruth current_pos = _telemetry->ground_truth();
CHECK(std::isfinite(current_pos.latitude_deg));
CHECK(std::isfinite(current_pos.longitude_deg));
LocalCoordinate local_pos= ct.local_from_global(GlobalCoordinate{current_pos.latitude_deg, current_pos.longitude_deg});
LocalCoordinate local_pos = ct.local_from_global(GlobalCoordinate{current_pos.latitude_deg, current_pos.longitude_deg});
return sq(local_pos.north_m) + sq(local_pos.east_m) < sq(acceptance_radius_m);
}
test/mavsdk_tests/autopilot_tester.h
+45 -8
View File
@@ -1,3 +1,36 @@
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#pragma once
#include <mavsdk/mavsdk.h>
@@ -16,7 +49,8 @@ extern std::string connection_url;
using namespace mavsdk;
using namespace mavsdk::geometry;
class AutopilotTester {
class AutopilotTester
{
public:
struct MissionOptions {
double leg_length_m {20.0};
@@ -40,7 +74,7 @@ public:
void prepare_square_mission(MissionOptions mission_options);
void execute_mission();
void execute_rtl();
void offboard_goto(const Offboard::PositionNEDYaw& target, float acceptance_radius_m = 0.3f,
void offboard_goto(const Offboard::PositionNEDYaw &target, float acceptance_radius_m = 0.3f,
std::chrono::seconds timeout_duration = std::chrono::seconds(60));
void offboard_land();
void request_ground_truth();
@@ -49,14 +83,14 @@ public:
private:
mavsdk::geometry::CoordinateTransformation get_coordinate_transformation();
std::shared_ptr<mavsdk::MissionItem> create_mission_item(
const mavsdk::geometry::CoordinateTransformation::LocalCoordinate& local_coordinate,
const MissionOptions& mission_options,
const mavsdk::geometry::CoordinateTransformation& ct);
const mavsdk::geometry::CoordinateTransformation::LocalCoordinate &local_coordinate,
const MissionOptions &mission_options,
const mavsdk::geometry::CoordinateTransformation &ct);
Telemetry::GroundTruth get_ground_truth_position();
bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth& target_pos, float acceptance_radius_m);
bool estimated_position_close_to(const Offboard::PositionNEDYaw& target_position, float acceptance_radius_m);
bool estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw& target_pos, float acceptance_radius_m);
bool ground_truth_horizontal_position_close_to(const Telemetry::GroundTruth &target_pos, float acceptance_radius_m);
bool estimated_position_close_to(const Offboard::PositionNEDYaw &target_position, float acceptance_radius_m);
bool estimated_horizontal_position_close_to(const Offboard::PositionNEDYaw &target_pos, float acceptance_radius_m);
mavsdk::Mavsdk _mavsdk{};
std::unique_ptr<mavsdk::Telemetry> _telemetry{};
@@ -75,12 +109,15 @@ bool poll_condition_with_timeout(
const std::chrono::milliseconds duration_ms(duration);
unsigned iteration = 0;
while (!fun()) {
std::this_thread::sleep_for(duration_ms / 10);
if (iteration++ >= 10) {
return false;
}
}
return true;
}
test/mavsdk_tests/test_main.cpp
+44 -12
View File
@@ -1,7 +1,35 @@
//
// Multicopter mission test.
//
// Author: Julian Oes <julian@oes.ch>
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#define CATCH_CONFIG_RUNNER
#include "catch2/catch.hpp"
@@ -12,10 +40,10 @@
#include "autopilot_tester.h"
static void usage(const std::string& bin_name);
static void remove_argv(int& argc, char** argv, int pos);
static void usage(const std::string &bin_name);
static void remove_argv(int &argc, char **argv, int pos);
int main(int argc, char** argv)
int main(int argc, char **argv)
{
for (int i = 0; i < argc; ++i) {
const std::string argv_string(argv[i]);
@@ -26,9 +54,10 @@ int main(int argc, char** argv)
if (argv_string == "--url") {
if (argc > i + 1) {
connection_url = argv[i+1];
connection_url = argv[i + 1];
remove_argv(argc, argv, i);
remove_argv(argc, argv, i);
} else {
std::cerr << "No connection URL supplied" << std::endl;
usage(argv[0]);
@@ -39,13 +68,15 @@ int main(int argc, char** argv)
Catch::Session session;
const int catch_ret = session.applyCommandLine(argc, argv);
if (catch_ret != 0) {
return catch_ret;
}
return session.run();
}
void usage(const std::string& bin_name)
void usage(const std::string &bin_name)
{
std::cout << std::endl
<< "Usage : " << bin_name << " [--url CONNECTION_URL] [catch2 arguments]" << std::endl
@@ -57,10 +88,11 @@ void usage(const std::string& bin_name)
<< std::endl;
}
void remove_argv(int& argc, char** argv, int pos)
void remove_argv(int &argc, char **argv, int pos)
{
for (int i = pos; i+1 < argc; ++i) {
argv[i] = argv[i+1];
for (int i = pos; i + 1 < argc; ++i) {
argv[i] = argv[i + 1];
}
argv[--argc] = nullptr;
}
test/mavsdk_tests/test_mission_multicopter.cpp
+32 -4
View File
@@ -1,7 +1,35 @@
//
// Multicopter mission test.
//
// Author: Julian Oes <julian@oes.ch>
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
test/mavsdk_tests/test_mission_vtol.cpp
+32 -4
View File
@@ -1,7 +1,35 @@
//
// VTOL mission test.
//
// Author: Lorenz Meier <lorenz@px4.io>
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>
test/mavsdk_tests/test_multicopter_offboard.cpp
+32 -4
View File
@@ -1,7 +1,35 @@
//
// Multicopter mission test.
//
// Author: Julian Oes <julian@oes.ch>
/****************************************************************************
*
* Copyright (c) 2020 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
#include <mavsdk/mavsdk.h>
#include <mavsdk/plugins/action/action.h>