From 14aee9d7a375daff80eba28b6f19df8208e13f5e Mon Sep 17 00:00:00 2001
From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com>
Date: Wed, 17 Jul 2024 14:49:44 +0200
Subject: [PATCH] added AFBRS50 LIDAR to GZBridge

---
 src/modules/simulation/gz_bridge/GZBridge.cpp | 30 +++++++++++++++++++
 src/modules/simulation/gz_bridge/GZBridge.hpp |  3 ++
 2 files changed, 33 insertions(+)

diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp
index 56c6d028a0cd..a4cdf9fddd0e 100644
--- a/src/modules/simulation/gz_bridge/GZBridge.cpp
+++ b/src/modules/simulation/gz_bridge/GZBridge.cpp
@@ -196,6 +196,14 @@ int GZBridge::init()
 		return PX4_ERROR;
 	}
 
+	// Distance Sensor(AFBRS50): optional
+	std::string lidar_sensor = "/world/" + _world_name + "/model/" + _model_name +
+				   "/link/BroadcomSensor/sensor/broadcomafbrs50/scan";
+
+	if (!_node.Subscribe(lidar_sensor, &GZBridge::LaserScantoLidarSensorCallback, this)) {
+		PX4_WARN("failed to subscribe to %s", lidar_sensor.c_str());
+	}
+
 #if 0
 	// Airspeed: /world/$WORLD/model/$MODEL/link/airspeed_link/sensor/air_speed/air_speed
 	std::string airpressure_topic = "/world/" + _world_name + "/model/" + _model_name +
@@ -741,6 +749,28 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat)
 	pthread_mutex_unlock(&_node_mutex);
 }
 
+void GZBridge::LaserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan)
+{
+	distance_sensor_s distance_sensor{};
+	distance_sensor.timestamp = hrt_absolute_time();
+	distance_sensor.device_id = 0;
+	distance_sensor.min_distance = scan.range_min();
+	distance_sensor.max_distance = scan.range_max();
+	distance_sensor.current_distance = scan.ranges()[0];
+	distance_sensor.variance = 0.0f;
+	distance_sensor.signal_quality = -1;
+	distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+	distance_sensor.h_fov = scan.angle_max() - scan.angle_min();
+	distance_sensor.v_fov = scan.angle_max() - scan.angle_min();
+	distance_sensor.q[0] = scan.world_pose().orientation().x();
+	distance_sensor.q[1] = scan.world_pose().orientation().y();
+	distance_sensor.q[2] = scan.world_pose().orientation().z();
+	distance_sensor.q[3] = scan.world_pose().orientation().w();
+	distance_sensor.orientation = distance_sensor_s::ROTATION_CUSTOM;
+
+	_distance_sensor_pub.publish(distance_sensor);
+}
+
 void GZBridge::rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU)
 {
 	// FLU (ROS) to FRD (PX4) static rotation
diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp
index 6d2e2670a15e..19d11771bb0c 100644
--- a/src/modules/simulation/gz_bridge/GZBridge.hpp
+++ b/src/modules/simulation/gz_bridge/GZBridge.hpp
@@ -49,6 +49,7 @@
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/distance_sensor.h>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
@@ -106,6 +107,7 @@ 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 LaserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan);
 
 	/**
 	*
@@ -121,6 +123,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<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)};
 	uORB::Publication<vehicle_global_position_s>  _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};