From 8c37e459de62630537b4c9206a2df67c162b72d4 Mon Sep 17 00:00:00 2001
From: Max Zimmermann <max.yvon.zimmermann@campus.tu-berlin.de>
Date: Tue, 1 Oct 2024 13:11:52 +0200
Subject: [PATCH] simulation/gz_bridge: add support for lidar altitude sensors

---
 src/modules/simulation/gz_bridge/GZBridge.cpp | 50 +++++++++++++++++--
 src/modules/simulation/gz_bridge/GZBridge.hpp |  5 +-
 2 files changed, 49 insertions(+), 6 deletions(-)

diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp
index 29d19fe11177..61bc7e14dcac 100644
--- a/src/modules/simulation/gz_bridge/GZBridge.cpp
+++ b/src/modules/simulation/gz_bridge/GZBridge.cpp
@@ -211,11 +211,20 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
-	// Laser Scan: optional
-	std::string laser_scan_topic = "/world/" + _world_name + "/model/" + _model_name + "/link/link/sensor/lidar_2d_v2/scan";
+	// Lidar 1D Distance Scan: optional
+	std::string lidar_distance_scan_topic = "/world/" + _world_name + "/model/" + _model_name +
+						"/link/base_link/sensor/distance_sensor/scan";
 
-	if (!_node.Subscribe(laser_scan_topic, &GZBridge::laserScanCallback, this)) {
-		PX4_WARN("failed to subscribe to %s", laser_scan_topic.c_str());
+	if (!_node.Subscribe(lidar_distance_scan_topic, &GZBridge::lidarDistanceScanCallback, this)) {
+		PX4_WARN("failed to subscribe to %s", lidar_distance_scan_topic.c_str());
+	}
+
+	// Lidar 2D Obstacle Scan: optional
+	std::string lidar_obstacle_scan_topic = "/world/" + _world_name + "/model/" + _model_name +
+						"/link/link/sensor/lidar_2d_v2/scan";
+
+	if (!_node.Subscribe(lidar_obstacle_scan_topic, &GZBridge::lidarObstacleScanCallback, this)) {
+		PX4_WARN("failed to subscribe to %s", lidar_obstacle_scan_topic.c_str());
 	}
 
 #if 0
@@ -763,7 +772,38 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat)
 	pthread_mutex_unlock(&_node_mutex);
 }
 
-void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan)
+void GZBridge::lidarDistanceScanCallback(const gz::msgs::LaserScan &scan)
+{
+	// Publish to uORB
+	distance_sensor_s distance_sensor {};
+
+	distance_sensor.timestamp = hrt_absolute_time();
+	distance_sensor.device_id = 0x20629a; // 0x276270: DRV_DIST_DEVTYPE_SIM, BUS: 0, ADDR: 0x62, TYPE: SIMULATION
+	distance_sensor.min_distance = scan.range_min();
+	distance_sensor.max_distance = scan.range_max();
+	distance_sensor.current_distance = 0;
+	distance_sensor.variance = 0;
+	distance_sensor.signal_quality = -1;
+	distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+	distance_sensor.mode = distance_sensor_s::MODE_ENABLED;
+
+	if (!scan.ranges().empty()) {
+		float distance = scan.ranges()[0];
+
+		if (!isinf(distance)) {
+			distance_sensor.current_distance = distance;
+			distance_sensor.signal_quality = 100;
+
+		} else {
+			distance_sensor.signal_quality = 0;
+		}
+	}
+
+	_distance_sensor_pub.publish(distance_sensor);
+}
+
+void GZBridge::lidarObstacleScanCallback(const gz::msgs::LaserScan &scan)
 {
 	static constexpr int SECTOR_SIZE_DEG = 10; // PX4 Collision Prevention only has 36 sectors of 10 degrees each
 
diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp
index fc2cb3a6e9ad..2eb225707ba3 100644
--- a/src/modules/simulation/gz_bridge/GZBridge.hpp
+++ b/src/modules/simulation/gz_bridge/GZBridge.hpp
@@ -58,6 +58,7 @@
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/vehicle_odometry.h>
 #include <uORB/topics/wheel_encoders.h>
+#include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/obstacle_distance.h>
 
 #include <gz/math.hh>
@@ -110,7 +111,8 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	void poseInfoCallback(const gz::msgs::Pose_V &pose);
 	void odometryCallback(const gz::msgs::OdometryWithCovariance &odometry);
 	void navSatCallback(const gz::msgs::NavSat &nav_sat);
-	void laserScanCallback(const gz::msgs::LaserScan &scan);
+	void lidarDistanceScanCallback(const gz::msgs::LaserScan &scan);
+	void lidarObstacleScanCallback(const gz::msgs::LaserScan &scan);
 
 	/**
 	 * @brief Call Entityfactory service
@@ -165,6 +167,7 @@ class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::S
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	//uORB::Publication<differential_pressure_s>    _differential_pressure_pub{ORB_ID(differential_pressure)};
+	uORB::Publication<distance_sensor_s>          _distance_sensor_pub{ORB_ID(distance_sensor)};
 	uORB::Publication<obstacle_distance_s>        _obstacle_distance_pub{ORB_ID(obstacle_distance)};
 	uORB::Publication<vehicle_angular_velocity_s> _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)};
 	uORB::Publication<vehicle_attitude_s>         _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)};