From fe5a07a96de2085bfb22a3e2339d906f48d398cf Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Mon, 13 May 2024 17:23:55 -0600 Subject: [PATCH] gz: added x500_lidar model for publishing obstacle_distance --- .../init.d-posix/airframes/4013_gz_x500_lidar | 10 +++ .../init.d-posix/airframes/CMakeLists.txt | 1 + platforms/posix/Debug/launch_sitl.json.in | 1 + src/modules/simulation/gz_bridge/GZBridge.cpp | 89 +++++++++++++++++++ src/modules/simulation/gz_bridge/GZBridge.hpp | 4 + 5 files changed, 105 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar b/ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar new file mode 100644 index 0000000000..a316310abe --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4013_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 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt index 33d6d857e8..9235b2e663 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt @@ -84,6 +84,7 @@ px4_add_romfs_files( 4010_gz_x500_mono_cam 4011_gz_lawnmower 4012_gz_rover_ackermann + 4013_gz_x500_lidar 6011_gazebo-classic_typhoon_h480 6011_gazebo-classic_typhoon_h480.post diff --git a/platforms/posix/Debug/launch_sitl.json.in b/platforms/posix/Debug/launch_sitl.json.in index 2fb0a4bc6b..6703f42903 100644 --- a/platforms/posix/Debug/launch_sitl.json.in +++ b/platforms/posix/Debug/launch_sitl.json.in @@ -223,6 +223,7 @@ "options": [ "x500", "x500_depth", + "x500_lidar", "rc_cessna", "standard_vtol", ], diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 56c6d028a0..117040555a 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; } + // Laser Scan + 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_ERR("failed to subscribe to %s", laser_scan_topic.c_str()); + return PX4_ERROR; + } + #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,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 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(scan.range_min() * 100.); + obs.max_distance = static_cast(scan.range_max() * 100.); + obs.angle_offset = static_cast(angle_min_deg); + obs.increment = static_cast(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::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 diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 6d2e2670a1..8a832f7961 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -58,6 +58,7 @@ #include #include #include +#include #include #include @@ -67,6 +68,7 @@ #include #include #include +#include using namespace time_literals; @@ -106,6 +108,7 @@ private: 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 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; //uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::Publication _obstacle_distance_pub{ORB_ID(obstacle_distance)}; uORB::Publication _angular_velocity_ground_truth_pub{ORB_ID(vehicle_angular_velocity_groundtruth)}; uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)};