From ef72892127e5a97f3350ca6296c21c2ffec3e400 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Fri, 1 Nov 2024 12:03:45 +0100 Subject: [PATCH 1/4] add two x500 lidar airframe, including the gazebo bridge for the lidar sensors and their orientation --- ...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 | 46 +++++++++++++++++++ src/modules/simulation/gz_bridge/GZBridge.hpp | 3 ++ 7 files changed, 75 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..a2d3f9e381af 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 536305adee09b9ace391b16107e625cf7c6db7e7 +Subproject commit a2d3f9e381af952e1ac5c7886eb8b4e2cc4851bb diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index 29d19fe11177..c9a5aebc6c8f 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,44 @@ 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(); + distance_sensor.device_id = 0; + 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..7d1c97eea64c 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -110,6 +111,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 +167,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)}; From c0995675e4ac2dfd48a39b22bda683db758c18d3 Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Tue, 5 Nov 2024 08:21:05 +0100 Subject: [PATCH 2/4] Update src/modules/simulation/gz_bridge/GZBridge.cpp Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> --- src/modules/simulation/gz_bridge/GZBridge.cpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index c9a5aebc6c8f..77858bb96452 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -778,7 +778,12 @@ void GZBridge::laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan) distance_sensor_s distance_sensor{}; distance_sensor.timestamp = hrt_absolute_time(); - distance_sensor.device_id = 0; + 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]); From 891817430f29bff73790ad6f258fcfc6a0b8dccc Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Tue, 5 Nov 2024 08:21:13 +0100 Subject: [PATCH 3/4] Update src/modules/simulation/gz_bridge/GZBridge.hpp Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> --- src/modules/simulation/gz_bridge/GZBridge.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 7d1c97eea64c..f31ecf062f68 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include #include From 0e529bdd261f5450d0a4c18807b2ea9faf78dd6a Mon Sep 17 00:00:00 2001 From: Claudio Chies <61051109+Claudio-Chies@users.noreply.github.com> Date: Tue, 5 Nov 2024 08:51:03 +0100 Subject: [PATCH 4/4] update submodule --- Tools/simulation/gz | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/simulation/gz b/Tools/simulation/gz index a2d3f9e381af..9e47793f2bc1 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit a2d3f9e381af952e1ac5c7886eb8b4e2cc4851bb +Subproject commit 9e47793f2bc18aa7cde39b1fc1c4b7bbc67e04ba