From e6e4d846fb2b88e2f5f4cd68b9a2b265cbca1b59 Mon Sep 17 00:00:00 2001 From: Martina Rivizzigno Date: Mon, 17 Jun 2019 11:52:27 +0200 Subject: [PATCH] add uORB message obstacle_distance_fused with data from offboard obstacle_distance and distance sensor --- msg/obstacle_distance.msg | 2 ++ src/lib/CollisionPrevention/CollisionPrevention.cpp | 13 +++++++++++++ src/lib/CollisionPrevention/CollisionPrevention.hpp | 2 ++ 3 files changed, 17 insertions(+) diff --git a/msg/obstacle_distance.msg b/msg/obstacle_distance.msg index d4ab2abaeb..73e9d3bc22 100644 --- a/msg/obstacle_distance.msg +++ b/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 diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index f6cb8b3d73..d1f1407695 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/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, diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 4246f563b2..83aba8a809 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -72,6 +72,7 @@ private: 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 _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 @@ private: 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);