simulator: add support for multi-distance_sensor instances (#14143)

* simulator: add support for multi-distance_sensor instances

* update submodule Tools/sitl_gazebo

* sitl_gazebo: add updated iris_obs_avoid; simulator: cleanup distance_sensor topic init

* update submodule Tools/sitl_gazebo

* simulator: delete _dist_pubs uORB::PublicationMulti<distance_sensor_s> in simulator destructor

Tools/sitl_gazebo

@@ -1 +1 @@
-Subproject commit 045156da19ac6518455c9b0e6a5a63b10388d4d7
+Subproject commit 44181fd63ebdcb9eaeb56557a3f807d501f58012

src/modules/simulator/simulator.h

@@ -45,6 +45,7 @@
 #include <battery/battery.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
+#include <drivers/drv_range_finder.h>
 #include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
 #include <lib/drivers/barometer/PX4Barometer.hpp>
 #include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
@@ -111,6 +112,10 @@ private:
 		perf_free(_perf_sim_delay);
 		perf_free(_perf_sim_interval);
 
+		for (size_t i = 0; i < sizeof(_dist_pubs) / sizeof(_dist_pubs[0]); i++) {
+			delete _dist_pubs[i];
+		}
+
 		_instance = nullptr;
 	}
 
@@ -134,11 +139,13 @@ private:
 	// uORB publisher handlers
 	uORB::Publication<battery_status_s>		_battery_pub{ORB_ID(battery_status)};
 	uORB::Publication<differential_pressure_s>	_differential_pressure_pub{ORB_ID(differential_pressure)};
-	uORB::PublicationMulti<distance_sensor_s>	_dist_pub{ORB_ID(distance_sensor)};
 	uORB::PublicationMulti<optical_flow_s>		_flow_pub{ORB_ID(optical_flow)};
 	uORB::Publication<irlock_report_s>		_irlock_report_pub{ORB_ID(irlock_report)};
 	uORB::Publication<vehicle_odometry_s>		_visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
 
+	uORB::PublicationMulti<distance_sensor_s>	*_dist_pubs[RANGE_FINDER_MAX_SENSORS] {};
+	uint8_t _dist_sensor_ids[RANGE_FINDER_MAX_SENSORS] {};
+
 	uORB::Subscription	_parameter_update_sub{ORB_ID(parameter_update)};
 
 	unsigned int _port{14560};

src/modules/simulator/simulator_mavlink.cpp

@@ -1124,7 +1124,6 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl
 	dist.orientation = dist_mavlink->orientation;
 	dist.variance = dist_mavlink->covariance * 1e-4f; // cm^2 to m^2
 	dist.signal_quality = -1;
-
 	dist.h_fov = dist_mavlink->horizontal_fov;
 	dist.v_fov = dist_mavlink->vertical_fov;
 	dist.q[0] = dist_mavlink->quaternion[0];
@@ -1132,7 +1131,21 @@ int Simulator::publish_distance_topic(const mavlink_distance_sensor_t *dist_mavl
 	dist.q[2] = dist_mavlink->quaternion[2];
 	dist.q[3] = dist_mavlink->quaternion[3];
 
-	_dist_pub.publish(dist);
+	// New publishers will be created based on the sensor ID's being different or not
+	for (size_t i = 0; i < sizeof(_dist_sensor_ids) / sizeof(_dist_sensor_ids[0]); i++) {
+		if (_dist_pubs[i] && _dist_sensor_ids[i] == dist.id) {
+			_dist_pubs[i]->publish(dist);
+			break;
+
+		}
+
+		if (_dist_pubs[i] == nullptr) {
+			_dist_pubs[i] = new uORB::PublicationMulti<distance_sensor_s> {ORB_ID(distance_sensor)};
+			_dist_sensor_ids[i] = dist.id;
+			_dist_pubs[i]->publish(dist);
+			break;
+		}
+	}
 
 	return PX4_OK;
 }