From 59d413aaf14ca7dc780755535f1ea3f73f3560a3 Mon Sep 17 00:00:00 2001 From: Silvan Date: Thu, 26 Dec 2024 21:13:51 +0100 Subject: [PATCH 1/3] remove avoidance library and logic Signed-off-by: Silvan --- Makefile | 2 +- .../1014_gazebo-classic_iris_obs_avoid | 2 - .../init.d-posix/airframes/4006_gz_px4vision | 1 - .../init.d/airframes/4016_holybro_px4vision | 1 - .../airframes/4020_holybro_px4vision_v1_5 | 1 - msg/versioned/VehicleStatus.msg | 3 - src/lib/CMakeLists.txt | 1 - src/lib/avoidance/CMakeLists.txt | 39 --- src/lib/avoidance/ObstacleAvoidance.cpp | 271 ------------------ src/lib/avoidance/ObstacleAvoidance.hpp | 152 ---------- src/lib/avoidance/ObstacleAvoidanceTest.cpp | 242 ---------------- src/lib/avoidance/ObstacleAvoidance_dummy.hpp | 93 ------ src/modules/commander/Commander.cpp | 26 -- src/modules/commander/Commander.hpp | 3 - .../checks/systemCheck.cpp | 21 -- src/modules/commander/commander_params.c | 8 - .../tasks/Auto/CMakeLists.txt | 2 +- .../tasks/Auto/FlightTaskAuto.cpp | 17 -- .../tasks/Auto/FlightTaskAuto.hpp | 10 - .../FlightTaskManualPosition.hpp | 2 +- src/modules/logger/params.c | 2 +- 21 files changed, 4 insertions(+), 895 deletions(-) delete mode 100644 src/lib/avoidance/CMakeLists.txt delete mode 100644 src/lib/avoidance/ObstacleAvoidance.cpp delete mode 100644 src/lib/avoidance/ObstacleAvoidance.hpp delete mode 100644 src/lib/avoidance/ObstacleAvoidanceTest.cpp delete mode 100644 src/lib/avoidance/ObstacleAvoidance_dummy.hpp diff --git a/Makefile b/Makefile index f549e4c8cecc..c65989ffe758 100644 --- a/Makefile +++ b/Makefile @@ -404,7 +404,7 @@ check_newlines: # Testing # -------------------------------------------------------------------- -.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard tests_avoidance +.PHONY: tests tests_coverage tests_mission tests_mission_coverage tests_offboard .PHONY: rostest python_coverage tests: diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid index 82219698733f..e3c25ec777d9 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/1014_gazebo-classic_iris_obs_avoid @@ -28,5 +28,3 @@ param set-default PWM_MAIN_FUNC1 101 param set-default PWM_MAIN_FUNC2 102 param set-default PWM_MAIN_FUNC3 103 param set-default PWM_MAIN_FUNC4 104 - -param set-default COM_OBS_AVOID 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index 65d290e0cc79..967051b0420e 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -19,7 +19,6 @@ param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 # Commander Parameters -param set-default COM_OBS_AVOID 0 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision index d60de46e7bea..9b86703c31cd 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision +++ b/ROMFS/px4fmu_common/init.d/airframes/4016_holybro_px4vision @@ -12,7 +12,6 @@ . ${R}etc/init.d/rc.mc_defaults # Commander Parameters -param set-default COM_OBS_AVOID 1 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters diff --git a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 index c9c3e5843ac7..e63a9eef01bb 100644 --- a/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 +++ b/ROMFS/px4fmu_common/init.d/airframes/4020_holybro_px4vision_v1_5 @@ -12,7 +12,6 @@ . ${R}etc/init.d/rc.mc_defaults # Commander Parameters -param set-default COM_OBS_AVOID 1 param set-default COM_DISARM_LAND 0.5 # EKF2 parameters diff --git a/msg/versioned/VehicleStatus.msg b/msg/versioned/VehicleStatus.msg index a347dfce3d9d..062c6f076556 100644 --- a/msg/versioned/VehicleStatus.msg +++ b/msg/versioned/VehicleStatus.msg @@ -131,9 +131,6 @@ bool open_drone_id_system_healthy bool parachute_system_present bool parachute_system_healthy -bool avoidance_system_required # Set to true if avoidance system is enabled via COM_OBS_AVOID parameter -bool avoidance_system_valid # Status of the obstacle avoidance system - bool rc_calibration_in_progress bool calibration_enabled diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 69ab23aacc0e..f5dcdcab35f9 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -34,7 +34,6 @@ add_subdirectory(adsb EXCLUDE_FROM_ALL) add_subdirectory(airspeed EXCLUDE_FROM_ALL) add_subdirectory(atmosphere EXCLUDE_FROM_ALL) -add_subdirectory(avoidance EXCLUDE_FROM_ALL) add_subdirectory(battery EXCLUDE_FROM_ALL) add_subdirectory(bezier EXCLUDE_FROM_ALL) add_subdirectory(button EXCLUDE_FROM_ALL) diff --git a/src/lib/avoidance/CMakeLists.txt b/src/lib/avoidance/CMakeLists.txt deleted file mode 100644 index c1da2a76dc67..000000000000 --- a/src/lib/avoidance/CMakeLists.txt +++ /dev/null @@ -1,39 +0,0 @@ -############################################################################ -# -# 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. -# -############################################################################ - -px4_add_library(avoidance - ObstacleAvoidance.cpp -) -target_link_libraries(avoidance PUBLIC hysteresis bezier) - -px4_add_functional_gtest(SRC ObstacleAvoidanceTest.cpp LINKLIBS avoidance) diff --git a/src/lib/avoidance/ObstacleAvoidance.cpp b/src/lib/avoidance/ObstacleAvoidance.cpp deleted file mode 100644 index f818f068a0af..000000000000 --- a/src/lib/avoidance/ObstacleAvoidance.cpp +++ /dev/null @@ -1,271 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * @file ObstacleAvoidance.cpp - */ - -#include "ObstacleAvoidance.hpp" -#include "bezier/BezierN.hpp" - -using namespace matrix; -using namespace time_literals; - -/** Timeout in us for trajectory data to get considered invalid */ -static constexpr uint64_t TRAJECTORY_STREAM_TIMEOUT_US = 500_ms; -/** If Flighttask fails, keep 0.5 seconds the current setpoint before going into failsafe land */ -static constexpr uint64_t TIME_BEFORE_FAILSAFE = 500_ms; -static constexpr uint64_t Z_PROGRESS_TIMEOUT_US = 2_s; - -ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) : - ModuleParams(parent) -{ - _desired_waypoint = empty_trajectory_waypoint; - _failsafe_position.setNaN(); - _avoidance_point_not_valid_hysteresis.set_hysteresis_time_from(false, TIME_BEFORE_FAILSAFE); - _no_progress_z_hysteresis.set_hysteresis_time_from(false, Z_PROGRESS_TIMEOUT_US); - -} - -void ObstacleAvoidance::injectAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp, - float &yaw_speed_sp) -{ - _sub_vehicle_status.update(); - _sub_vehicle_trajectory_waypoint.update(); - _sub_vehicle_trajectory_bezier.update(); - - const auto &wp_msg = _sub_vehicle_trajectory_waypoint.get(); - const auto &bezier_msg = _sub_vehicle_trajectory_bezier.get(); - - const bool wp_msg_timeout = hrt_elapsed_time((hrt_abstime *)&wp_msg.timestamp) > TRAJECTORY_STREAM_TIMEOUT_US; - const bool bezier_msg_timeout = hrt_elapsed_time((hrt_abstime *)&bezier_msg.timestamp) > hrt_abstime( - bezier_msg.control_points[bezier_msg.bezier_order - 1].delta * 1e6f); - const bool avoidance_data_timeout = wp_msg_timeout && bezier_msg_timeout; - - const bool avoidance_point_valid = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid; - const bool avoidance_bezier_valid = bezier_msg.bezier_order > 0; - - _avoidance_point_not_valid_hysteresis.set_state_and_update(!avoidance_point_valid - && !avoidance_bezier_valid, hrt_absolute_time()); - - const bool avoidance_invalid = (avoidance_data_timeout || _avoidance_point_not_valid_hysteresis.get_state()); - - if ((_sub_vehicle_status.get().nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER) && avoidance_invalid) { - // if in nav_state LOITER and avoidance isn't healthy, don't inject setpoints from avoidance system - return; - } - - if (avoidance_invalid) { - if (_avoidance_activated) { - // Invalid point received: deactivate - PX4_WARN("Obstacle Avoidance system failed, loitering"); - _publishVehicleCmdDoLoiter(); - _avoidance_activated = false; - } - - if (!_failsafe_position.isAllFinite()) { - // save vehicle position when entering failsafe - _failsafe_position = _position; - } - - pos_sp = _failsafe_position; - vel_sp.setNaN(); - yaw_sp = NAN; - yaw_speed_sp = NAN; - - // Do nothing further - wait until activation - return; - - } else if (!_avoidance_activated) { - // First setpoint has been received: activate - PX4_INFO("Obstacle Avoidance system activated"); - _failsafe_position.setNaN(); - _avoidance_activated = true; - } - - if (avoidance_point_valid && !wp_msg_timeout) { - const auto &point0 = wp_msg.waypoints[vehicle_trajectory_waypoint_s::POINT_0]; - pos_sp = Vector3f(point0.position); - vel_sp = Vector3f(point0.velocity); - - if (!_ext_yaw_active) { - // inject yaw setpoints only if weathervane isn't active - yaw_sp = point0.yaw; - yaw_speed_sp = point0.yaw_speed; - } - - } else if (avoidance_bezier_valid && !bezier_msg_timeout) { - float yaw = NAN, yaw_speed = NAN; - _generateBezierSetpoints(pos_sp, vel_sp, yaw, yaw_speed); - - if (!_ext_yaw_active) { - // inject yaw setpoints only if weathervane isn't active - yaw_sp = yaw; - yaw_speed_sp = yaw_speed; - } - } -} - -void ObstacleAvoidance::_generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, - float &yaw, float &yaw_velocity) -{ - const auto &msg = _sub_vehicle_trajectory_bezier.get(); - int bezier_order = msg.bezier_order; - matrix::Vector3f bezier_points[bezier_order]; - float bezier_yaws[bezier_order]; - - for (int i = 0; i < bezier_order; i++) { - bezier_points[i] = Vector3f(msg.control_points[i].position); - bezier_yaws[i] = msg.control_points[i].yaw; - } - - const float duration_s = msg.control_points[bezier_order - 1].delta; - const hrt_abstime now = hrt_absolute_time(); - const hrt_abstime start = msg.timestamp; - const hrt_abstime end = start + hrt_abstime(duration_s * 1e6f); - - float T = NAN; - - if (bezier::calculateT(start, end, now, T) && - bezier::calculateBezierPosVel(bezier_points, bezier_order, T, position, velocity) && - bezier::calculateBezierYaw(bezier_yaws, bezier_order, T, yaw, yaw_velocity) - ) { - // translate bezier velocities T [0;1] into real velocities m/s - yaw_velocity /= duration_s; - velocity /= duration_s; - - } else { - PX4_WARN("Obstacle Avoidance system failed, bad trajectory"); - } -} - - -void ObstacleAvoidance::updateAvoidanceDesiredWaypoints(const Vector3f &curr_wp, const float curr_yaw, - const float curr_yawspeed, const Vector3f &next_wp, const float next_yaw, const float next_yawspeed, - const bool ext_yaw_active, const int wp_type) -{ - _desired_waypoint.timestamp = hrt_absolute_time(); - _desired_waypoint.type = vehicle_trajectory_waypoint_s::MAV_TRAJECTORY_REPRESENTATION_WAYPOINTS; - _curr_wp = curr_wp; - _ext_yaw_active = ext_yaw_active; - - curr_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].position); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].velocity); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].acceleration); - - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw = curr_yaw; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].yaw_speed = curr_yawspeed; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].point_valid = true; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_1].type = wp_type; - - next_wp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].position); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].velocity); - Vector3f(NAN, NAN, NAN).copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].acceleration); - - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw = next_yaw; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].yaw_speed = next_yawspeed; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_2].point_valid = true; -} - -void ObstacleAvoidance::updateAvoidanceDesiredSetpoints(const Vector3f &pos_sp, const Vector3f &vel_sp, const int type) -{ - pos_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].position); - vel_sp.copyTo(_desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity); - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].type = type; - _desired_waypoint.waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid = true; - - _pub_traj_wp_avoidance_desired.publish(_desired_waypoint); - - _desired_waypoint = empty_trajectory_waypoint; -} - -void ObstacleAvoidance::checkAvoidanceProgress(const Vector3f &pos, const Vector3f &prev_wp, - float target_acceptance_radius, const Vector2f &closest_pt) -{ - _position = pos; - position_controller_status_s pos_control_status = {}; - pos_control_status.timestamp = hrt_absolute_time(); - - // vector from previous triplet to current target - Vector2f prev_to_target = Vector2f(_curr_wp - prev_wp); - // vector from previous triplet to the vehicle projected position on the line previous-target triplet - Vector2f prev_to_closest_pt = closest_pt - Vector2f(prev_wp); - // fraction of the previous-tagerget line that has been flown - const float prev_curr_travelled = prev_to_closest_pt.length() / prev_to_target.length(); - - Vector2f pos_to_target = Vector2f(_curr_wp - _position); - - if (prev_curr_travelled > 1.0f) { - // if the vehicle projected position on the line previous-target is past the target waypoint, - // increase the target acceptance radius such that navigator will update the triplets - pos_control_status.acceptance_radius = pos_to_target.length() + 0.5f; - } - - const float pos_to_target_z = fabsf(_curr_wp(2) - _position(2)); - - bool no_progress_z = (pos_to_target_z > _prev_pos_to_target_z); - _no_progress_z_hysteresis.set_state_and_update(no_progress_z, hrt_absolute_time()); - - if (pos_to_target.length() < target_acceptance_radius && pos_to_target_z > _param_nav_mc_alt_rad.get() - && _no_progress_z_hysteresis.get_state()) { - // vehicle above or below the target waypoint - pos_control_status.altitude_acceptance = pos_to_target_z + 0.5f; - } - - _prev_pos_to_target_z = pos_to_target_z; - - // do not check for waypoints yaw acceptance in navigator - pos_control_status.yaw_acceptance = NAN; - - _pub_pos_control_status.publish(pos_control_status); -} - -void ObstacleAvoidance::_publishVehicleCmdDoLoiter() -{ - vehicle_command_s command{}; - command.timestamp = hrt_absolute_time(); - command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; - command.param1 = (float)1; // base mode - command.param3 = (float)0; // sub mode - command.target_system = 1; - command.target_component = 1; - command.source_system = 1; - command.source_component = 1; - command.confirmation = false; - command.from_external = false; - command.param2 = (float)PX4_CUSTOM_MAIN_MODE_AUTO; - command.param3 = (float)PX4_CUSTOM_SUB_MODE_AUTO_LOITER; - - // publish the vehicle command - _pub_vehicle_command.publish(command); -} diff --git a/src/lib/avoidance/ObstacleAvoidance.hpp b/src/lib/avoidance/ObstacleAvoidance.hpp deleted file mode 100644 index 8a27ffbbf102..000000000000 --- a/src/lib/avoidance/ObstacleAvoidance.hpp +++ /dev/null @@ -1,152 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * @file ObstacleAvoidance.hpp - * This class is used to inject the setpoints of an obstacle avoidance system - * into the FlightTasks - * - * @author Martina Rivizzigno - */ - -#pragma once - -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#include - -#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: - ObstacleAvoidance(ModuleParams *parent); - ~ObstacleAvoidance() = default; - - /** - * Inject setpoints from obstacle avoidance system into FlightTasks. - * @param pos_sp, position setpoint - * @param vel_sp, velocity setpoint - * @param yaw_sp, yaw setpoint - * @param yaw_speed_sp, yaw speed setpoint - */ - void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp); - - /** - * Updates the desired waypoints to send to the obstacle avoidance system. These messages don't have any direct impact on the flight. - * @param curr_wp, current position triplet - * @param curr_yaw, current yaw triplet - * @param curr_yawspeed, current yaw speed triplet - * @param next_wp, next position triplet - * @param next_yaw, next yaw triplet - * @param next_yawspeed, next yaw speed triplet - * @param wp_type, current triplet type - */ - void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, const float curr_yawspeed, - const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, - const int wp_type); - /** - * Updates the desired setpoints to send to the obstacle avoidance system. - * @param pos_sp, desired position setpoint computed by the active FlightTask - * @param vel_sp, desired velocity setpoint computed by the active FlightTask - * @param type, current triplet type - */ - void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, const int type); - - /** - * Checks the vehicle progress between previous and current position waypoint of the triplet. - * @param pos, vehicle position - * @param prev_wp, previous position triplet - * @param target_acceptance_radius, current position triplet xy acceptance radius - * @param closest_pt, closest point to the vehicle on the line previous-current position triplet - */ - void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, - float target_acceptance_radius, const matrix::Vector2f &closest_pt); - -protected: - - uORB::SubscriptionData _sub_vehicle_trajectory_bezier{ORB_ID(vehicle_trajectory_bezier)}; /**< vehicle trajectory waypoint subscription */ - uORB::SubscriptionData _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)}; /**< vehicle trajectory waypoint subscription */ - uORB::SubscriptionData _sub_vehicle_status{ORB_ID(vehicle_status)}; /**< vehicle status subscription */ - - 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 */ - uORB::Publication _pub_pos_control_status{ORB_ID(position_controller_status)}; /**< position controller status publication */ - uORB::Publication _pub_vehicle_command{ORB_ID(vehicle_command)}; /**< vehicle command do publication */ - - matrix::Vector3f _curr_wp = {}; /**< current position triplet */ - matrix::Vector3f _position = {}; /**< current vehicle position */ - matrix::Vector3f _failsafe_position = {}; /**< vehicle position when entered in failsafe */ - - bool _avoidance_activated{false}; /**< true after the first avoidance setpoint is received */ - - systemlib::Hysteresis _avoidance_point_not_valid_hysteresis{false}; /**< becomes true if the companion doesn't start sending valid setpoints */ - systemlib::Hysteresis _no_progress_z_hysteresis{false}; /**< becomes true if the vehicle is not making progress towards the z component of the goal */ - - float _prev_pos_to_target_z = -1.f; /**< z distance to the goal */ - - bool _ext_yaw_active = false; /**< true, if external yaw handling is active */ - - /** - * Publishes vehicle command. - */ - void _publishVehicleCmdDoLoiter(); - void _generateBezierSetpoints(matrix::Vector3f &position, matrix::Vector3f &velocity, float &yaw, float &yaw_velocity); - - DEFINE_PARAMETERS( - (ParamFloat) _param_nav_mc_alt_rad /**< Acceptance radius for multicopter altitude */ - ); - -}; diff --git a/src/lib/avoidance/ObstacleAvoidanceTest.cpp b/src/lib/avoidance/ObstacleAvoidanceTest.cpp deleted file mode 100644 index 008a8758a9e2..000000000000 --- a/src/lib/avoidance/ObstacleAvoidanceTest.cpp +++ /dev/null @@ -1,242 +0,0 @@ -/**************************************************************************** - * - * 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=ObstacleAvoidance - -class ObstacleAvoidanceTest : public ::testing::Test -{ -public: - Vector3f pos_sp; - Vector3f vel_sp; - float yaw_sp = 0.123f; - float yaw_speed_sp = NAN; - void SetUp() override - - { - param_control_autosave(false); - 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 FlightTaskAuto and a vehicle_trajectory_waypoint message coming - // from offboard - TestObstacleAvoidance oa; - - 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 - uORB::Publication vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; - vehicle_trajectory_waypoint_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // 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_healthy_bezier) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message coming - // from offboard - TestObstacleAvoidance oa; - - vehicle_trajectory_bezier_s message {}; - message.timestamp = hrt_absolute_time(); - message.bezier_order = 2; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[0] = 2.6f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[1] = 2.4f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].position[2] = 2.7f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].yaw = 0.23f; - message.control_points[vehicle_trajectory_bezier_s::POINT_0].delta = NAN; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[0] = 2.6f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[1] = 2.4f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].position[2] = 3.7f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].yaw = 0.23f; - message.control_points[vehicle_trajectory_bezier_s::POINT_1].delta = 0.5f; - - // GIVEN: and we publish the vehicle_trajectory_waypoint message and vehicle status message - uORB::Publication vehicle_trajectory_bezier_pub{ORB_ID(vehicle_trajectory_bezier)}; - vehicle_trajectory_bezier_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // 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(2.6f, pos_sp(0)); - EXPECT_FLOAT_EQ(2.4f, pos_sp(1)); - EXPECT_LT(2.7f, pos_sp(2)); - EXPECT_GT(2.8f, pos_sp(2)); // probably only a tiny bit above 2.7, but let's not have flakey tests - EXPECT_FLOAT_EQ(vel_sp.xy().norm(), 0); - EXPECT_FLOAT_EQ(vel_sp(2), (3.7f - 2.7f) / 0.5f); - EXPECT_FLOAT_EQ(0.23, yaw_sp); - EXPECT_FLOAT_EQ(yaw_speed_sp, 0); -} - -TEST_F(ObstacleAvoidanceTest, oa_enabled_not_healthy) -{ - // GIVEN: the flight controller setpoints from FlightTaskAuto and a vehicle_trajectory_waypoint message - TestObstacleAvoidance oa; - - 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 - uORB::Publication vehicle_trajectory_waypoint_pub{ORB_ID(vehicle_trajectory_waypoint)}; - vehicle_trajectory_waypoint_pub.publish(message); - - vehicle_status_s vehicle_status{}; - vehicle_status.nav_state = vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION; - uORB::Publication vehicle_status_pub{ORB_ID(vehicle_status)}; - vehicle_status_pub.publish(vehicle_status); - - // 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 FlightTaskAuto and the waypoints from FLightTaskAuto - TestObstacleAvoidance oa; - - 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)}; - - // 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); - -} diff --git a/src/lib/avoidance/ObstacleAvoidance_dummy.hpp b/src/lib/avoidance/ObstacleAvoidance_dummy.hpp deleted file mode 100644 index 7831c919fd2d..000000000000 --- a/src/lib/avoidance/ObstacleAvoidance_dummy.hpp +++ /dev/null @@ -1,93 +0,0 @@ -/**************************************************************************** - * - * 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. - * - ****************************************************************************/ - -/** - * @file ObstacleAvoidance_dummy.hpp - * This is a dummy class to reduce flash space for when obstacle avoidance is not required - * - * @author Julian Kent - */ - -#pragma once - -#include -#include - - -#include - - -class ObstacleAvoidance -{ -public: - ObstacleAvoidance(void *) {} // takes void* argument to be compatible with ModuleParams constructor - - - void injectAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, - float &yaw_speed_sp) - { - notify_dummy(); - }; - - void updateAvoidanceDesiredWaypoints(const matrix::Vector3f &curr_wp, const float curr_yaw, - const float curr_yawspeed, - const matrix::Vector3f &next_wp, const float next_yaw, const float next_yawspeed, const bool ext_yaw_active, - const int wp_type) - { - notify_dummy(); - }; - - void updateAvoidanceDesiredSetpoints(const matrix::Vector3f &pos_sp, const matrix::Vector3f &vel_sp, - const int type) - { - notify_dummy(); - } - - - void checkAvoidanceProgress(const matrix::Vector3f &pos, const matrix::Vector3f &prev_wp, - float target_acceptance_radius, const matrix::Vector2f &closest_pt) - { - notify_dummy(); - }; - -protected: - - bool _logged_error = false; - void notify_dummy() - { - if (!_logged_error) { - PX4_ERR("Dummy avoidance library called!"); - _logged_error = true; - } - } -}; diff --git a/src/modules/commander/Commander.cpp b/src/modules/commander/Commander.cpp index 5fc8ee4d8f3c..370f1e913331 100644 --- a/src/modules/commander/Commander.cpp +++ b/src/modules/commander/Commander.cpp @@ -1717,8 +1717,6 @@ void Commander::updateParameters() _vehicle_status.system_type = value_int32; } - _vehicle_status.avoidance_system_required = _param_com_obs_avoid.get(); - _auto_disarm_killed.set_hysteresis_time_from(false, _param_com_kill_disarm.get() * 1_s); const bool is_rotary = is_rotary_wing(_vehicle_status) || (is_vtol(_vehicle_status) @@ -1799,9 +1797,6 @@ void Commander::run() _status_changed = true; } - /* Update OA parameter */ - _vehicle_status.avoidance_system_required = _param_com_obs_avoid.get(); - handlePowerButtonState(); systemPowerUpdate(); @@ -2813,16 +2808,6 @@ void Commander::dataLinkCheck() _vehicle_status.open_drone_id_system_present = true; _vehicle_status.open_drone_id_system_healthy = healthy; } - - if (telemetry.heartbeat_component_obstacle_avoidance) { - if (_avoidance_system_lost) { - _avoidance_system_lost = false; - _status_changed = true; - } - - _datalink_last_heartbeat_avoidance_system = telemetry.timestamp; - _vehicle_status.avoidance_system_valid = telemetry.avoidance_system_healthy; - } } } @@ -2874,17 +2859,6 @@ void Commander::dataLinkCheck() _open_drone_id_system_lost = true; _status_changed = true; } - - // AVOIDANCE SYSTEM state check (only if it is enabled) - if (_vehicle_status.avoidance_system_required && !_onboard_controller_lost) { - // if heartbeats stop - if (!_avoidance_system_lost && (_datalink_last_heartbeat_avoidance_system > 0) - && (hrt_elapsed_time(&_datalink_last_heartbeat_avoidance_system) > 5_s)) { - - _avoidance_system_lost = true; - _vehicle_status.avoidance_system_valid = false; - } - } } void Commander::battery_status_check() diff --git a/src/modules/commander/Commander.hpp b/src/modules/commander/Commander.hpp index e5bdb947aacb..7156e2b046a8 100644 --- a/src/modules/commander/Commander.hpp +++ b/src/modules/commander/Commander.hpp @@ -242,7 +242,6 @@ class Commander : public ModuleBase, public ModuleParams Hysteresis _auto_disarm_killed{false}; hrt_abstime _datalink_last_heartbeat_open_drone_id_system{0}; - hrt_abstime _datalink_last_heartbeat_avoidance_system{0}; hrt_abstime _datalink_last_heartbeat_gcs{0}; hrt_abstime _datalink_last_heartbeat_onboard_controller{0}; hrt_abstime _datalink_last_heartbeat_parachute_system{0}; @@ -269,7 +268,6 @@ class Commander : public ModuleBase, public ModuleParams bool _failsafe_user_override_request{false}; ///< override request due to stick movements bool _open_drone_id_system_lost{true}; - bool _avoidance_system_lost{false}; bool _onboard_controller_lost{false}; bool _parachute_system_lost{true}; @@ -344,7 +342,6 @@ class Commander : public ModuleBase, public ModuleParams (ParamBool) _param_com_force_safety, (ParamFloat) _param_com_kill_disarm, (ParamBool) _param_com_mot_test_en, - (ParamBool) _param_com_obs_avoid, (ParamFloat) _param_com_obc_loss_t, (ParamInt) _param_com_prearm_mode, (ParamInt) _param_com_rc_override, diff --git a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp index 32beaba9af7b..1f2592c9ed27 100644 --- a/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp +++ b/src/modules/commander/HealthAndArmingChecks/checks/systemCheck.cpp @@ -121,27 +121,6 @@ void SystemChecks::checkAndReport(const Context &context, Report &reporter) } } - // avoidance system - if (context.status().avoidance_system_required) { - if (context.status().avoidance_system_valid) { - reporter.setIsPresent(health_component_t::avoidance); - - } else { - /* EVENT - * @description - * - * This check can be configured via COM_OBS_AVOID parameter. - * - */ - reporter.armingCheckFailure(NavModes::All, health_component_t::system, events::ID("check_system_avoidance_not_ready"), - events::Log::Error, "Avoidance system not ready"); - - if (reporter.mavlink_log_pub()) { - mavlink_log_critical(reporter.mavlink_log_pub(), "Preflight Fail: Avoidance system not ready"); - } - } - } - // VTOL in transition if (context.status().is_vtol && !context.isArmed()) { if (context.status().in_transition_mode) { diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 48b0cce5221a..52775b87ab8f 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -640,14 +640,6 @@ PARAM_DEFINE_INT32(COM_RCL_EXCEPT, 0); */ PARAM_DEFINE_INT32(COM_ACT_FAIL_ACT, 0); -/** - * Flag to enable obstacle avoidance. - * - * @boolean - * @group Commander - */ -PARAM_DEFINE_INT32(COM_OBS_AVOID, 0); - /** * Expect and require a healthy MAVLink parachute system * diff --git a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt index 2fadf2754d2d..905e08e80be7 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt +++ b/src/modules/flight_mode_manager/tasks/Auto/CMakeLists.txt @@ -35,5 +35,5 @@ px4_add_library(FlightTaskAuto FlightTaskAuto.cpp ) -target_link_libraries(FlightTaskAuto PUBLIC avoidance FlightTask FlightTaskUtility WeatherVane) +target_link_libraries(FlightTaskAuto PUBLIC FlightTask FlightTaskUtility WeatherVane) target_include_directories(FlightTaskAuto PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 834f79465fca..040b1df74fd0 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -156,12 +156,6 @@ bool FlightTaskAuto::update() break; } - if (_param_com_obs_avoid.get()) { - _obstacle_avoidance.updateAvoidanceDesiredSetpoints(_position_setpoint, _velocity_setpoint, (int)_type); - _obstacle_avoidance.injectAvoidanceSetpoints(_position_setpoint, _velocity_setpoint, _yaw_setpoint, - _yawspeed_setpoint); - } - _checkEmergencyBraking(); Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp}; @@ -483,17 +477,6 @@ bool FlightTaskAuto::_evaluateTriplets() _updateInternalWaypoints(); } - if (_param_com_obs_avoid.get() - && _sub_vehicle_status.get().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) { - _obstacle_avoidance.updateAvoidanceDesiredWaypoints(_triplet_target, _yaw_setpoint, _yawspeed_setpoint, - _triplet_next_wp, - _sub_triplet_setpoint.get().next.yaw, - (float)NAN, - _weathervane.isActive(), _sub_triplet_setpoint.get().current.type); - _obstacle_avoidance.checkAvoidanceProgress( - _position, _triplet_prev_wp, _target_acceptance_radius, Vector2f(_closest_pt)); - } - // set heading _weathervane.update(); diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp index 90a98cac23ad..6c49dd872621 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.hpp @@ -53,13 +53,6 @@ #include "StickAccelerationXY.hpp" #include "StickYaw.hpp" -// TODO: make this switchable in the board config, like a module -#if CONSTRAINED_FLASH -#include -#else -#include -#endif - /** * This enum has to agree with position_setpoint_s type definition * The only reason for not using the struct position_setpoint is because @@ -147,8 +140,6 @@ class FlightTaskAuto : public FlightTask AlphaFilter _yawspeed_filter; bool _yaw_sp_aligned{false}; - ObstacleAvoidance _obstacle_avoidance{this}; /**< class adjusting setpoints according to external avoidance module's input */ - PositionSmoothing _position_smoothing; Vector3f _unsmoothed_velocity_setpoint; Sticks _sticks{this}; @@ -165,7 +156,6 @@ class FlightTaskAuto : public FlightTask (ParamFloat) _param_nav_mc_alt_rad, //vertical acceptance radius at which waypoints are updated (ParamInt) _param_mpc_yaw_mode, // defines how heading is executed, - (ParamInt) _param_com_obs_avoid, // obstacle avoidance active (ParamFloat) _param_mpc_yawrauto_max, (ParamFloat) _param_mis_yaw_err, // yaw-error threshold (ParamFloat) _param_mpc_acc_hor, // acceleration in flight diff --git a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp index fb7f5dd4e571..b1f05092d346 100644 --- a/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -68,5 +68,5 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude uint8_t _reset_counter{0}; /**< counter for estimator resets in xy-direction */ WeatherVane _weathervane{this}; /**< weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ - CollisionPrevention _collision_prevention{this}; /**< collision avoidance setpoint amendment */ + CollisionPrevention _collision_prevention{this}; /**< collision prevention setpoint amendment */ }; diff --git a/src/modules/logger/params.c b/src/modules/logger/params.c index 3d7e873854d2..e1bbd5145f51 100644 --- a/src/modules/logger/params.c +++ b/src/modules/logger/params.c @@ -123,7 +123,7 @@ PARAM_DEFINE_INT32(SDLOG_MISSION, 0); * 4 : Full rates for analysis of fast maneuvers (RC, attitude, rates and actuators) * 5 : Debugging topics (debug_*.msg topics, for custom code) * 6 : Topics for sensor comparison (low rate raw IMU, Baro and magnetometer data) - * 7 : Topics for computer vision and collision avoidance + * 7 : Topics for computer vision and collision prevention * 8 : Raw FIFO high-rate IMU (Gyro) * 9 : Raw FIFO high-rate IMU (Accel) * 10: Logging of mavlink tunnel message (useful for payload communication debugging) From 1eee7557528a870c648213f28f2a9a59d2f13734 Mon Sep 17 00:00:00 2001 From: Silvan Date: Thu, 26 Dec 2024 21:44:30 +0100 Subject: [PATCH 2/3] PositionControllerStatus: remove unused fields Remove yaw_acceptance and altitude_acceptance_radius fields as they were only filled by now removed avoidance controller. Signed-off-by: Silvan --- msg/PositionControllerStatus.msg | 2 -- .../fw_pos_control/FixedwingPositionControl.cpp | 2 -- src/modules/navigator/navigator_main.cpp | 14 -------------- .../rover_pos_control/RoverPositionControl.cpp | 1 - 4 files changed, 19 deletions(-) diff --git a/msg/PositionControllerStatus.msg b/msg/PositionControllerStatus.msg index 7237351fd0f3..44ec5412ba21 100644 --- a/msg/PositionControllerStatus.msg +++ b/msg/PositionControllerStatus.msg @@ -7,6 +7,4 @@ float32 target_bearing # Bearing angle from aircraft to current target [rad] float32 xtrack_error # Signed track error [m] float32 wp_dist # Distance to active (next) waypoint [m] float32 acceptance_radius # Current horizontal acceptance radius [m] -float32 yaw_acceptance # Yaw acceptance error[rad] -float32 altitude_acceptance # Current vertical acceptance error [m] uint8 type # Current (applied) position setpoint type (see PositionSetpoint.msg) diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index a15f1e8f7154..2f914dad1fcb 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -487,8 +487,6 @@ FixedwingPositionControl::status_publish() pos_ctrl_status.wp_dist = get_distance_to_next_waypoint(_current_latitude, _current_longitude, _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); - pos_ctrl_status.yaw_acceptance = NAN; - pos_ctrl_status.timestamp = hrt_absolute_time(); pos_ctrl_status.type = _position_sp_type; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 201eeb6123f6..34b4d2c00e25 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1142,17 +1142,11 @@ float Navigator::get_altitude_acceptance_radius() } else { float alt_acceptance_radius = _param_nav_mc_alt_rad.get(); - - const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - const position_setpoint_s &curr_sp = get_position_setpoint_triplet()->current; if (PX4_ISFINITE(curr_sp.alt_acceptance_radius) && curr_sp.alt_acceptance_radius > FLT_EPSILON) { alt_acceptance_radius = curr_sp.alt_acceptance_radius; - } else if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) - && pos_ctrl_status.altitude_acceptance > alt_acceptance_radius) { - alt_acceptance_radius = pos_ctrl_status.altitude_acceptance; } return alt_acceptance_radius; @@ -1214,14 +1208,6 @@ bool Navigator::get_yaw_to_be_accepted(float mission_item_yaw) { float yaw = mission_item_yaw; - const position_controller_status_s &pos_ctrl_status = _position_controller_status_sub.get(); - - // if yaw_acceptance from position controller is NaN overwrite the mission item yaw such that - // the waypoint can be reached from any direction - if ((pos_ctrl_status.timestamp > _pos_sp_triplet.timestamp) && !PX4_ISFINITE(pos_ctrl_status.yaw_acceptance)) { - yaw = pos_ctrl_status.yaw_acceptance; - } - return PX4_ISFINITE(yaw); } diff --git a/src/modules/rover_pos_control/RoverPositionControl.cpp b/src/modules/rover_pos_control/RoverPositionControl.cpp index 7466077a9334..119c8c18e879 100644 --- a/src/modules/rover_pos_control/RoverPositionControl.cpp +++ b/src/modules/rover_pos_control/RoverPositionControl.cpp @@ -441,7 +441,6 @@ RoverPositionControl::Run() _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon); pos_ctrl_status.acceptance_radius = turn_distance; - pos_ctrl_status.yaw_acceptance = NAN; pos_ctrl_status.timestamp = hrt_absolute_time(); From 0a7e8a5e501bed20abfd8563acd19e59f5875ac7 Mon Sep 17 00:00:00 2001 From: Silvan Date: Thu, 26 Dec 2024 21:16:12 +0100 Subject: [PATCH 3/3] FlightTaskAuto: add todo for check Signed-off-by: Silvan --- src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp index 040b1df74fd0..b59d7709e218 100644 --- a/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp +++ b/src/modules/flight_mode_manager/tasks/Auto/FlightTaskAuto.cpp @@ -159,7 +159,7 @@ bool FlightTaskAuto::update() _checkEmergencyBraking(); Vector3f waypoints[] = {_prev_wp, _position_setpoint, _next_wp}; - if (isTargetModified()) { + if (isTargetModified()) { // TODO check if still needed // In case object avoidance has injected a new setpoint, we take this as the next waypoints waypoints[2] = _position_setpoint; }