SIM: MPC: add two x500 lidar airframes (PX4#23879)

* add two x500 lidar airframe, including the gazebo bridge for the lidar sensors and their orientation * Update src/modules/simulation/gz_bridge/GZBridge.cpp Co-authored-by: Jacob Dahl <[email protected]> * Update src/modules/simulation/gz_bridge/GZBridge.hpp Co-authored-by: Jacob Dahl <[email protected]> * update submodule --------- Co-authored-by: Jacob Dahl <[email protected]>
NXPHoverGames · Nov 13, 2024 · d14c054 · d14c054
1 parent 1e23f25
commit d14c054
Show file tree

Hide file tree

Showing 7 changed files with 81 additions and 4 deletions.
diff --git a/...init.d-posix/airframes/4013_gz_x500_lidar → ...t.d-posix/airframes/4013_gz_x500_lidar_2d b/...init.d-posix/airframes/4013_gz_x500_lidar → ...t.d-posix/airframes/4013_gz_x500_lidar_2d
@@ -1,10 +1,10 @@
 #!/bin/sh
 #
-# @name Gazebo x500 lidar
+# @name Gazebo x500 lidar 2d
 #
 # @type Quadrotor
 #
 
-PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_lidar}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_lidar_2d}
 
 . ${R}etc/init.d-posix/airframes/4001_gz_x500
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4016_gz_x500_lidar_down b/ROMFS/px4fmu_common/init.d-posix/airframes/4016_gz_x500_lidar_down
@@ -0,0 +1,10 @@
+#!/bin/sh
+#
+# @name Gazebo x500 lidar down
+#
+# @type Quadrotor
+#
+
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_lidar_down}
+
+. ${R}etc/init.d-posix/airframes/4001_gz_x500
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4017_gz_x500_lidar_front b/ROMFS/px4fmu_common/init.d-posix/airframes/4017_gz_x500_lidar_front
@@ -0,0 +1,10 @@
+#!/bin/sh
+#
+# @name Gazebo x500 lidar front
+#
+# @type Quadrotor
+#
+
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_lidar_front}
+
+. ${R}etc/init.d-posix/airframes/4001_gz_x500
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
@@ -84,9 +84,11 @@ px4_add_romfs_files(
 	4010_gz_x500_mono_cam
 	4011_gz_lawnmower
 	4012_gz_rover_ackermann
-	4013_gz_x500_lidar
+	4013_gz_x500_lidar_2d
 	4014_gz_x500_mono_cam_down
 	4015_gz_r1_rover_mecanum
+	4016_gz_x500_lidar_down
+	4017_gz_x500_lidar_front
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

diff --git a/Tools/simulation/gz b/Tools/simulation/gz
diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp
@@ -218,6 +218,14 @@ int GZBridge::init()
 		PX4_WARN("failed to subscribe to %s", laser_scan_topic.c_str());
 	}
 
+	// Distance Sensor(AFBRS50): optional
+	std::string lidar_sensor = "/world/" + _world_name + "/model/" + _model_name +
+				   "/link/lidar_sensor_link/sensor/lidar/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 +
@@ -762,6 +770,49 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat)
 
 	pthread_mutex_unlock(&_node_mutex);
 }
+void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan)
+{
+	if (hrt_absolute_time() == 0) {
+		return;
+	}
+
+	distance_sensor_s distance_sensor{};
+	distance_sensor.timestamp = hrt_absolute_time();
+	device::Device::DeviceId id;
+	id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION;
+	id.devid_s.bus = 0;
+	id.devid_s.address = 0;
+	id.devid_s.devtype = DRV_DIST_DEVTYPE_SIM;
+	distance_sensor.device_id = id.devid;
+	distance_sensor.min_distance = static_cast<float>(scan.range_min());
+	distance_sensor.max_distance = static_cast<float>(scan.range_max());
+	distance_sensor.current_distance = static_cast<float>(scan.ranges()[0]);
+	distance_sensor.variance = 0.0f;
+	distance_sensor.signal_quality = -1;
+	distance_sensor.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
+
+	gz::msgs::Quaternion pose_orientation = scan.world_pose().orientation();
+	gz::math::Quaterniond q_sensor = gz::math::Quaterniond(
+			pose_orientation.w(),
+			pose_orientation.x(),
+			pose_orientation.y(),
+			pose_orientation.z());
+
+	const gz::math::Quaterniond q_front(0.7071068, 0.7071068, 0, 0);
+	const gz::math::Quaterniond q_down(0, 1, 0, 0);
+
+	if (q_sensor.Equal(q_front, 0.03)) {
+		distance_sensor.orientation = distance_sensor_s::ROTATION_FORWARD_FACING;
+
+	} else if (q_sensor.Equal(q_down, 0.03)) {
+		distance_sensor.orientation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+
+	} else {
+		distance_sensor.orientation = distance_sensor_s::ROTATION_CUSTOM;
+	}
+
+	_distance_sensor_pub.publish(distance_sensor);
+}
 
 void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan)
 {

diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp
@@ -49,6 +49,8 @@
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/distance_sensor.h>
+#include <lib/drivers/device/Device.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
@@ -110,6 +112,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);
 	void laserScanCallback(const gz::msgs::LaserScan &scan);
 
 	/**
@@ -165,6 +168,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)};