diff --git a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp index 645ab439dde1..6de157339f26 100644 --- a/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp +++ b/src/lib/FlightTasks/tasks/FlightTask/FlightTask.hpp @@ -157,12 +157,6 @@ class FlightTask : public ModuleParams */ static const landing_gear_s empty_landing_gear_default_keep; - /** - * Empty desired waypoints. - * All waypoints are set to NAN. - */ - static const vehicle_trajectory_waypoint_s empty_trajectory_waypoint; - /** * Call this whenever a parameter update notification is received (parameter_update uORB message) */ diff --git a/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt b/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt index 428053010f81..14cf505b344e 100644 --- a/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt +++ b/src/lib/FlightTasks/tasks/Utility/CMakeLists.txt @@ -39,7 +39,8 @@ px4_add_library(FlightTaskUtility VelocitySmoothing.cpp ) -target_link_libraries(FlightTaskUtility PUBLIC FlightTask) +target_link_libraries(FlightTaskUtility PUBLIC FlightTask hysteresis) target_include_directories(FlightTaskUtility PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) px4_add_unit_gtest(SRC VelocitySmoothingTest.cpp LINKLIBS FlightTaskUtility) +px4_add_functional_gtest(SRC ObstacleAvoidanceTest.cpp LINKLIBS FlightTaskUtility) diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp index b499d1f66e79..eafcd6f6ed9b 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp @@ -46,15 +46,6 @@ static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; static constexpr uint64_t TIME_BEFORE_FAILSAFE = 500_ms; static constexpr uint64_t Z_PROGRESS_TIMEOUT_US = 2_s; -const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0}, - { {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, - {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}} - } -}; - ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) : ModuleParams(parent) { diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp index df0a9279f4b9..241575750bd2 100644 --- a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp @@ -60,6 +60,15 @@ #include +const vehicle_trajectory_waypoint_s empty_trajectory_waypoint = {0, 0, {0, 0, 0, 0, 0, 0, 0}, + { {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, + {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, + {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, + {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}}, + {0, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, NAN, NAN, false, UINT8_MAX, {0, 0}} + } +}; + class ObstacleAvoidance : public ModuleParams { public: @@ -108,15 +117,11 @@ class ObstacleAvoidance : public ModuleParams void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, float target_acceptance_radius, const matrix::Vector2f &closest_pt); -private: +protected: uORB::SubscriptionPollable *_sub_vehicle_trajectory_waypoint{nullptr}; /**< vehicle trajectory waypoint subscription */ uORB::SubscriptionPollable *_sub_vehicle_status{nullptr}; /**< vehicle status subscription */ - DEFINE_PARAMETERS( - (ParamFloat) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */ - ); - vehicle_trajectory_waypoint_s _desired_waypoint{}; /**< desired vehicle trajectory waypoint to be sent to OA */ uORB::Publication _pub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; /**< trajectory waypoint desired publication */ @@ -139,4 +144,8 @@ class ObstacleAvoidance : public ModuleParams */ void _publishVehicleCmdDoLoiter(); + DEFINE_PARAMETERS( + (ParamFloat) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */ + ); + }; diff --git a/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp new file mode 100644 index 000000000000..9f25375ec9ae --- /dev/null +++ b/src/lib/FlightTasks/tasks/Utility/ObstacleAvoidanceTest.cpp @@ -0,0 +1,207 @@ +/**************************************************************************** + * + * Copyright (C) 2019 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 +#include +#include + + +using namespace matrix; +// to run: make tests TESTFILTER=CollisionPrevention + +class ObstacleAvoidanceTest : public ::testing::Test +{ +public: + SubscriptionArray subscription_array; + Vector3f pos_sp; + Vector3f vel_sp; + float yaw_sp = 0.123f; + float yaw_speed_sp = NAN; + void SetUp() override + + { + param_reset_all(); + pos_sp = Vector3f(1.f, 1.2f, 0.1f); + vel_sp = Vector3f(0.3f, 0.4f, 0.1f); + } +}; + +class TestObstacleAvoidance : public ::ObstacleAvoidance +{ +public: + TestObstacleAvoidance() : ObstacleAvoidance(nullptr) {} + void paramsChanged() {ObstacleAvoidance::updateParamsImpl();} + void test_setPosition(Vector3f &pos) {_position = pos;} +}; + +TEST_F(ObstacleAvoidanceTest, instantiation) { ObstacleAvoidance oa(nullptr); } + +TEST_F(ObstacleAvoidanceTest, oa_enabled_healthy) +{ + // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message coming + // from offboard + TestObstacleAvoidance oa; + oa.initializeSubscriptions(subscription_array); + + vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; + message.timestamp = hrt_absolute_time(); + message.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; + message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0] = 2.6f; + message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1] = 2.4f; + message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2] = 2.7f; + message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw = 0.23f; + message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; + + // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message + orb_advert_t vehicle_trajectory_waypoint_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint), &message); + orb_publish(ORB_ID(vehicle_trajectory_waypoint), vehicle_trajectory_waypoint_pub, &message); + + vehicle_status_s vehicle_status{}; + vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; + orb_advert_t vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &vehicle_status); + orb_publish(ORB_ID(vehicle_status), vehicle_status_pub, &vehicle_status); + + subscription_array.update(); + + // WHEN: we inject the vehicle_trajectory_waypoint in the interface + oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); + + // THEN: the setpoints should be injected + EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0], pos_sp(0)); + EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1], pos_sp(1)); + EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2], pos_sp(2)); + EXPECT_TRUE(vel_sp.isAllNan()); + EXPECT_FLOAT_EQ(message.waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw, yaw_sp); + EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp)); +} + +TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy) +{ + // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and a vehicle_trajectory_waypoint message + TestObstacleAvoidance oa; + oa.initializeSubscriptions(subscription_array); + + vehicle_trajectory_waypoint_s message = empty_trajectory_waypoint; + Vector3f pos(3.1f, 4.7f, 5.2f); + oa.test_setPosition(pos); + + // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle_status + orb_advert_t vehicle_trajectory_waypoint_pub = orb_advertise(ORB_ID(vehicle_trajectory_waypoint), &message); + orb_publish(ORB_ID(vehicle_trajectory_waypoint), vehicle_trajectory_waypoint_pub, &message); + + vehicle_status_s vehicle_status{}; + vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; + orb_advert_t vehicle_status_pub = orb_advertise(ORB_ID(vehicle_status), &vehicle_status); + orb_publish(ORB_ID(vehicle_status), vehicle_status_pub, &vehicle_status); + + subscription_array.update(); + + // WHEN: we inject the vehicle_trajectory_waypoint in the interface + oa.injectAvoidanceSetpoints(pos_sp, vel_sp, yaw_sp, yaw_speed_sp); + + // THEN: the setpoints shouldn't be injected + EXPECT_FLOAT_EQ(pos(0), pos_sp(0)); + EXPECT_FLOAT_EQ(pos(1), pos_sp(1)); + EXPECT_FLOAT_EQ(pos(2), pos_sp(2)); + EXPECT_TRUE(vel_sp.isAllNan()); + EXPECT_FALSE(PX4_ISFINITE(yaw_sp)); + EXPECT_FALSE(PX4_ISFINITE(yaw_speed_sp)); +} + +TEST_F(ObstacleAvoidanceTest, oa_desired) +{ + // GIVEN: the flight controller setpoints from FlightTaskAutoMapper and the waypoints from FLightTaskAuto + TestObstacleAvoidance oa; + oa.initializeSubscriptions(subscription_array); + + pos_sp = Vector3f(1.f, 1.2f, NAN); + vel_sp = Vector3f(NAN, NAN, 0.7f); + int type = 4; + Vector3f curr_wp(1.f, 1.2f, 5.0f); + float curr_yaw = 1.02f; + float curr_yawspeed = NAN; + Vector3f next_wp(11.f, 1.2f, 5.0f); + float next_yaw = 0.82f; + float next_yawspeed = NAN; + bool ext_yaw_active = false; + + // WHEN: we inject the setpoints and waypoints in the interface + oa.updateAvoidanceDesiredWaypoints(curr_wp, curr_yaw, curr_yawspeed, next_wp, next_yaw, next_yawspeed, ext_yaw_active, + type); + oa.updateAvoidanceDesiredSetpoints(pos_sp, vel_sp, type); + + // WHEN: we subscribe to the uORB message out of the interface + uORB::SubscriptionData _sub_traj_wp_avoidance_desired{ORB_ID(vehicle_trajectory_waypoint_desired)}; + _sub_traj_wp_avoidance_desired.update(); + + // THEN: we expect the setpoints in POINT_0 and waypoints in POINT_1 and POINT_2 + EXPECT_FLOAT_EQ(pos_sp(0), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[0]); + EXPECT_FLOAT_EQ(pos_sp(1), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[1]); + EXPECT_FALSE(PX4_ISFINITE( + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position[2])); + EXPECT_FALSE(PX4_ISFINITE( + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[0])); + EXPECT_FALSE(PX4_ISFINITE( + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[1])); + EXPECT_FLOAT_EQ(vel_sp(2), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity[2]); + EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].type); + EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid); + + EXPECT_FLOAT_EQ(curr_wp(0), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[0]); + EXPECT_FLOAT_EQ(curr_wp(1), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[1]); + EXPECT_FLOAT_EQ(curr_wp(2), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].position[2]); + EXPECT_FLOAT_EQ(curr_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw); + EXPECT_FALSE(PX4_ISFINITE( + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed)); + EXPECT_EQ(type, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].type); + EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid); + + EXPECT_FLOAT_EQ(next_wp(0), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[0]); + EXPECT_FLOAT_EQ(next_wp(1), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[1]); + EXPECT_FLOAT_EQ(next_wp(2), + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].position[2]); + EXPECT_FLOAT_EQ(next_yaw, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw); + EXPECT_FALSE(PX4_ISFINITE( + _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed)); + EXPECT_EQ(UINT8_MAX, _sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].type); + EXPECT_TRUE(_sub_traj_wp_avoidance_desired.get().waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid); + +}