PX4-Autopilot/src/lib/CollisionPrevention/CollisionPrevention.cpp

/****************************************************************************
 *
 *   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 (_mavlink_log_pub != nullptr) {
		orb_unadvertise(_mavlink_log_pub);
	}

	if (_constraints_pub != nullptr) {
		orb_unadvertise(_constraints_pub);
	}

	if (_obstacle_distance_pub != nullptr) {
		orb_unadvertise(_obstacle_distance_pub);
	}

	if (_pub_vehicle_command != nullptr) {
		orb_unadvertise(_pub_vehicle_command);
	}
}

void CollisionPrevention::_publishConstrainedSetpoint(const Vector2f &original_setpoint,
		const Vector2f &adapted_setpoint)
{
	collision_constraints_s	constraints{};	/**< collision constraints message */

	//fill in values
	constraints.timestamp = hrt_absolute_time();

	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::_publishObstacleDistance(obstacle_distance_s &obstacle)
{
	// publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor
	if (_obstacle_distance_pub != nullptr) {
		orb_publish(ORB_ID(obstacle_distance_fused), _obstacle_distance_pub, &obstacle);

	} else {
		_obstacle_distance_pub = orb_advertise(ORB_ID(obstacle_distance_fused), &obstacle);
	}
}

void CollisionPrevention::_updateOffboardObstacleDistance(obstacle_distance_s &obstacle)
{
	_sub_obstacle_distance.update();
	const obstacle_distance_s &obstacle_distance = _sub_obstacle_distance.get();

	// Update with offboard data if the data is not stale
	if (hrt_elapsed_time(&obstacle_distance.timestamp) < RANGE_STREAM_TIMEOUT_US) {
		obstacle = obstacle_distance;
	}
}

void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle)
{
	for (unsigned i = 0; i < ORB_MULTI_MAX_INSTANCES; i++) {
		distance_sensor_s distance_sensor {};
		_sub_distance_sensor[i].copy(&distance_sensor);

		// consider only instaces with updated, valid data and orientations useful for collision prevention
		if ((hrt_elapsed_time(&distance_sensor.timestamp) < RANGE_STREAM_TIMEOUT_US) &&
		    (distance_sensor.orientation != distance_sensor_s::ROTATION_DOWNWARD_FACING) &&
		    (distance_sensor.orientation != distance_sensor_s::ROTATION_UPWARD_FACING)) {


			if (obstacle.increment > 0) {
				// data from companion
				obstacle.timestamp = math::max(obstacle.timestamp, distance_sensor.timestamp);
				obstacle.max_distance = math::max((int)obstacle.max_distance,
								  (int)distance_sensor.max_distance * 100);
				obstacle.min_distance = math::min((int)obstacle.min_distance,
								  (int)distance_sensor.min_distance * 100);
				// since the data from the companion are already in the distances data structure,
				// keep the increment that is sent
				obstacle.angle_offset = 0.f; //companion not sending this field (needs mavros update)

			} else {
				obstacle.timestamp = distance_sensor.timestamp;
				obstacle.max_distance = distance_sensor.max_distance * 100; // convert to cm
				obstacle.min_distance = distance_sensor.min_distance * 100; // convert to cm
				memset(&obstacle.distances[0], 0xff, sizeof(obstacle.distances));
				obstacle.increment = math::degrees(distance_sensor.h_fov);
				obstacle.angle_offset = 0.f;
			}

			if ((distance_sensor.current_distance > distance_sensor.min_distance) &&
			    (distance_sensor.current_distance < distance_sensor.max_distance)) {

				float sensor_yaw_body_rad = _sensorOrientationToYawOffset(distance_sensor, obstacle.angle_offset);

				matrix::Quatf attitude = Quatf(_sub_vehicle_attitude.get().q);
				// convert the sensor orientation from body to local frame in the range [0, 360]
				float sensor_yaw_local_deg  = math::degrees(wrap_2pi(Eulerf(attitude).psi() + sensor_yaw_body_rad));

				// calculate the field of view boundary bin indices
				int lower_bound = (int)floor((sensor_yaw_local_deg  - math::degrees(distance_sensor.h_fov / 2.0f)) /
							     obstacle.increment);
				int upper_bound = (int)floor((sensor_yaw_local_deg  + math::degrees(distance_sensor.h_fov / 2.0f)) /
							     obstacle.increment);

				// if increment is lower than 5deg, use an offset
				const int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]);

				if (((lower_bound < 0 || upper_bound < 0) || (lower_bound >= distances_array_size
						|| upper_bound >= distances_array_size)) && obstacle.increment < 5.f) {
					obstacle.angle_offset = sensor_yaw_local_deg ;
					upper_bound  = abs(upper_bound - lower_bound);
					lower_bound  = 0;
				}

				// rotate vehicle attitude into the sensor body frame
				matrix::Quatf attitude_sensor_frame = attitude;
				attitude_sensor_frame.rotate(Vector3f(0.f, 0.f, sensor_yaw_body_rad));
				float attitude_sensor_frame_pitch = cosf(Eulerf(attitude_sensor_frame).theta());

				for (int bin = lower_bound; bin <= upper_bound; ++bin) {
					int wrap_bin = bin;

					if (wrap_bin < 0) {
						// wrap bin index around the array
						wrap_bin = (int)floor(360.f / obstacle.increment) + bin;
					}

					if (wrap_bin >= distances_array_size) {
						// wrap bin index around the array
						wrap_bin = bin - distances_array_size;
					}

					// compensate measurement for vehicle tilt and convert to cm
					obstacle.distances[wrap_bin] = math::min((int)obstacle.distances[wrap_bin],
								       (int)(100 * distance_sensor.current_distance * attitude_sensor_frame_pitch));
				}
			}
		}
	}

	_publishObstacleDistance(obstacle);
}

void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint,
		const Vector2f &curr_pos, const Vector2f &curr_vel)
{
	obstacle_distance_s obstacle{};
	_updateOffboardObstacleDistance(obstacle);
	_updateDistanceSensor(obstacle);

	float setpoint_length = setpoint.norm();

	if (hrt_elapsed_time(&obstacle.timestamp) < RANGE_STREAM_TIMEOUT_US) {
		if (setpoint_length > 0.001f) {

			Vector2f setpoint_dir = setpoint / setpoint_length;
			float vel_max = setpoint_length;
			int distances_array_size = sizeof(obstacle.distances) / sizeof(obstacle.distances[0]);

			for (int i = 0; i < distances_array_size; i++) {

				if (obstacle.distances[i] < obstacle.max_distance &&
				    obstacle.distances[i] > obstacle.min_distance && (float)i * obstacle.increment < 360.f) {
					float distance = obstacle.distances[i] / 100.0f; //convert to meters
					float angle = math::radians((float)i * obstacle.increment);

					if (obstacle.angle_offset > 0.f) {
						angle += math::radians(obstacle.angle_offset);
					}

					//check if the bin must be considered regarding the given stick input
					Vector2f bin_direction = {cos(angle), sin(angle)};

					if (setpoint_dir.dot(bin_direction) > 0
					    && setpoint_dir.dot(bin_direction) > cosf(math::radians(_param_mpc_col_prev_ang.get()))) {
						//calculate max allowed velocity with a P-controller (same gain as in the position controller)
						float curr_vel_parallel = math::max(0.f, curr_vel.dot(bin_direction));
						float delay_distance = curr_vel_parallel * _param_mpc_col_prev_dly.get();
						float vel_max_posctrl = math::max(0.f,
										  _param_mpc_xy_p.get() * (distance - _param_mpc_col_prev_d.get() - delay_distance));
						Vector2f  vel_max_vec = bin_direction * vel_max_posctrl;
						float vel_max_bin = vel_max_vec.dot(setpoint_dir);

						//constrain the velocity
						if (vel_max_bin >= 0) {
							vel_max = math::min(vel_max, vel_max_bin);
						}
					}
				}
			}

			setpoint = setpoint_dir * vel_max;
		}

	} else {
		// if distance data are stale, switch to Loiter
		_publishVehicleCmdDoLoiter();
		mavlink_log_critical(&_mavlink_log_pub, "No range data received, loitering.");
	}
}

void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const float max_speed,
		const Vector2f &curr_pos, const Vector2f &curr_vel)
{
	//calculate movement constraints based on range data
	Vector2f new_setpoint = original_setpoint;
	_calculateConstrainedSetpoint(new_setpoint, curr_pos, curr_vel);

	//warn user if collision prevention starts to interfere
	bool currently_interfering = (new_setpoint(0) < original_setpoint(0) - 0.05f * max_speed
				      || new_setpoint(0) > original_setpoint(0) + 0.05f * max_speed
				      || new_setpoint(1) < original_setpoint(1) - 0.05f * max_speed
				      || new_setpoint(1) > original_setpoint(1) + 0.05f * max_speed);

	if (currently_interfering && (currently_interfering != _interfering)) {
		mavlink_log_critical(&_mavlink_log_pub, "Collision Warning");
	}

	_interfering = currently_interfering;
	_publishConstrainedSetpoint(original_setpoint, new_setpoint);
	original_setpoint = new_setpoint;
}

void CollisionPrevention::_publishVehicleCmdDoLoiter()
{
	vehicle_command_s command{};
	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
	if (_pub_vehicle_command == nullptr) {
		_pub_vehicle_command = orb_advertise_queue(ORB_ID(vehicle_command), &command,
				       vehicle_command_s::ORB_QUEUE_LENGTH);

	} else {
		orb_publish(ORB_ID(vehicle_command), _pub_vehicle_command, &command);
	}
}