diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 5780155febd8..c66a74858756 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -42,6 +42,7 @@ set(msg_files camera_capture.msg camera_trigger.msg collision_report.msg + collision_constraints.msg commander_state.msg cpuload.msg debug_array.msg diff --git a/msg/collision_constraints.msg b/msg/collision_constraints.msg new file mode 100644 index 000000000000..4c08ac801ddd --- /dev/null +++ b/msg/collision_constraints.msg @@ -0,0 +1,12 @@ +# Local setpoint constraints in NED frame +# setting something to NaN means that no limit is provided + +uint64 timestamp # time since system start (microseconds) + +#value of 0 means no constraint along this axis, value of 1 means no movements alowed, value bigger than 1 forces negative movement. +float32[2] constraints_normalized_x # constraints determined by range sensor measurements [x negative, x positive] +float32[2] constraints_normalized_y # constraintss determined by range sensor measurements [y negative, y positive] + +float32[2] original_setpoint # velocities demanded +float32[2] adapted_setpoint # velocities allowed + diff --git a/src/lib/CMakeLists.txt b/src/lib/CMakeLists.txt index 45956ee7f8b5..c0734c41019d 100644 --- a/src/lib/CMakeLists.txt +++ b/src/lib/CMakeLists.txt @@ -56,3 +56,4 @@ add_subdirectory(terrain_estimation) add_subdirectory(tunes) add_subdirectory(version) add_subdirectory(WeatherVane) +add_subdirectory(CollisionPrevention) diff --git a/src/lib/CollisionPrevention/CMakeLists.txt b/src/lib/CollisionPrevention/CMakeLists.txt new file mode 100644 index 000000000000..b9ef18f7c361 --- /dev/null +++ b/src/lib/CollisionPrevention/CMakeLists.txt @@ -0,0 +1,34 @@ +############################################################################ +# +# Copyright (c) 2018 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(CollisionPrevention CollisionPrevention.cpp) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp new file mode 100644 index 000000000000..76a32e1c6f0a --- /dev/null +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -0,0 +1,194 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 CollisionPrevention.cpp + * CollisionPrevention controller. + * + */ + +#include +using namespace matrix; +using namespace time_literals; + + +CollisionPrevention::CollisionPrevention(ModuleParams *parent) : + ModuleParams(parent) +{ + +} + +CollisionPrevention::~CollisionPrevention() +{ + //unadvertise publishers + if (_constraints_pub != nullptr) { + orb_unadvertise(_constraints_pub); + } + + if (_mavlink_log_pub != nullptr) { + orb_unadvertise(_mavlink_log_pub); + } +} + +bool CollisionPrevention::initializeSubscriptions(SubscriptionArray &subscription_array) +{ + if (!subscription_array.get(ORB_ID(obstacle_distance), _sub_obstacle_distance)) { + return false; + } + + return true; +} + +void CollisionPrevention::reset_constraints() +{ + + _move_constraints_x_normalized.zero(); //normalized constraint in x-direction + _move_constraints_y_normalized.zero(); //normalized constraint in y-direction + + _move_constraints_x.zero(); //constraint in x-direction + _move_constraints_y.zero(); //constraint in y-direction +} + +void CollisionPrevention::publish_constraints(const Vector2f &original_setpoint, const Vector2f &adapted_setpoint) +{ + + collision_constraints_s constraints; /**< collision constraints message */ + + //fill in values + constraints.timestamp = hrt_absolute_time(); + constraints.constraints_normalized_x[0] = _move_constraints_x_normalized(0); + constraints.constraints_normalized_x[1] = _move_constraints_x_normalized(1); + constraints.constraints_normalized_y[0] = _move_constraints_y_normalized(0); + constraints.constraints_normalized_y[1] = _move_constraints_y_normalized(1); + + constraints.original_setpoint[0] = original_setpoint(0); + constraints.original_setpoint[1] = original_setpoint(1); + constraints.adapted_setpoint[0] = adapted_setpoint(0); + constraints.adapted_setpoint[1] = adapted_setpoint(1); + + // publish constraints + if (_constraints_pub != nullptr) { + orb_publish(ORB_ID(collision_constraints), _constraints_pub, &constraints); + + } else { + _constraints_pub = orb_advertise(ORB_ID(collision_constraints), &constraints); + } +} + +void CollisionPrevention::update_range_constraints() +{ + const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance->get(); + + if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) { + float max_detection_distance = obstacle_distance.max_distance / 100.0f; //convert to meters + + int distances_array_size = sizeof(obstacle_distance.distances) / sizeof(obstacle_distance.distances[0]); + + for (int i = 0; i < distances_array_size; i++) { + //determine if distance bin is valid and contains a valid distance measurement + if (obstacle_distance.distances[i] < obstacle_distance.max_distance && + obstacle_distance.distances[i] > obstacle_distance.min_distance && i * obstacle_distance.increment < 360) { + float distance = obstacle_distance.distances[i] / 100.0f; //convert to meters + float angle = math::radians((float)i * obstacle_distance.increment); + + //calculate normalized velocity reductions + float vel_lim_x = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * cos(angle); + float vel_lim_y = (max_detection_distance - distance) / (max_detection_distance - MPC_COL_PREV_D.get()) * sin(angle); + + if (vel_lim_x > 0 && vel_lim_x > _move_constraints_x_normalized(1)) { _move_constraints_x_normalized(1) = vel_lim_x; } + + if (vel_lim_y > 0 && vel_lim_y > _move_constraints_y_normalized(1)) { _move_constraints_y_normalized(1) = vel_lim_y; } + + if (vel_lim_x < 0 && -vel_lim_x > _move_constraints_x_normalized(0)) { _move_constraints_x_normalized(0) = -vel_lim_x; } + + if (vel_lim_y < 0 && -vel_lim_y > _move_constraints_y_normalized(0)) { _move_constraints_y_normalized(0) = -vel_lim_y; } + } + } + + } else if (_last_message + MESSAGE_THROTTLE_US < hrt_absolute_time()) { + mavlink_log_critical(&_mavlink_log_pub, "No range data received"); + _last_message = hrt_absolute_time(); + } +} + +void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed) +{ + reset_constraints(); + + //calculate movement constraints based on range data + update_range_constraints(); + _move_constraints_x = _move_constraints_x_normalized; + _move_constraints_y = _move_constraints_y_normalized; + + // calculate the maximum velocity along x,y axis when moving in the demanded direction + float vel_mag = original_setpoint.norm(); + float v_max_x, v_max_y; + + if (vel_mag > 0.0f) { + v_max_x = abs(max_speed / vel_mag * original_setpoint(0)); + v_max_y = abs(max_speed / vel_mag * original_setpoint(1)); + + } else { + v_max_x = 0.0f; + v_max_y = 0.0f; + } + + //scale the velocity reductions with the maximum possible velocity along the respective axis + _move_constraints_x *= v_max_x; + _move_constraints_y *= v_max_y; + + //apply the velocity reductions to form velocity limits + _move_constraints_x(0) = v_max_x - _move_constraints_x(0); + _move_constraints_x(1) = v_max_x - _move_constraints_x(1); + _move_constraints_y(0) = v_max_y - _move_constraints_y(0); + _move_constraints_y(1) = v_max_y - _move_constraints_y(1); + + //constrain the velocity setpoint to respect the velocity limits + Vector2f new_setpoint; + new_setpoint(0) = math::constrain(original_setpoint(0), -_move_constraints_x(0), _move_constraints_x(1)); + new_setpoint(1) = math::constrain(original_setpoint(1), -_move_constraints_y(0), _move_constraints_y(1)); + + //warn user if collision prevention starts to interfere + bool currently_interfering = (new_setpoint(0) < 0.95f * original_setpoint(0) + || new_setpoint(0) > 1.05f * original_setpoint(0) || new_setpoint(1) < 0.95f * original_setpoint(1) + || new_setpoint(1) > 1.05f * original_setpoint(1)); + + if (currently_interfering && (currently_interfering != _interfering)) { + mavlink_log_critical(&_mavlink_log_pub, "Collision Warning"); + } + + _interfering = currently_interfering; + + publish_constraints(original_setpoint, new_setpoint); + original_setpoint = new_setpoint; +} diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp new file mode 100644 index 000000000000..527655611de8 --- /dev/null +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -0,0 +1,107 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 CollisionPrevention.hpp + * @author Tanja Baumann + * + * CollisionPrevention controller. + * + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +using uORB::Publication; +#include +#include +#include + +class CollisionPrevention : public ModuleParams +{ +public: + CollisionPrevention(ModuleParams *parent); + + ~CollisionPrevention(); + + /** + * Initialize the uORB subscriptions using an array + * @return true on success, false on error + */ + bool initializeSubscriptions(SubscriptionArray &subscription_array); + + bool is_active() {return MPC_COL_PREV_D.get() > 0 ;} + + void modifySetpoint(matrix::Vector2f &original_setpoint, const float max_speed); + +private: + + bool _is_active = true; + bool _interfering = false; /**< states if the collision prevention interferes with the user input */ + + orb_advert_t _constraints_pub{nullptr}; /**< constraints publication */ + orb_advert_t _mavlink_log_pub = nullptr; /**< Mavlink log uORB handle */ + + uORB::Subscription *_sub_obstacle_distance{nullptr}; /**< obstacle distances received form a range sensor */ + + static constexpr uint64_t RANGE_STREAM_TIMEOUT_US = 500000; + static constexpr uint64_t MESSAGE_THROTTLE_US = 5000000; + + hrt_abstime _last_message; + + matrix::Vector2f _move_constraints_x_normalized; + matrix::Vector2f _move_constraints_y_normalized; + matrix::Vector2f _move_constraints_x; + matrix::Vector2f _move_constraints_y; + + DEFINE_PARAMETERS( + (ParamFloat) MPC_COL_PREV_D /**< collision prevention keep minimum distance */ + ) + + void update(); + + void update_range_constraints(); + + void reset_constraints(); + + void publish_constraints(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); + +}; diff --git a/src/lib/CollisionPrevention/collisionprevention_params.c b/src/lib/CollisionPrevention/collisionprevention_params.c new file mode 100644 index 000000000000..1b4175396b6a --- /dev/null +++ b/src/lib/CollisionPrevention/collisionprevention_params.c @@ -0,0 +1,52 @@ +/**************************************************************************** + * + * Copyright (c) 2018 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 collisionprevention_params.c + * + * Parameters defined by the collisionprevention lib. + * + * @author Tanja Baumann + */ + +/** + * Minimum distance the vehicle should keep to all obstacles + * + * Only used in Position mode. Collision avoidace is disable by setting this parameter to a negative value + * + * @min -1 + * @max 15 + * @unit meters + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_COL_PREV_D, -1.0f); diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp index 12bc4586dea8..6b2ec40431d8 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.cpp @@ -41,6 +41,24 @@ using namespace matrix; +FlightTaskManualPosition::FlightTaskManualPosition() : _collision_prevention(this) +{ + +} + +bool FlightTaskManualPosition::initializeSubscriptions(SubscriptionArray &subscription_array) +{ + if (!FlightTaskManualAltitude::initializeSubscriptions(subscription_array)) { + return false; + } + + if (!_collision_prevention.initializeSubscriptions(subscription_array)) { + return false; + } + + return true; +} + bool FlightTaskManualPosition::updateInitialize() { bool ret = FlightTaskManualAltitude::updateInitialize(); @@ -113,6 +131,12 @@ void FlightTaskManualPosition::_scaleSticks() /* Rotate setpoint into local frame. */ _rotateIntoHeadingFrame(vel_sp_xy); + + // collision prevention + if (_collision_prevention.is_active()) { + _collision_prevention.modifySetpoint(vel_sp_xy, _constraints.speed_xy); + } + _velocity_setpoint(0) = vel_sp_xy(0); _velocity_setpoint(1) = vel_sp_xy(1); } diff --git a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp index 04a2064fdfa1..031c5cb1a423 100644 --- a/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp +++ b/src/lib/FlightTasks/tasks/ManualPosition/FlightTaskManualPosition.hpp @@ -40,14 +40,16 @@ #pragma once +#include #include "FlightTaskManualAltitude.hpp" class FlightTaskManualPosition : public FlightTaskManualAltitude { public: - FlightTaskManualPosition() = default; + FlightTaskManualPosition(); virtual ~FlightTaskManualPosition() = default; + bool initializeSubscriptions(SubscriptionArray &subscription_array) override; bool activate() override; bool updateInitialize() override; @@ -56,6 +58,7 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude */ void setYawHandler(WeatherVane *yaw_handler) override { _weathervane_yaw_handler = yaw_handler; } + protected: void _updateXYlock(); /**< applies position lock based on stick and velocity */ void _updateSetpoints() override; @@ -74,4 +77,5 @@ class FlightTaskManualPosition : public FlightTaskManualAltitude WeatherVane *_weathervane_yaw_handler = nullptr; /**< external weathervane library, used to implement a yaw control law that turns the vehicle nose into the wind */ + CollisionPrevention _collision_prevention; /**< collision avoidance setpoint amendment */ }; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index b930dae9b5bb..fbd65614f463 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -622,6 +622,7 @@ void Logger::add_default_topics() add_topic("battery_status", 500); add_topic("camera_capture"); add_topic("camera_trigger"); + add_topic("collision_constraints"); add_topic("cpuload"); add_topic("distance_sensor", 100); add_topic("ekf2_innovations", 200); @@ -633,6 +634,7 @@ void Logger::add_default_topics() add_topic("manual_control_setpoint", 200); add_topic("mission"); add_topic("mission_result"); + add_topic("obstacle_distance"); add_topic("optical_flow", 50); add_topic("position_setpoint_triplet", 200); add_topic("radio_status"); diff --git a/src/modules/mc_pos_control/CMakeLists.txt b/src/modules/mc_pos_control/CMakeLists.txt index f60e2abe39a7..f649cecbef69 100644 --- a/src/modules/mc_pos_control/CMakeLists.txt +++ b/src/modules/mc_pos_control/CMakeLists.txt @@ -45,4 +45,5 @@ px4_add_module( git_ecl ecl_geo WeatherVane + CollisionPrevention ) diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 4ac55f0decf5..f86eb782061c 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -737,3 +737,4 @@ PARAM_DEFINE_INT32(MPC_OBS_AVOID, 0); * @group Mission */ PARAM_DEFINE_INT32(MPC_YAW_MODE, 0); +