Add a collision avoidance library which uses range data

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

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
+

src/lib/CMakeLists.txt

@@ -56,3 +56,4 @@ add_subdirectory(terrain_estimation)
 add_subdirectory(tunes)
 add_subdirectory(version)
 add_subdirectory(WeatherVane)
+add_subdirectory(CollisionPrevention)

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)

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 <CollisionPrevention/CollisionPrevention.hpp>
+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;
+}

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 <tanja@auterion.com>
+ *
+ * CollisionPrevention controller.
+ *
+ */
+
+#pragma once
+
+#include <px4_module_params.h>
+#include <float.h>
+#include <matrix/matrix/math.hpp>
+#include <uORB/topics/obstacle_distance.h>
+#include <uORB/topics/collision_constraints.h>
+#include <mathlib/mathlib.h>
+#include <drivers/drv_hrt.h>
+#include <uORB/topics/mavlink_log.h>
+#include <uORB/Publication.hpp>
+using uORB::Publication;
+#include <uORB/uORB.h>
+#include <systemlib/mavlink_log.h>
+#include <lib/FlightTasks/tasks/FlightTask/SubscriptionArray.hpp>
+
+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<obstacle_distance_s> *_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<px4::params::MPC_COL_PREV_D>) 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);
+
+};