[gz-sim] x500_lidar for testing CollisionPrevention

ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_x500_lidar

@@ -0,0 +1,10 @@
+#!/bin/sh
+#
+# @name Gazebo x500 lidar
+#
+# @type Quadrotor
+#
+
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500_lidar}
+
+. ${R}etc/init.d-posix/airframes/4001_gz_x500

ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt

@@ -83,6 +83,7 @@ px4_add_romfs_files(
 	4009_gz_r1_rover
 	4010_gz_x500_mono_cam
 	4011_gz_lawnmower
+	4012_gz_x500_lidar
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post

platforms/posix/Debug/launch_sitl.json.in

@@ -223,6 +223,7 @@
             "options": [
               "x500",
               "x500_depth",
+              "x500_lidar",
               "rc_cessna",
               "standard_vtol",
             ],

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -196,6 +196,13 @@ 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";
+
+	if (!_node.Subscribe(laser_scan_topic, &GZBridge::laserScanCallback, this)) {
+		PX4_WARN("failed to subscribe to %s", laser_scan_topic.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 +748,87 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat)
 	pthread_mutex_unlock(&_node_mutex);
 }
 
+void GZBridge::laserScanCallback(const gz::msgs::LaserScan &scan)
+{
+	static constexpr int SECTOR_SIZE_DEG = 10; // PX4 Collision Prevention only has 36 sectors of 10 degrees each
+
+	double angle_min_deg = scan.angle_min() * 180 / M_PI;
+	double angle_step_deg = scan.angle_step() * 180 / M_PI;
+
+	int samples_per_sector = std::round(SECTOR_SIZE_DEG / angle_step_deg);
+	int number_of_sectors = scan.ranges_size() / samples_per_sector;
+
+	std::vector<double> ds_array(number_of_sectors, UINT16_MAX);
+
+	// Downsample -- take average of samples per sector
+	for (int i = 0; i < number_of_sectors; i++) {
+
+		double sum = 0;
+
+		int samples_used_in_sector = 0;
+
+		for (int j = 0; j < samples_per_sector; j++) {
+
+			double distance = scan.ranges()[i * samples_per_sector + j];
+
+			// inf values mean no object
+			if (isinf(distance)) {
+				continue;
+			}
+
+			sum += distance;
+			samples_used_in_sector++;
+		}
+
+		// If all samples in a sector are inf then it means the sector is clear
+		if (samples_used_in_sector == 0) {
+			ds_array[i] = scan.range_max();
+
+		} else {
+			ds_array[i] = sum / samples_used_in_sector;
+		}
+	}
+
+	// Publish to uORB
+	obstacle_distance_s obs {};
+
+	// Initialize unknown
+	for (auto &i : obs.distances) {
+		i = UINT16_MAX;
+	}
+
+	obs.timestamp = hrt_absolute_time();
+	obs.frame = obstacle_distance_s::MAV_FRAME_BODY_FRD;
+	obs.sensor_type = obstacle_distance_s::MAV_DISTANCE_SENSOR_LASER;
+	obs.min_distance = static_cast<uint16_t>(scan.range_min() * 100.);
+	obs.max_distance = static_cast<uint16_t>(scan.range_max() * 100.);
+	obs.angle_offset = static_cast<float>(angle_min_deg);
+	obs.increment = static_cast<float>(SECTOR_SIZE_DEG);
+
+	// Map samples in FOV into sectors in ObstacleDistance
+	int index = 0;
+
+	// Iterate in reverse because array is FLU and we need FRD
+	for (std::vector<double>::reverse_iterator i = ds_array.rbegin(); i != ds_array.rend(); ++i) {
+
+		uint16_t distance_cm = (*i) * 100.;
+
+		if (distance_cm >= obs.max_distance) {
+			obs.distances[index] = obs.max_distance + 1;
+
+		} else if (distance_cm < obs.min_distance) {
+			obs.distances[index] = 0;
+
+		} else {
+			obs.distances[index] = distance_cm;
+		}
+
+		index++;
+	}
+
+	_obstacle_distance_pub.publish(obs);
+}
+
 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

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/obstacle_distance.h>
 
 #include <gz/math.hh>
 #include <gz/msgs.hh>
@@ -67,6 +68,7 @@
 #include <gz/msgs/fluid_pressure.pb.h>
 #include <gz/msgs/model.pb.h>
 #include <gz/msgs/odometry_with_covariance.pb.h>
+#include <gz/msgs/laserscan.pb.h>
 
 using namespace time_literals;
 
@@ -106,6 +108,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 laserScanCallback(const gz::msgs::LaserScan &scan);
 
 	/**
 	*
@@ -121,6 +124,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<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)};
 	uORB::Publication<vehicle_global_position_s>  _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};