From b0f82ee8ac50e4246fb10d957bb962b6fe2073ff Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 2 Sep 2019 15:35:34 -0400 Subject: [PATCH] CollisionPrevention: update orb_publish to uORB::Publication<> --- .../CollisionPrevention.cpp | 66 ++++--------------- .../CollisionPrevention.hpp | 42 ++++++------ 2 files changed, 31 insertions(+), 77 deletions(-) diff --git a/src/lib/CollisionPrevention/CollisionPrevention.cpp b/src/lib/CollisionPrevention/CollisionPrevention.cpp index bc5ce95aa1..6bc246e038 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.cpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.cpp @@ -56,51 +56,6 @@ CollisionPrevention::~CollisionPrevention() 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) @@ -197,7 +152,8 @@ void CollisionPrevention::_updateDistanceSensor(obstacle_distance_s &obstacle) } } - _publishObstacleDistance(obstacle); + // publish fused obtacle distance message with data from offboard obstacle_distance and distance sensor + _obstacle_distance_pub.publish(obstacle); } void CollisionPrevention::_calculateConstrainedSetpoint(Vector2f &setpoint, @@ -298,13 +254,21 @@ void CollisionPrevention::modifySetpoint(Vector2f &original_setpoint, const floa } _interfering = currently_interfering; - _publishConstrainedSetpoint(original_setpoint, new_setpoint); + + // publish constraints + collision_constraints_s constraints{}; + constraints.timestamp = hrt_absolute_time(); + original_setpoint.copyTo(constraints.original_setpoint); + new_setpoint.copyTo(constraints.adapted_setpoint); + _constraints_pub.publish(constraints); + original_setpoint = new_setpoint; } void CollisionPrevention::_publishVehicleCmdDoLoiter() { vehicle_command_s command{}; + command.timestamp = hrt_absolute_time(); command.command = vehicle_command_s::VEHICLE_CMD_DO_SET_MODE; command.param1 = (float)1; // base mode command.param3 = (float)0; // sub mode @@ -318,11 +282,5 @@ void CollisionPrevention::_publishVehicleCmdDoLoiter() 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); - } + _vehicle_command_pub.publish(command); } diff --git a/src/lib/CollisionPrevention/CollisionPrevention.hpp b/src/lib/CollisionPrevention/CollisionPrevention.hpp index 525fa09b34..648cd2b038 100644 --- a/src/lib/CollisionPrevention/CollisionPrevention.hpp +++ b/src/lib/CollisionPrevention/CollisionPrevention.hpp @@ -41,20 +41,23 @@ #pragma once -#include #include + +#include +#include +#include #include -#include -#include +#include +#include +#include +#include +#include #include +#include +#include +#include #include #include -#include -#include -#include -#include -#include -#include class CollisionPrevention : public ModuleParams { @@ -81,10 +84,11 @@ private: 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 */ - orb_advert_t _obstacle_distance_pub{nullptr}; /**< obstacle_distance publication */ - orb_advert_t _pub_vehicle_command{nullptr}; /**< vehicle command do publication */ + + uORB::Publication _constraints_pub{ORB_ID(collision_constraints)}; /**< constraints publication */ + uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance_fused)}; /**< obstacle_distance publication */ + uORB::PublicationQueued _vehicle_command_pub{ORB_ID(vehicle_command)}; /**< vehicle command do 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 */ @@ -140,27 +144,19 @@ private: */ void _calculateConstrainedSetpoint(matrix::Vector2f &setpoint, const matrix::Vector2f &curr_pos, const matrix::Vector2f &curr_vel); - /** - * Publishes collision_constraints message - * @param original_setpoint, setpoint before collision prevention intervention - * @param adapted_setpoint, collision prevention adaped setpoint - */ - void _publishConstrainedSetpoint(const matrix::Vector2f &original_setpoint, const matrix::Vector2f &adapted_setpoint); - /** - * Publishes obstacle_distance message with fused data from offboard and from distance sensors - * @param obstacle, obstacle_distance message to be publsihed - */ - void _publishObstacleDistance(obstacle_distance_s &obstacle); + /** * Updates obstacle distance message with measurement from offboard * @param obstacle, obstacle_distance message to be updated */ void _updateOffboardObstacleDistance(obstacle_distance_s &obstacle); + /** * Updates obstacle distance message with measurement from distance sensors * @param obstacle, obstacle_distance message to be updated */ void _updateDistanceSensor(obstacle_distance_s &obstacle); + /** * Publishes vehicle command. */