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 💄
2026-10-31 02:21:36 +08:00 · 2022-06-16 16:14:57 -04:00
parent 285556e463
commit de1fa11e96
32 changed files with 2121 additions and 713 deletions
@@ -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);
+}