add uORB message obstacle_distance_fused with data from offboard

obstacle_distance and distance sensor

msg/obstacle_distance.msg

@@ -15,3 +15,5 @@ uint16 min_distance # Minimum distance the sensor can measure in centimeters.
 uint16 max_distance # Maximum distance the sensor can measure in centimeters.
 
 float32 angle_offset # Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive values are offsets to the right.
+
+# TOPICS obstacle_distance obstacle_distance_fused

src/lib/CollisionPrevention/CollisionPrevention.cpp

@@ -77,6 +77,17 @@ void CollisionPrevention::publishConstrainedSetpoint(const Vector2f &original_se
 	}
 }
 
+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();
@@ -171,6 +182,8 @@ void CollisionPrevention::updateDistanceSensor(obstacle_distance_s &obstacle)
 			}
 		}
 	}
+
+	publishObstacleDistance(obstacle);
 }
 
 void CollisionPrevention::calculateConstrainedSetpoint(Vector2f &setpoint,

src/lib/CollisionPrevention/CollisionPrevention.hpp

@@ -72,6 +72,7 @@ class CollisionPrevention : public ModuleParams
 
 	orb_advert_t _constraints_pub{nullptr};  	/**< constraints publication */
 	orb_advert_t _mavlink_log_pub{nullptr};	 	/**< Mavlink log uORB handle */
+	orb_advert_t _obstacle_distance_pub{nullptr}; /**< obstacle_distance publication */
 
 	uORB::SubscriptionData<obstacle_distance_s> _sub_obstacle_distance{ORB_ID(obstacle_distance)}; /**< obstacle distances received form a range sensor */
 	uORB::Subscription _sub_distance_sensor[ORB_MULTI_MAX_INSTANCES] {{ORB_ID(distance_sensor), 0}, {ORB_ID(distance_sensor), 1}, {ORB_ID(distance_sensor), 2}, {ORB_ID(distance_sensor), 3}}; /**< distance data received from onboard rangefinders */
@@ -120,6 +121,7 @@ class CollisionPrevention : public ModuleParams
 					  const matrix::Vector2f &curr_vel);
 
 	void publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint);
+	void publishObstacleDistance(obstacle_distance_s &obstacle);
 
 	void updateOffboardObstacleDistance(obstacle_distance_s &obstacle);
 	void updateDistanceSensor(obstacle_distance_s &obstacle);