use subscription array in ObstacleAvoidance library

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.cpp

@@ -42,6 +42,12 @@ using namespace matrix;
 
 static constexpr float SIGMA_NORM	= 0.001f;
 
+FlightTaskAuto::FlightTaskAuto() :
+	_obstacle_avoidance(this)
+{
+
+}
+
 bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_array)
 {
 	if (!FlightTask::initializeSubscriptions(subscription_array)) {
@@ -60,6 +66,10 @@ bool FlightTaskAuto::initializeSubscriptions(SubscriptionArray &subscription_arr
 		return false;
 	}
 
+	if (!_obstacle_avoidance.initializeSubscriptions(subscription_array)) {
+		return false;
+	}
+
 	return true;
 }
 

src/lib/FlightTasks/tasks/Auto/FlightTaskAuto.hpp

@@ -46,6 +46,8 @@
 #include <uORB/topics/position_controller_status.h>
 #include <uORB/topics/vehicle_status.h>
 #include <lib/ecl/geo/geo.h>
+#include "lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp"
+
 
 /**
  * This enum has to agree with position_setpoint_s type definition
@@ -73,7 +75,7 @@ enum class State {
 class FlightTaskAuto : public FlightTask
 {
 public:
-	FlightTaskAuto() = default;
+	FlightTaskAuto();
 
 	virtual ~FlightTaskAuto() = default;
 	bool initializeSubscriptions(SubscriptionArray &subscription_array) override;
@@ -84,6 +86,7 @@ public:
 	 * Sets an external yaw handler which can be used to implement a different yaw control strategy.
 	 */
 	void setYawHandler(WeatherVane *ext_yaw_handler) override {_ext_yaw_handler = ext_yaw_handler;}
+	ObstacleAvoidance _obstacle_avoidance;
 
 protected:
 	void _setDefaultConstraints() override;
@@ -113,6 +116,8 @@ protected:
 					(ParamInt<px4::params::COM_OBS_AVOID>) _param_com_obs_avoid // obstacle avoidance active
 				       );
 
+
+
 private:
 	matrix::Vector2f _lock_position_xy{NAN, NAN}; /**< if no valid triplet is received, lock positition to current position */
 	bool _yaw_lock = false; /**< if within acceptance radius, lock yaw to current yaw */

src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.cpp

@@ -40,11 +40,7 @@
 
 using namespace matrix;
 
-FlightTaskAutoMapper2::FlightTaskAutoMapper2() :
-	_obstacle_avoidance(this)
-{
 
-}
 
 bool FlightTaskAutoMapper2::activate()
 {

src/lib/FlightTasks/tasks/AutoMapper2/FlightTaskAutoMapper2.hpp

@@ -41,12 +41,11 @@
 #pragma once
 
 #include "FlightTaskAuto.hpp"
-#include "lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp"
 
 class FlightTaskAutoMapper2 : public FlightTaskAuto
 {
 public:
-	FlightTaskAutoMapper2();
+	FlightTaskAutoMapper2() = default;
 	virtual ~FlightTaskAutoMapper2() = default;
 	bool activate() override;
 	bool update() override;
@@ -81,5 +80,5 @@ private:
 	void _reset(); /**< Resets member variables to current vehicle state */
 	WaypointType _type_previous{WaypointType::idle}; /**< Previous type of current target triplet. */
 	bool _highEnoughForLandingGear(); /**< Checks if gears can be lowered. */
-	ObstacleAvoidance _obstacle_avoidance;
+
 };

src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.cpp

@@ -52,6 +52,15 @@ ObstacleAvoidance::ObstacleAvoidance(ModuleParams *parent) :
 
 }
 
+bool ObstacleAvoidance::initializeSubscriptions(SubscriptionArray &subscription_array)
+{
+	if (!subscription_array.get(ORB_ID(vehicle_trajectory_waypoint), _sub_vehicle_trajectory_waypoint)) {
+		return false;
+	}
+
+	return true;
+}
+
 void ObstacleAvoidance::prepareAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &vel_sp, float &yaw_sp,
 		float &yaw_speed_sp)
 {
@@ -60,16 +69,15 @@ void ObstacleAvoidance::prepareAvoidanceSetpoints(Vector3f &pos_sp, Vector3f &ve
 		return;
 	}
 
-	_sub_vehicle_trajectory_waypoint.update();
-	const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint.get().timestamp) >
-					    TRAJECTORY_STREAM_TIMEOUT_US;
+	const bool avoidance_data_timeout = hrt_elapsed_time((hrt_abstime *)&_sub_vehicle_trajectory_waypoint->get().timestamp)
+					    > TRAJECTORY_STREAM_TIMEOUT_US;
 	const bool avoidance_point_valid =
-		_sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
+		_sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].point_valid == true;
 
 	if (!avoidance_data_timeout && avoidance_point_valid) {
-		pos_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position;
-		vel_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity;
-		yaw_sp =  _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw;
-		yaw_speed_sp = _sub_vehicle_trajectory_waypoint.get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
+		pos_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].position;
+		vel_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].velocity;
+		yaw_sp =  _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw;
+		yaw_speed_sp = _sub_vehicle_trajectory_waypoint->get().waypoints[vehicle_trajectory_waypoint_s::POINT_0].yaw_speed;
 	}
 }

src/lib/FlightTasks/tasks/Utility/ObstacleAvoidance.hpp

@@ -53,11 +53,13 @@ public:
 	ObstacleAvoidance(ModuleParams *parent);
 	~ObstacleAvoidance() = default;
 
+	bool initializeSubscriptions(SubscriptionArray &subscription_array);
+
 	void prepareAvoidanceSetpoints(matrix::Vector3f &pos_sp, matrix::Vector3f &vel_sp, float &yaw_sp, float &yaw_speed_sp);
 
 private:
 
-	uORB::Subscription<vehicle_trajectory_waypoint_s> _sub_vehicle_trajectory_waypoint{ORB_ID(vehicle_trajectory_waypoint)};
+	uORB::Subscription<vehicle_trajectory_waypoint_s> *_sub_vehicle_trajectory_waypoint{nullptr};
 
   DEFINE_PARAMETERS(
     (ParamInt<px4::params::COM_OBS_AVOID>) COM_OBS_AVOID             /**< obstacle avoidance enabled */