mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-20 16:37:35 +08:00
New follow-me flight task
rename follow_me_status to follow_target_status
enable follow_target_estimator on skynode
implement the responsiveness parameter:
The responsiveness parameter should behave similarly to the previous
follow-me implementation in navigator. The difference here is that
there are now two separate gains for position and velocity fusion.
The previous implemenation in navigator had no velocity fusion.
Allow follow-me to be flown without RC
SITL tests for follow-me flight task
This includes:
- Testing the setting for the follow-me angle
- Testing that streaming position only or position
and velocity measurements both work
- Testing that RC override works
Most of these tests are done with a simulated model
of a point object that moves on a straight line. So
nothing too spectacular. But it makes the test checks
much easier.
Since the estimator for the target actually checks new
measurements and compares them to old ones, I also added
random gausian noise to the measurements with a fixed seed
for deterministic randomness. So repeated runs produce
exactly the same results over and over.
Half of the angles are still missing in MAVSDK. Need to create
an upstream PR to add center left/right and rear left/right options.
These and the corresponding SITL tests need to be implemented
later.
sitl: Increase position tolerance during follow-me
Astro seems to barely exceed the current tolerance (4.3 !< 4.0)
causing CI to fail. The point of the CI test is not to check
the accuracy of the flight behaviour, but only the fact that the
drone is doing the expected thing. So the exact value of this
tolerance is not really important.
follow-me: gimbal control in follow-me
follow-me: create sub-routines in flight task class
follow-me: use ground-dist for emergency ascent
dist_bottom is only defined when a ground facing distance sensor exist.
It's therefore better to use dist_ground instead, which has the distance
to the home altitude if no distance sensor is available.
As a consequence it will only be possible to use follow-me in a valley
when the drone has a distance sensor.
follow-me: point gimbal to the ground in 2D mode
follow-me: another fuzzy msg handling for the estimator
follow-me: bugfix in acceleration saturation limit
follow-me: parameter for filter delay compensation
mantis: dont use flow for terrain estimation
follow-me: default responsiveness 0.5 -> 0.1
0.5 is way too jerky in real and simulated tests.
flight_task: clarify comments for bottom distance
follow-me: minor comment improvement
follow-me: [debug] log emergency_ascent
follow-me: [debug] log gimbal pitch
follow-me: [debug] status values for follow-me estimator
follow-me: setting for gimbal tracking mode
follow-me: release gimbal control at destruction
mavsdk: cosmetics 💄
This commit is contained in:
Alessandro Simovic
committed by
Daniel Agar
Daniel Agar
parent
285556e463
commit
de1fa11e96
@@ -17,6 +17,7 @@ if(MAVSDK_FOUND)
|
||||
test_main.cpp
|
||||
autopilot_tester.cpp
|
||||
autopilot_tester_failure.cpp
|
||||
autopilot_tester_follow_me.cpp
|
||||
test_multicopter_control_allocation.cpp
|
||||
test_multicopter_failure_injection.cpp
|
||||
test_multicopter_failsafe.cpp
|
||||
@@ -24,6 +25,7 @@ if(MAVSDK_FOUND)
|
||||
test_multicopter_offboard.cpp
|
||||
test_multicopter_manual.cpp
|
||||
test_vtol_mission.cpp
|
||||
test_multicopter_follow_me.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(mavsdk_tests
|
||||
|
||||
@@ -37,6 +37,7 @@
|
||||
#include <future>
|
||||
#include <thread>
|
||||
#include <unistd.h>
|
||||
#include <cmath>
|
||||
|
||||
std::string connection_url {"udp://"};
|
||||
std::optional<float> speed_factor {std::nullopt};
|
||||
@@ -64,7 +65,7 @@ void AutopilotTester::connect(const std::string uri)
|
||||
REQUIRE(poll_condition_with_timeout(
|
||||
[this]() { return _mavsdk.systems().size() > 0; }, std::chrono::seconds(25)));
|
||||
|
||||
auto system = _mavsdk.systems().at(0);
|
||||
auto system = get_system();
|
||||
|
||||
_action.reset(new Action(system));
|
||||
_failure.reset(new Failure(system));
|
||||
@@ -534,6 +535,11 @@ void AutopilotTester::check_tracks_mission(float corridor_radius_m)
|
||||
});
|
||||
}
|
||||
|
||||
void AutopilotTester::check_current_altitude(float target_rel_altitude_m, float max_distance_m)
|
||||
{
|
||||
CHECK(std::abs(_telemetry->position().relative_altitude_m - target_rel_altitude_m) <= max_distance_m);
|
||||
}
|
||||
|
||||
std::array<float, 3> AutopilotTester::get_current_position_ned()
|
||||
{
|
||||
mavsdk::Telemetry::PositionVelocityNed position_velocity_ned = _telemetry->position_velocity_ned();
|
||||
|
||||
@@ -135,6 +135,7 @@ public:
|
||||
void check_tracks_mission(float corridor_radius_m = 1.5f);
|
||||
void start_checking_altitude(const float max_deviation_m);
|
||||
void stop_checking_altitude();
|
||||
void check_current_altitude(float target_rel_altitude_m, float max_distance_m = 1.5f);
|
||||
|
||||
// Blocking call to get the drone's current position in NED frame
|
||||
std::array<float, 3> get_current_position_ned();
|
||||
@@ -147,7 +148,41 @@ public:
|
||||
protected:
|
||||
mavsdk::Param *getParams() const { return _param.get();}
|
||||
mavsdk::Telemetry *getTelemetry() const { return _telemetry.get();}
|
||||
mavsdk::ManualControl *getManualControl() const { return _manual_control.get();}
|
||||
std::shared_ptr<System> get_system() { return _mavsdk.systems().at(0);}
|
||||
Telemetry::GroundTruth getHome()
|
||||
{
|
||||
// Check if home was stored before it is accessed
|
||||
CHECK(_home.absolute_altitude_m != NAN);
|
||||
CHECK(_home.latitude_deg != NAN);
|
||||
CHECK(_home.longitude_deg != NAN);
|
||||
return _home;
|
||||
}
|
||||
|
||||
template<typename Rep, typename Period>
|
||||
void sleep_for(std::chrono::duration<Rep, Period> duration)
|
||||
{
|
||||
const std::chrono::microseconds duration_us(duration);
|
||||
|
||||
if (_telemetry && _telemetry->attitude_quaternion().timestamp_us != 0) {
|
||||
|
||||
const int64_t start_time_us = _telemetry->attitude_quaternion().timestamp_us;
|
||||
|
||||
while (true) {
|
||||
// Hopefully this is often enough not to have PX4 time out on us.
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
|
||||
const int64_t elapsed_time_us = _telemetry->attitude_quaternion().timestamp_us - start_time_us;
|
||||
|
||||
if (elapsed_time_us > duration_us.count()) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
std::this_thread::sleep_for(duration);
|
||||
}
|
||||
}
|
||||
|
||||
private:
|
||||
mavsdk::geometry::CoordinateTransformation get_coordinate_transformation();
|
||||
@@ -218,30 +253,7 @@ private:
|
||||
return true;
|
||||
}
|
||||
|
||||
template<typename Rep, typename Period>
|
||||
void sleep_for(std::chrono::duration<Rep, Period> duration)
|
||||
{
|
||||
const std::chrono::microseconds duration_us(duration);
|
||||
|
||||
if (_telemetry && _telemetry->attitude_quaternion().timestamp_us != 0) {
|
||||
|
||||
const int64_t start_time_us = _telemetry->attitude_quaternion().timestamp_us;
|
||||
|
||||
while (true) {
|
||||
// Hopefully this is often enough not to have PX4 time out on us.
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
|
||||
const int64_t elapsed_time_us = _telemetry->attitude_quaternion().timestamp_us - start_time_us;
|
||||
|
||||
if (elapsed_time_us > duration_us.count()) {
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
std::this_thread::sleep_for(duration);
|
||||
}
|
||||
}
|
||||
|
||||
mavsdk::Mavsdk _mavsdk{};
|
||||
std::unique_ptr<mavsdk::Action> _action{};
|
||||
|
||||
@@ -0,0 +1,453 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 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_follow_me.h"
|
||||
|
||||
// #include <mavsdk/plugins/follow_me/follow_me.h>
|
||||
|
||||
#include "math_helpers.h"
|
||||
#include <iostream>
|
||||
#include <future>
|
||||
#include <thread>
|
||||
#include <unistd.h>
|
||||
#include <cmath>
|
||||
#include <random>
|
||||
|
||||
|
||||
FollowTargetSimulator::FollowTargetSimulator(std::array<float, 3> initial_position_ned,
|
||||
mavsdk::Telemetry::GroundTruth home) :
|
||||
_position_ned(initial_position_ned), _home(home)
|
||||
{
|
||||
_velocity_ned[0] = 0.0f;
|
||||
_velocity_ned[1] = 0.0f;
|
||||
_velocity_ned[2] = 0.0f;
|
||||
}
|
||||
|
||||
FollowTargetSimulator::~FollowTargetSimulator()
|
||||
{
|
||||
|
||||
}
|
||||
|
||||
void FollowTargetSimulator::update(float delta_t_s)
|
||||
{
|
||||
const float velocity_m_s = 2.0;
|
||||
|
||||
_velocity_ned[0] = velocity_m_s;
|
||||
_velocity_ned[1] = 0.0;
|
||||
|
||||
_position_ned[0] += _velocity_ned[0] * delta_t_s;
|
||||
_position_ned[1] += _velocity_ned[1] * delta_t_s;
|
||||
_position_ned[2] += _velocity_ned[2] * delta_t_s;
|
||||
|
||||
_udpate_count++;
|
||||
}
|
||||
|
||||
std::array<double, 3> FollowTargetSimulator::get_position_global(bool add_noise)
|
||||
{
|
||||
std::array<float, 3> pos_ned = _position_ned;
|
||||
|
||||
if (add_noise) {
|
||||
unsigned seed = _udpate_count;
|
||||
std::default_random_engine generator(seed);
|
||||
std::normal_distribution<double> distribution(0.0, 1.0);
|
||||
pos_ned[0] += distribution(generator);
|
||||
pos_ned[1] += distribution(generator);
|
||||
pos_ned[2] += distribution(generator);
|
||||
}
|
||||
|
||||
CHECK(std::isfinite(_home.latitude_deg));
|
||||
CHECK(std::isfinite(_home.longitude_deg));
|
||||
const auto ct = CoordinateTransformation({_home.latitude_deg, _home.longitude_deg});
|
||||
|
||||
mavsdk::geometry::CoordinateTransformation::LocalCoordinate local_coordinate{pos_ned[0], pos_ned[1]};
|
||||
mavsdk::geometry::CoordinateTransformation::GlobalCoordinate global_coordinate = ct.global_from_local(local_coordinate);
|
||||
std::array<double, 3> global_pos{global_coordinate.latitude_deg, global_coordinate.longitude_deg, pos_ned[2] + _home.absolute_altitude_m};
|
||||
return global_pos;
|
||||
}
|
||||
|
||||
std::array<float, 3> FollowTargetSimulator::get_position_ned(bool add_noise)
|
||||
{
|
||||
|
||||
std::array<float, 3> pos_ned = _position_ned;
|
||||
|
||||
if (add_noise) {
|
||||
unsigned seed = _udpate_count;
|
||||
std::default_random_engine generator(seed);
|
||||
std::normal_distribution<double> distribution(0.0, pos_noise_std);
|
||||
pos_ned[0] += distribution(generator);
|
||||
pos_ned[1] += distribution(generator);
|
||||
pos_ned[2] += distribution(generator);
|
||||
}
|
||||
|
||||
return pos_ned;
|
||||
}
|
||||
|
||||
std::array<float, 3> FollowTargetSimulator::get_velocity_ned_noisy()
|
||||
{
|
||||
return get_velocity_ned(true);
|
||||
}
|
||||
|
||||
std::array<float, 3> FollowTargetSimulator::get_velocity_ned_ground_truth()
|
||||
{
|
||||
return get_velocity_ned(false);
|
||||
}
|
||||
|
||||
std::array<float, 3> FollowTargetSimulator::get_velocity_ned(bool add_noise)
|
||||
{
|
||||
std::array<float, 3> vel_ned = _velocity_ned;
|
||||
|
||||
if (add_noise) {
|
||||
unsigned seed = _udpate_count;
|
||||
std::default_random_engine generator(seed);
|
||||
std::normal_distribution<double> distribution(0.0, vel_noise_std);
|
||||
|
||||
vel_ned[0] += distribution(generator);
|
||||
vel_ned[1] += distribution(generator);
|
||||
vel_ned[2] += distribution(generator);
|
||||
}
|
||||
|
||||
return vel_ned;
|
||||
}
|
||||
|
||||
std::array<float, 3> FollowTargetSimulator::get_position_ned_noisy()
|
||||
{
|
||||
return get_position_ned(true);
|
||||
}
|
||||
|
||||
std::array<float, 3> FollowTargetSimulator::get_position_ground_truth_ned()
|
||||
{
|
||||
return get_position_ned(false);
|
||||
}
|
||||
|
||||
std::array<double, 3> FollowTargetSimulator::get_position_global_noisy()
|
||||
{
|
||||
return get_position_global(true);
|
||||
}
|
||||
|
||||
std::array<double, 3> FollowTargetSimulator::get_position_global_ground_truth()
|
||||
{
|
||||
return get_position_global(false);
|
||||
}
|
||||
|
||||
void FollowTargetSimulator::check_follow_angle(FollowMe::Config config, std::array<float, 3> drone_pos_ned,
|
||||
std::array<float, 3> target_pos_ned, float tolerance)
|
||||
{
|
||||
// This check assumes that the target is travelling straight on the x-axis
|
||||
const float x_dist_to_target = target_pos_ned[0] - drone_pos_ned[0];
|
||||
const float y_dist_to_target = target_pos_ned[1] - drone_pos_ned[1];
|
||||
|
||||
switch (config.follow_direction) {
|
||||
case FollowMe::Config::FollowDirection::None:
|
||||
CHECK(x_dist_to_target < tolerance);
|
||||
CHECK(x_dist_to_target > -tolerance);
|
||||
CHECK(y_dist_to_target < tolerance);
|
||||
CHECK(y_dist_to_target > -tolerance);
|
||||
break;
|
||||
|
||||
case FollowMe::Config::FollowDirection::Behind:
|
||||
CHECK(drone_pos_ned[0] < target_pos_ned[0]);
|
||||
CHECK(y_dist_to_target < tolerance);
|
||||
CHECK(y_dist_to_target > -tolerance);
|
||||
break;
|
||||
|
||||
case FollowMe::Config::FollowDirection::Front:
|
||||
CHECK(drone_pos_ned[0] > target_pos_ned[0]);
|
||||
CHECK(y_dist_to_target < tolerance);
|
||||
CHECK(y_dist_to_target > -tolerance);
|
||||
break;
|
||||
|
||||
case FollowMe::Config::FollowDirection::FrontRight:
|
||||
CHECK(drone_pos_ned[0] > target_pos_ned[0]);
|
||||
CHECK(drone_pos_ned[1] > target_pos_ned[1]);
|
||||
break;
|
||||
|
||||
case FollowMe::Config::FollowDirection::FrontLeft:
|
||||
CHECK(drone_pos_ned[0] > target_pos_ned[0]);
|
||||
CHECK(drone_pos_ned[1] < target_pos_ned[1]);
|
||||
break;
|
||||
|
||||
default:
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
void AutopilotTesterFollowMe::connect(const std::string uri)
|
||||
{
|
||||
AutopilotTester::connect(uri);
|
||||
|
||||
auto system = get_system();
|
||||
_follow_me.reset(new FollowMe(system));
|
||||
}
|
||||
|
||||
|
||||
void AutopilotTesterFollowMe::straight_line_test(const float altitude_m, const bool stream_velocity)
|
||||
{
|
||||
const unsigned location_update_rate = 1;
|
||||
const float position_tolerance = 6.0f;
|
||||
|
||||
// Start with simulated target on the same plane as drone's home position
|
||||
std::array<float, 3> start_location_ned = get_current_position_ned();
|
||||
FollowTargetSimulator target_simulator(start_location_ned, getHome());
|
||||
|
||||
// Configure Min height of the drone to be "20 meters" above home & Follow direction as "Front
|
||||
// right".
|
||||
FollowMe::Config config;
|
||||
config.min_height_m = altitude_m;
|
||||
config.follow_distance_m = 8.0f;
|
||||
|
||||
// Allow some time for mode switch
|
||||
sleep_for(std::chrono::milliseconds(1000));
|
||||
|
||||
// Start Follow Me
|
||||
CHECK(FollowMe::Result::Success == _follow_me->start());
|
||||
|
||||
// Allow some time for mode switch
|
||||
sleep_for(std::chrono::milliseconds(1000));
|
||||
|
||||
// task loop
|
||||
bool target_moving = false;
|
||||
bool perform_checks = false;
|
||||
|
||||
for (unsigned i = 0; i < 75 * location_update_rate; ++i) {
|
||||
std::array<float, 3> target_pos_ned_ground_truth = target_simulator.get_position_ground_truth_ned();
|
||||
std::array<float, 3> position_ned = get_current_position_ned();
|
||||
const float distance_to_target = norm(diff(target_pos_ned_ground_truth, position_ned));
|
||||
|
||||
// poor-man's state machine
|
||||
if (i < 5) {
|
||||
// Stream target location without moving
|
||||
|
||||
} else if (i == 5) {
|
||||
// Change config
|
||||
perform_checks = false;
|
||||
config.follow_direction = FollowMe::Config::FollowDirection::Behind;
|
||||
CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
|
||||
|
||||
} else if (i > 5 && i < 15) {
|
||||
// Move target and wait for steady state of drone
|
||||
target_moving = true;
|
||||
|
||||
} else if (i >= 15 && i < 20) {
|
||||
// Perform positional checks in steady state
|
||||
perform_checks = true;
|
||||
|
||||
} else if (i == 20) {
|
||||
// Change config
|
||||
perform_checks = false;
|
||||
config.follow_direction = FollowMe::Config::FollowDirection::Front;
|
||||
CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
|
||||
|
||||
} else if (i > 20 && i < 30) {
|
||||
// Move target and wait for steady state of drone
|
||||
|
||||
} else if (i >= 30 && i < 35) {
|
||||
// Perform positional checks in steady state
|
||||
perform_checks = true;
|
||||
|
||||
} else if (i == 35) {
|
||||
// Change config
|
||||
perform_checks = false;
|
||||
config.follow_direction = FollowMe::Config::FollowDirection::FrontRight;
|
||||
CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
|
||||
|
||||
} else if (i > 35 && i < 45) {
|
||||
// Move target and wait for steady state of drone
|
||||
|
||||
|
||||
} else if (i >= 45 && i < 55) {
|
||||
// Perform positional checks in steady state
|
||||
perform_checks = true;
|
||||
|
||||
} else if (i == 55) {
|
||||
// Change config
|
||||
perform_checks = false;
|
||||
config.follow_direction = FollowMe::Config::FollowDirection::FrontLeft;
|
||||
CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
|
||||
|
||||
} else if (i > 55 && i < 65) {
|
||||
// Move target and wait for steady state of drone
|
||||
|
||||
} else if (i >= 65 && i < 75) {
|
||||
// Perform positional checks in steady state
|
||||
perform_checks = true;
|
||||
}
|
||||
|
||||
if (target_moving) {
|
||||
target_simulator.update(1.0f / location_update_rate);
|
||||
}
|
||||
|
||||
if (perform_checks) {
|
||||
check_current_altitude(altitude_m);
|
||||
CHECK(distance_to_target <= config.follow_distance_m + position_tolerance);
|
||||
CHECK(distance_to_target >= config.follow_distance_m - position_tolerance);
|
||||
target_simulator.check_follow_angle(config, position_ned, target_pos_ned_ground_truth, position_tolerance);
|
||||
}
|
||||
|
||||
// Construct follow-me message
|
||||
std::array<double, 3> global_coordinate = target_simulator.get_position_global_noisy();
|
||||
|
||||
FollowMe::TargetLocation target_location{};
|
||||
target_location.latitude_deg = global_coordinate[0];
|
||||
target_location.longitude_deg = global_coordinate[1];
|
||||
target_location.absolute_altitude_m = global_coordinate[2];
|
||||
|
||||
if (stream_velocity) {
|
||||
std::array<float, 3> target_vel_ned = target_simulator.get_velocity_ned_noisy();
|
||||
target_location.velocity_x_m_s = target_vel_ned[0];
|
||||
target_location.velocity_y_m_s = target_vel_ned[1];
|
||||
target_location.velocity_z_m_s = target_vel_ned[2];
|
||||
|
||||
} else {
|
||||
target_location.velocity_x_m_s = NAN;
|
||||
target_location.velocity_y_m_s = NAN;
|
||||
target_location.velocity_z_m_s = NAN;
|
||||
}
|
||||
|
||||
|
||||
// Send message and check result
|
||||
CHECK(FollowMe::Result::Success == _follow_me->set_target_location(target_location));
|
||||
|
||||
sleep_for(std::chrono::milliseconds(1000 / location_update_rate));
|
||||
}
|
||||
|
||||
CHECK(FollowMe::Result::Success == _follow_me->stop());
|
||||
}
|
||||
|
||||
void AutopilotTesterFollowMe::stream_velocity_only()
|
||||
{
|
||||
const unsigned loop_update_rate = 1;
|
||||
const float position_tolerance = 4.0f;
|
||||
|
||||
// Configure follow-me
|
||||
FollowMe::Config config;
|
||||
config.follow_direction = FollowMe::Config::FollowDirection::Behind;
|
||||
CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
|
||||
|
||||
// Allow some time for mode switch
|
||||
sleep_for(std::chrono::milliseconds(1000));
|
||||
|
||||
// Start Follow Me
|
||||
CHECK(FollowMe::Result::Success == _follow_me->start());
|
||||
|
||||
// Allow some time for mode switch
|
||||
sleep_for(std::chrono::milliseconds(1000));
|
||||
|
||||
std::array<float, 3> drone_initial_pos = get_current_position_ned();
|
||||
|
||||
// Start streaming velocity only. The drone should not move.
|
||||
for (unsigned i = 0; i < 15 * loop_update_rate; i++) {
|
||||
FollowMe::TargetLocation target_location{};
|
||||
target_location.latitude_deg = NAN;
|
||||
target_location.longitude_deg = NAN;
|
||||
target_location.absolute_altitude_m = NAN;
|
||||
target_location.velocity_x_m_s = 2.0f;
|
||||
target_location.velocity_y_m_s = 1.0f;
|
||||
target_location.velocity_z_m_s = 0.5f;
|
||||
|
||||
// Send message and check result
|
||||
CHECK(FollowMe::Result::Success == _follow_me->set_target_location(target_location));
|
||||
|
||||
sleep_for(std::chrono::milliseconds(1000 / loop_update_rate));
|
||||
}
|
||||
|
||||
// Check that drone is still close to initial position and has not moved much
|
||||
std::array<float, 3> drone_final_pos = get_current_position_ned();
|
||||
const float distance_travelled = norm(diff(drone_initial_pos, drone_final_pos));
|
||||
CHECK(distance_travelled < position_tolerance);
|
||||
}
|
||||
|
||||
void AutopilotTesterFollowMe::rc_override_test(const float altitude_m)
|
||||
{
|
||||
const unsigned loop_update_rate = 50;
|
||||
const float position_tolerance = 4.0f;
|
||||
|
||||
// Start with simulated target on the same plane as drone's home position
|
||||
std::array<float, 3> start_location_ned = get_current_position_ned();
|
||||
FollowTargetSimulator target_simulator(start_location_ned, getHome());
|
||||
|
||||
// Configure Min height of the drone to be "20 meters" above home & Follow direction as "Front
|
||||
// right".
|
||||
FollowMe::Config config;
|
||||
config.min_height_m = altitude_m;
|
||||
config.follow_distance_m = 8.0f;
|
||||
config.follow_direction = FollowMe::Config::FollowDirection::Behind;
|
||||
CHECK(FollowMe::Result::Success == _follow_me->set_config(config));
|
||||
|
||||
// task loop
|
||||
std::array<float, 3> drone_initial_pos;
|
||||
|
||||
for (unsigned i = 0; i <= 30 * loop_update_rate; ++i) {
|
||||
// Start streaming target data after x seconds to provide RC before switching to the flight task
|
||||
bool stream_follow_me_data = (i > 7 * loop_update_rate);
|
||||
|
||||
// Deflect a stick for a short time only
|
||||
bool deflect_rc_sticks = (i > 10 * loop_update_rate && i <= 11 * loop_update_rate);
|
||||
|
||||
// Switch to follow-me at this instant
|
||||
if (i == 5 * loop_update_rate) {
|
||||
// Start Follow Me
|
||||
CHECK(FollowMe::Result::Success == _follow_me->start());
|
||||
}
|
||||
|
||||
// After approximately 10 seconds we would expect the drone to have stopped because of the RC stick input
|
||||
if (i == 20 * loop_update_rate) {
|
||||
drone_initial_pos = get_current_position_ned();
|
||||
}
|
||||
|
||||
if (stream_follow_me_data) {
|
||||
target_simulator.update(1.0f / loop_update_rate);
|
||||
std::array<double, 3> global_coordinate = target_simulator.get_position_global_noisy();
|
||||
FollowMe::TargetLocation target_location{};
|
||||
target_location.latitude_deg = global_coordinate[0];
|
||||
target_location.longitude_deg = global_coordinate[1];
|
||||
target_location.absolute_altitude_m = global_coordinate[2];
|
||||
target_location.velocity_x_m_s = NAN;
|
||||
target_location.velocity_y_m_s = NAN;
|
||||
target_location.velocity_z_m_s = NAN;
|
||||
_follow_me->set_target_location(target_location);
|
||||
}
|
||||
|
||||
if (deflect_rc_sticks) {
|
||||
CHECK(getManualControl()->set_manual_control_input(1.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
|
||||
|
||||
} else {
|
||||
CHECK(getManualControl()->set_manual_control_input(0.f, 0.f, 0.5f, 0.f) == ManualControl::Result::Success);
|
||||
}
|
||||
|
||||
sleep_for(std::chrono::milliseconds(1000 / loop_update_rate));
|
||||
}
|
||||
|
||||
std::array<float, 3> drone_final_pos = get_current_position_ned();
|
||||
const float distance_travelled = norm(diff(drone_initial_pos, drone_final_pos));
|
||||
CHECK(distance_travelled < position_tolerance);
|
||||
}
|
||||
@@ -0,0 +1,121 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 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 "autopilot_tester.h"
|
||||
|
||||
#include <mavsdk/mavsdk.h>
|
||||
#include <mavsdk/plugins/action/action.h>
|
||||
#include <mavsdk/plugins/follow_me/follow_me.h>
|
||||
|
||||
|
||||
// Simulated a target moving on a straight line
|
||||
class FollowTargetSimulator
|
||||
{
|
||||
public:
|
||||
FollowTargetSimulator(std::array<float, 3> initial_position_ned, mavsdk::Telemetry::GroundTruth home);
|
||||
~FollowTargetSimulator();
|
||||
|
||||
// Integrate simulator by one time step
|
||||
// This moves the target on a line
|
||||
void update(float delta_t_s);
|
||||
|
||||
// Retrieve noisy version of position state in NED coordinate frame
|
||||
// The noise is deterministic and changes whenever the update() function is called
|
||||
std::array<float, 3> get_position_ned_noisy();
|
||||
|
||||
// Retrieve ground truth of position state in NED coordinate frame
|
||||
std::array<float, 3> get_position_ground_truth_ned();
|
||||
|
||||
// Retrieve noisy version of velocity state in NED coordinate frame
|
||||
// The noise is deterministic and changes whenever the update() function is called
|
||||
std::array<float, 3> get_velocity_ned_noisy();
|
||||
|
||||
// Retrieve ground truth of velocity state in NED coordinate frame
|
||||
std::array<float, 3> get_velocity_ned_ground_truth();
|
||||
|
||||
// Retrieve noisy version of position state in global coordinate frame (lat, lon, alt)
|
||||
// The noise is deterministic and changes whenever the update() function is called
|
||||
std::array<double, 3> get_position_global_noisy();
|
||||
|
||||
// Retrieve ground truth of position state in global coordinate frame (lat, lon, alt)
|
||||
std::array<double, 3> get_position_global_ground_truth();
|
||||
|
||||
// Run checks whether the drone has the correct angle towards the target, specified by the follow-me configuration
|
||||
void check_follow_angle(FollowMe::Config config, std::array<float, 3> drone_pos_ned,
|
||||
std::array<float, 3> target_pos_ned, float tolerance);
|
||||
|
||||
private:
|
||||
// Retrieve estimate with the option to add deterministic gaussian noise
|
||||
//
|
||||
// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
|
||||
std::array<double, 3> get_position_global(bool add_noise);
|
||||
|
||||
// Retrieve estimate with the option to add deterministic gaussian noise
|
||||
//
|
||||
// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
|
||||
std::array<float, 3> get_position_ned(bool add_noise);
|
||||
|
||||
// Retrieve estimate with the option to add deterministic gaussian noise
|
||||
//
|
||||
// @param add_noise: Add gaussian noise to the state. Noise is deterministic and changes with each inokation of update()
|
||||
std::array<float, 3> get_velocity_ned(bool add_noise);
|
||||
|
||||
std::array<float, 3> _position_ned;
|
||||
std::array<float, 3> _velocity_ned;
|
||||
mavsdk::Telemetry::GroundTruth _home;
|
||||
size_t _udpate_count = 0;
|
||||
|
||||
const double pos_noise_std = 0.3;
|
||||
const double vel_noise_std = 0.1;
|
||||
|
||||
std::unique_ptr<mavsdk::Telemetry> _telemetry{};
|
||||
};
|
||||
|
||||
class AutopilotTesterFollowMe : public AutopilotTester
|
||||
{
|
||||
public:
|
||||
AutopilotTesterFollowMe() = default;
|
||||
~AutopilotTesterFollowMe() = default;
|
||||
void connect(const std::string uri);
|
||||
|
||||
void straight_line_test(const float altitude_m, const bool stream_velocity);
|
||||
|
||||
void stream_velocity_only();
|
||||
|
||||
void rc_override_test(const float altitude_m);
|
||||
|
||||
private:
|
||||
std::unique_ptr<mavsdk::FollowMe> _follow_me{};
|
||||
};
|
||||
@@ -0,0 +1,129 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2021 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 <chrono>
|
||||
|
||||
#include "autopilot_tester_follow_me.h"
|
||||
|
||||
|
||||
|
||||
TEST_CASE("Follow target - Position streaming", "[multicopter]")
|
||||
{
|
||||
const bool stream_velocity = false;
|
||||
AutopilotTesterFollowMe tester;
|
||||
tester.connect(connection_url);
|
||||
tester.wait_until_ready();
|
||||
tester.store_home();
|
||||
tester.arm();
|
||||
|
||||
const float takeoff_altitude = 10.0;
|
||||
tester.set_takeoff_altitude(takeoff_altitude);
|
||||
tester.takeoff();
|
||||
tester.wait_until_hovering();
|
||||
|
||||
tester.straight_line_test(takeoff_altitude, stream_velocity);
|
||||
|
||||
tester.land();
|
||||
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
|
||||
tester.wait_until_disarmed(until_disarmed_timeout);
|
||||
}
|
||||
|
||||
TEST_CASE("Follow target - Position and velocity streaming", "[multicopter]")
|
||||
{
|
||||
const bool stream_velocity = true;
|
||||
AutopilotTesterFollowMe tester;
|
||||
tester.connect(connection_url);
|
||||
tester.wait_until_ready();
|
||||
tester.store_home();
|
||||
tester.arm();
|
||||
|
||||
const float takeoff_altitude = 10.0;
|
||||
tester.set_takeoff_altitude(takeoff_altitude);
|
||||
tester.takeoff();
|
||||
tester.wait_until_hovering();
|
||||
|
||||
tester.straight_line_test(takeoff_altitude, stream_velocity);
|
||||
|
||||
tester.land();
|
||||
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
|
||||
tester.wait_until_disarmed(until_disarmed_timeout);
|
||||
}
|
||||
|
||||
TEST_CASE("Follow target - Velocity streaming only", "[multicopter]")
|
||||
{
|
||||
// Streaming only velocity should not work, the drone should not move.
|
||||
// There needs to be at least one position message for follow-me
|
||||
// to be able to integrate velocity.
|
||||
AutopilotTesterFollowMe tester;
|
||||
tester.connect(connection_url);
|
||||
tester.wait_until_ready();
|
||||
tester.store_home();
|
||||
tester.arm();
|
||||
|
||||
const float takeoff_altitude = 10.0;
|
||||
tester.set_takeoff_altitude(takeoff_altitude);
|
||||
tester.takeoff();
|
||||
tester.wait_until_hovering();
|
||||
|
||||
tester.stream_velocity_only();
|
||||
|
||||
tester.land();
|
||||
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
|
||||
tester.wait_until_disarmed(until_disarmed_timeout);
|
||||
tester.check_home_within(1.0f);
|
||||
}
|
||||
|
||||
TEST_CASE("Follow target - Manual takeover", "[multicopter]")
|
||||
{
|
||||
AutopilotTesterFollowMe tester;
|
||||
tester.connect(connection_url);
|
||||
tester.wait_until_ready();
|
||||
tester.store_home();
|
||||
tester.arm();
|
||||
|
||||
const float takeoff_altitude = 10.0;
|
||||
tester.set_takeoff_altitude(takeoff_altitude);
|
||||
tester.takeoff();
|
||||
tester.wait_until_hovering();
|
||||
|
||||
tester.rc_override_test(takeoff_altitude);
|
||||
|
||||
tester.land();
|
||||
std::chrono::seconds until_disarmed_timeout = std::chrono::seconds(300);
|
||||
tester.wait_until_disarmed(until_disarmed_timeout);
|
||||
}
|
||||
|
||||
TEST_CASE("Follow target - Spamming duplicate messages", "[multicopter]")
|
||||
{
|
||||
|
||||
}
|
||||
Reference in New Issue
Block a user