Simulation Gazebo: Use Gazebo Airpressure sensor (#21176)

* Simulation Gazebo: Use Gazebo Airpressure sensor

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

* Fixed build

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

* Added feedback

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

* make linters happy

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

---------

Signed-off-by: Alejandro Hernández Cordero <ahcorde@gmail.com>

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -157,12 +157,20 @@ int GZBridge::init()
 	std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name +
 					"/link/airspeed_link/sensor/air_speed/air_speed";
 
-	if (!_node.Subscribe(airpressure_topic, &GZBridge::airpressureCallback, this)) {
+	if (!_node.Subscribe(airpressure_topic, &GZBridge::airspeedCallback, this)) {
 		PX4_ERR("failed to subscribe to %s", airpressure_topic.c_str());
 		return PX4_ERROR;
 	}
 
 #endif
+	// Air pressure: /world/$WORLD/model/$MODEL/link/base_link/sensor/air_pressure_sensor/air_pressure
+	std::string air_pressure_topic = "/world/" + _world_name + "/model/" + _model_name +
+					 "/link/base_link/sensor/air_pressure_sensor/air_pressure";
+
+	if (!_node.Subscribe(air_pressure_topic, &GZBridge::barometerCallback, this)) {
+		PX4_ERR("failed to subscribe to %s", air_pressure_topic.c_str());
+		return PX4_ERROR;
+	}
 
 	if (!_mixing_interface_esc.init(_model_name)) {
 		PX4_ERR("failed to init ESC output");
@@ -185,6 +193,7 @@ int GZBridge::task_spawn(int argc, char *argv[])
 	const char *model_pose = nullptr;
 	const char *model_sim = nullptr;
 	const char *px4_instance = nullptr;
+	std::string model_name_std;
 
 
 	bool error_flag = false;
@@ -248,7 +257,7 @@ int GZBridge::task_spawn(int argc, char *argv[])
 		}
 
 	} else if (!model_name) {
-		std::string model_name_std = std::string(model_sim) + "_" + std::string(px4_instance);
+		model_name_std = std::string(model_sim) + "_" + std::string(px4_instance);
 		model_name = model_name_std.c_str();
 	}
 
@@ -329,28 +338,53 @@ void GZBridge::clockCallback(const gz::msgs::Clock &clock)
 	pthread_mutex_unlock(&_node_mutex);
 }
 
-#if 0
-void GZBridge::airpressureCallback(const gz::msgs::FluidPressure &air_pressure)
+void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure)
 {
 	if (hrt_absolute_time() == 0) {
 		return;
 	}
 
-	pthread_mutex_lock(&_mutex);
+	pthread_mutex_lock(&_node_mutex);
 
 	const uint64_t time_us = (air_pressure.header().stamp().sec() * 1000000)
 				 + (air_pressure.header().stamp().nsec() / 1000);
 
-	double air_pressure_value = air_pressure.pressure();
+	// publish
+	sensor_baro_s sensor_baro{};
+	sensor_baro.timestamp_sample = time_us;
+	sensor_baro.device_id = 6620172; // 6620172: DRV_BARO_DEVTYPE_BAROSIM, BUS: 1, ADDR: 4, TYPE: SIMULATION
+	sensor_baro.pressure = air_pressure.pressure();
+	sensor_baro.temperature = this->_temperature;
+	sensor_baro.timestamp = hrt_absolute_time();
+	_sensor_baro_pub.publish(sensor_baro);
+
+	pthread_mutex_unlock(&_node_mutex);
+}
+
+#if 0
+void GZBridge::airspeedCallback(const gz::msgs::AirSpeedSensor &air_speed)
+{
+	if (hrt_absolute_time() == 0) {
+		return;
+	}
+
+	pthread_mutex_lock(&_node_mutex);
+
+	const uint64_t time_us = (air_speed.header().stamp().sec() * 1000000)
+				 + (air_speed.header().stamp().nsec() / 1000);
+
+	double air_speed_value = air_speed.diff_pressure();
 
 	differential_pressure_s report{};
 	report.timestamp_sample = time_us;
 	report.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
-	report.differential_pressure_pa = static_cast<float>(air_pressure_value); // hPa to Pa;
-	report.temperature = static_cast<float>(air_pressure.variance()) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // K to C
+	report.differential_pressure_pa = static_cast<float>(air_speed_value); // hPa to Pa;
+	report.temperature = static_cast<float>(air_speed.temperature()) + CONSTANTS_ABSOLUTE_NULL_CELSIUS; // K to C
 	report.timestamp = hrt_absolute_time();;
 	_differential_pressure_pub.publish(report);
 
+	this->_temperature = report.temperature;
+
 	pthread_mutex_unlock(&_node_mutex);
 }
 #endif

src/modules/simulation/gz_bridge/GZBridge.hpp

@@ -54,13 +54,14 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/sensor_baro.h>
 
 #include <gz/math.hh>
 #include <gz/msgs.hh>
 #include <gz/transport.hh>
 
-// #include <gz/msgs/fluid_pressure.pb.h>
 #include <gz/msgs/imu.pb.h>
+#include <gz/msgs/fluid_pressure.pb.h>
 
 using namespace time_literals;
 
@@ -94,7 +95,8 @@ private:
 
 	void clockCallback(const gz::msgs::Clock &clock);
 
-	//void airpressureCallback(const gz::msgs::FluidPressure &air_pressure);
+	// void airspeedCallback(const gz::msgs::AirSpeedSensor &air_pressure);
+	void barometerCallback(const gz::msgs::FluidPressure &air_pressure);
 	void imuCallback(const gz::msgs::IMU &imu);
 	void poseInfoCallback(const gz::msgs::Pose_V &pose);
 
@@ -106,6 +108,7 @@ private:
 	uORB::Publication<vehicle_attitude_s>         _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};
 	uORB::Publication<vehicle_global_position_s>  _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};
 	uORB::Publication<vehicle_local_position_s>   _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
+	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
 
 	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
 	uORB::PublicationMulti<sensor_gyro_s>  _sensor_gyro_pub{ORB_ID(sensor_gyro)};
@@ -129,6 +132,8 @@ private:
 	const std::string _model_sim;
 	const std::string _model_pose;
 
+	float _temperature{288.15};  // 15 degrees
+
 	gz::transport::Node _node;
 
 	DEFINE_PARAMETERS(