From d14c054149b5bf300c3b4002d2ff2fe51eb4f6e8 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Wed, 13 Nov 2024 08:44:19 +0100 Subject: [PATCH] SIM: MPC: add two x500 lidar airframes (#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 <37091262+dakejahl@users.noreply.github.com> * Update src/modules/simulation/gz_bridge/GZBridge.hpp Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> * update submodule --------- Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> --- ...13_gz_x500_lidar => 4013_gz_x500_lidar_2d} | 4 +- .../airframes/4016_gz_x500_lidar_down | 10 ++++ .../airframes/4017_gz_x500_lidar_front | 10 ++++ .../init.d-posix/airframes/CMakeLists.txt | 4 +- Tools/simulation/gz | 2 +- src/modules/simulation/gz_bridge/GZBridge.cpp | 51 +++++++++++++++++++ src/modules/simulation/gz_bridge/GZBridge.hpp | 4 ++ 7 files changed, 81 insertions(+), 4 deletions(-) rename ROMFS/px4fmu_common/init.d-posix/airframes/{4013_gz_x500_lidar => 4013_gz_x500_lidar_2d} (52%) create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4016_gz_x500_lidar_down create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4017_gz_x500_lidar_front 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_2d similarity index 52% rename from ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar rename to ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar_2d index a316310abe42..da908b6d55fb 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4013_gz_x500_lidar +++ b/ROMFS/px4fmu_common/init.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 new file mode 100644 index 000000000000..99e986231e1a --- /dev/null +++ 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 new file mode 100644 index 000000000000..6d8773e09176 --- /dev/null +++ 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 index 1ba611c2755a..96e1737e5d53 100644 --- 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 index 536305adee09..9e47793f2bc1 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 536305adee09b9ace391b16107e625cf7c6db7e7 +Subproject commit 9e47793f2bc18aa7cde39b1fc1c4b7bbc67e04ba diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 29d19fe11177..77858bb96452 100644 --- 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(scan.range_min()); + distance_sensor.max_distance = static_cast(scan.range_max()); + distance_sensor.current_distance = static_cast(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 index fc2cb3a6e9ad..f31ecf062f68 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -49,6 +49,8 @@ #include #include #include +#include +#include #include #include #include @@ -110,6 +112,7 @@ class GZBridge : public ModuleBase, 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, public ModuleParams, public px4::S uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; //uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; + uORB::Publication _distance_sensor_pub{ORB_ID(distance_sensor)}; 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)};