diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 index bca3feddfb8b..3923b44cb4ab 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4001_gz_x500 @@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=x500} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default CA_AIRFRAME 0 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna index 04fc1acfd897..ea7b3a2f03fb 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4003_gz_rc_cessna @@ -12,8 +12,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=rc_cessna} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol index 96bb25d69a52..69b0cec559c1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4004_gz_standard_vtol @@ -13,8 +13,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=standard_vtol} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision index 967051b0420e..e988f1f9b0d1 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4006_gz_px4vision @@ -14,8 +14,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=px4vision} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 # Commander Parameters diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane index 5d5c50df3399..8479f2e38c34 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4008_gz_advanced_plane @@ -11,7 +11,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=advanced_plane} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 param set-default SENS_EN_MAGSIM 1 param set-default SENS_EN_ARSPDSIM 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover index f05bbdcf4572..5ff39b3f9693 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4009_gz_r1_rover @@ -46,10 +46,7 @@ param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 # Simulated sensors -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 101 # right wheel diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower index 0fec76b5c4f6..a56cf9300540 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4011_gz_lawnmower @@ -12,10 +12,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=lawnmower} param set-default SIM_GZ_EN 1 # Gazebo bridge # Simulated sensors -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 # We can arm and drive in manual mode when it slides and GPS check fails: param set-default COM_ARM_WO_GPS 1 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann index cd870caabbf8..5867b4d3e394 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4012_gz_rover_ackermann @@ -45,10 +45,7 @@ param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 # Simulated sensors -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 # Wheels param set-default SIM_GZ_WH_FUNC1 101 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum index 724e7613dfa8..45f80383e831 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4015_gz_r1_rover_mecanum @@ -45,10 +45,7 @@ param set-default PP_LOOKAHD_MAX 10 param set-default PP_LOOKAHD_MIN 1 # Simulated sensors -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 # Actuator mapping param set-default SIM_GZ_WH_FUNC1 102 # right wheel front diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter index 9856b1bf273e..1af6e721bc71 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4018_gz_quadtailsitter @@ -13,11 +13,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=quadtailsitter} param set-default SIM_GZ_EN 1 # Gazebo bridge -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 - param set-default MAV_TYPE 20 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor index c798650b5473..8325f67a2c98 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4020_gz_tiltrotor @@ -13,10 +13,7 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=tiltrotor} param set-default SIM_GZ_EN 1 # Gazebo bridge -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 -param set-default SENS_EN_ARSPDSIM 0 param set-default MAV_TYPE 21 diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d index 480454d72bf6..4cc0a02e413f 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/71002_gz_spacecraft_2d @@ -15,8 +15,6 @@ PX4_SIM_MODEL=${PX4_SIM_MODEL:=spacecraft_2d} param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 1 param set-default SENS_EN_MAGSIM 1 param set-default COM_ARM_CHK_ESCS 0 # We don't have ESCs param set-default FD_ESCS_EN 0 # We don't have ESCs - but maybe we need this later? diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter index b43b61025c2e..0f6748edafc6 100644 --- a/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter +++ b/ROMFS/px4fmu_common/init.d-posix/airframes/8011_gz_omnicopter @@ -83,8 +83,6 @@ param set-default CA_ROTOR7_AY -0.211325 param set-default CA_ROTOR7_AZ -0.57735 param set-default SIM_GZ_EN 1 -param set-default SENS_EN_GPSSIM 1 -param set-default SENS_EN_BAROSIM 0 param set-default SENS_EN_MAGSIM 1 param set-default SIM_GZ_EC_FUNC1 101 diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 183cbee9e225..23170a91255d 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 183cbee9e2250d1ca5517cd76c5d9a7e47cc0b7a +Subproject commit 23170a91255d99aea8960d1101541afce0f209d9 diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp index f5d10989fa1a..63ed7f4d682f 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.cpp +++ b/src/modules/simulation/gz_bridge/GZBridge.cpp @@ -1,20 +1,20 @@ /**************************************************************************** * - * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions * are met: * * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. + * notice, this list of conditions and the following disclaimer. * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. + * used to endorse or promote products derived from this software + * without specific prior written permission. * * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT @@ -52,8 +52,6 @@ GZBridge::GZBridge(const char *world, const char *name, const char *model, _model_sim(model), _model_pose(pose_str) { - pthread_mutex_init(&_node_mutex, nullptr); - updateParams(); } @@ -429,15 +427,11 @@ bool GZBridge::updateClock(const uint64_t tv_sec, const uint64_t tv_nsec) void GZBridge::clockCallback(const gz::msgs::Clock &clock) { - pthread_mutex_lock(&_node_mutex); - const uint64_t time_us = (clock.sim().sec() * 1000000) + (clock.sim().nsec() / 1000); if (time_us > _world_time_us.load()) { updateClock(clock.sim().sec(), clock.sim().nsec()); } - - pthread_mutex_unlock(&_node_mutex); } void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure) @@ -446,8 +440,6 @@ void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure) return; } - pthread_mutex_lock(&_node_mutex); - const uint64_t time_us = (air_pressure.header().stamp().sec() * 1000000) + (air_pressure.header().stamp().nsec() / 1000); @@ -459,19 +451,11 @@ void GZBridge::barometerCallback(const gz::msgs::FluidPressure &air_pressure) sensor_baro.temperature = this->_temperature; sensor_baro.timestamp = hrt_absolute_time(); _sensor_baro_pub.publish(sensor_baro); - - pthread_mutex_unlock(&_node_mutex); } void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &air_speed) { - if (hrt_absolute_time() == 0) { - return; - } - - pthread_mutex_lock(&_node_mutex); - const uint64_t time_us = (air_speed.header().stamp().sec() * 1000000) + (air_speed.header().stamp().nsec() / 1000); @@ -486,18 +470,10 @@ void GZBridge::airspeedCallback(const gz::msgs::AirSpeed &air_speed) _differential_pressure_pub.publish(report); this->_temperature = report.temperature; - - pthread_mutex_unlock(&_node_mutex); } void GZBridge::imuCallback(const gz::msgs::IMU &imu) { - if (hrt_absolute_time() == 0) { - return; - } - - pthread_mutex_lock(&_node_mutex); - const uint64_t time_us = (imu.header().stamp().sec() * 1000000) + (imu.header().stamp().nsec() / 1000); if (time_us > _world_time_us.load()) { @@ -551,18 +527,10 @@ void GZBridge::imuCallback(const gz::msgs::IMU &imu) sensor_gyro.temperature = NAN; sensor_gyro.samples = 1; _sensor_gyro_pub.publish(sensor_gyro); - - pthread_mutex_unlock(&_node_mutex); } void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) { - if (hrt_absolute_time() == 0) { - return; - } - - pthread_mutex_lock(&_node_mutex); - for (int p = 0; p < pose.pose_size(); p++) { if (pose.pose(p).name() == _model_name) { @@ -663,23 +631,13 @@ void GZBridge::poseInfoCallback(const gz::msgs::Pose_V &pose) local_position_groundtruth.timestamp = hrt_absolute_time(); _lpos_ground_truth_pub.publish(local_position_groundtruth); - - pthread_mutex_unlock(&_node_mutex); return; } } - - pthread_mutex_unlock(&_node_mutex); } void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry) { - if (hrt_absolute_time() == 0) { - return; - } - - pthread_mutex_lock(&_node_mutex); - const uint64_t time_us = (odometry.header().stamp().sec() * 1000000) + (odometry.header().stamp().nsec() / 1000); if (time_us > _world_time_us.load()) { @@ -744,18 +702,74 @@ void GZBridge::odometryCallback(const gz::msgs::OdometryWithCovariance &odometry // odom.reset_counter = vpe.reset_counter; _visual_odometry_pub.publish(odom); - - pthread_mutex_unlock(&_node_mutex); } -void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat) +static float generate_wgn() { - if (hrt_absolute_time() == 0) { - return; + // generate white Gaussian noise sample with std=1 + + // algorithm 1: + // float temp=((float)(rand()+1))/(((float)RAND_MAX+1.0f)); + // return sqrtf(-2.0f*logf(temp))*cosf(2.0f*M_PI_F*rand()/RAND_MAX); + // algorithm 2: from BlockRandGauss.hpp + static float V1, V2, S; + static bool phase = true; + float X; + + if (phase) { + do { + float U1 = (float)rand() / (float)RAND_MAX; + float U2 = (float)rand() / (float)RAND_MAX; + V1 = 2.0f * U1 - 1.0f; + V2 = 2.0f * U2 - 1.0f; + S = V1 * V1 + V2 * V2; + } while (S >= 1.0f || fabsf(S) < 1e-8f); + + X = V1 * float(sqrtf(-2.0f * float(logf(S)) / S)); + + } else { + X = V2 * float(sqrtf(-2.0f * float(logf(S)) / S)); } - pthread_mutex_lock(&_node_mutex); + phase = !phase; + return X; +} + +void GZBridge::addRealisticGpsNoise(double &latitude, double &longitude, double &altitude, + float &vel_north, float &vel_east, float &vel_down) +{ + _gps_pos_noise_n = _pos_markov_time * _gps_pos_noise_n + + _pos_random_walk * generate_wgn() * _pos_noise_amplitude - + 0.02f * _gps_pos_noise_n; + + _gps_pos_noise_e = _pos_markov_time * _gps_pos_noise_e + + _pos_random_walk * generate_wgn() * _pos_noise_amplitude - + 0.02f * _gps_pos_noise_e; + _gps_pos_noise_d = _pos_markov_time * _gps_pos_noise_d + + _pos_random_walk * generate_wgn() * _pos_noise_amplitude * 1.5f - + 0.02f * _gps_pos_noise_d; + + latitude += math::degrees((double)_gps_pos_noise_n / CONSTANTS_RADIUS_OF_EARTH); + longitude += math::degrees((double)_gps_pos_noise_e / CONSTANTS_RADIUS_OF_EARTH); + altitude += (double)_gps_pos_noise_d; + + _gps_vel_noise_n = _vel_markov_time * _gps_vel_noise_n + + _vel_noise_density * generate_wgn() * _vel_noise_amplitude; + + _gps_vel_noise_e = _vel_markov_time * _gps_vel_noise_e + + _vel_noise_density * generate_wgn() * _vel_noise_amplitude; + + _gps_vel_noise_d = _vel_markov_time * _gps_vel_noise_d + + _vel_noise_density * generate_wgn() * _vel_noise_amplitude * 1.2f; + + vel_north += _gps_vel_noise_n; + vel_east += _gps_vel_noise_e; + vel_down += _gps_vel_noise_d; +} + +void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat) +{ const uint64_t time_us = (nav_sat.header().stamp().sec() * 1000000) + (nav_sat.header().stamp().nsec() / 1000); if (time_us > _world_time_us.load()) { @@ -766,29 +780,94 @@ void GZBridge::navSatCallback(const gz::msgs::NavSat &nav_sat) if (!_pos_ref.isInitialized()) { _pos_ref.initReference(nav_sat.latitude_deg(), nav_sat.longitude_deg(), hrt_absolute_time()); _alt_ref = nav_sat.altitude(); + return; + } - } else { - // publish GPS groundtruth - vehicle_global_position_s global_position_groundtruth{}; + double latitude = nav_sat.latitude_deg(); + double longitude = nav_sat.longitude_deg(); + double altitude = nav_sat.altitude(); + float vel_north = nav_sat.velocity_north(); + float vel_east = nav_sat.velocity_east(); + float vel_down = -nav_sat.velocity_up(); + + vehicle_global_position_s gps_truth{}; #if defined(ENABLE_LOCKSTEP_SCHEDULER) - global_position_groundtruth.timestamp_sample = time_us; + gps_truth.timestamp_sample = time_us; + uint64_t timestamp = time_us; #else - global_position_groundtruth.timestamp_sample = hrt_absolute_time(); + uint64_t timestamp = hrt_absolute_time(); #endif - global_position_groundtruth.lat = nav_sat.latitude_deg(); - global_position_groundtruth.lon = nav_sat.longitude_deg(); - global_position_groundtruth.alt = nav_sat.altitude(); - _gpos_ground_truth_pub.publish(global_position_groundtruth); + + // Publish GPS groundtruth + gps_truth.timestamp_sample = timestamp; + gps_truth.lat = latitude; + gps_truth.lon = longitude; + gps_truth.alt = altitude; + _gpos_ground_truth_pub.publish(gps_truth); + + // Apply noise model (based on ublox F9P) + addRealisticGpsNoise(latitude, longitude, altitude, vel_north, vel_east, vel_down); + + // Device ID + device::Device::DeviceId device_id; + device_id.devid_s.bus_type = device::Device::DeviceBusType::DeviceBusType_SIMULATION; + device_id.devid_s.bus = 0; + device_id.devid_s.address = 0; + device_id.devid_s.devtype = DRV_GPS_DEVTYPE_SIM; + + sensor_gps_s sensor_gps{}; + + if (_sim_gps_used.get() >= 4) { + // fix + sensor_gps.fix_type = 3; // 3D fix + sensor_gps.s_variance_m_s = 0.4f; + sensor_gps.c_variance_rad = 0.1f; + sensor_gps.eph = 0.9f; + sensor_gps.epv = 1.78f; + sensor_gps.hdop = 0.7f; + sensor_gps.vdop = 1.1f; + + } else { + // no fix + sensor_gps.fix_type = 0; // No fix + sensor_gps.s_variance_m_s = 100.f; + sensor_gps.c_variance_rad = 100.f; + sensor_gps.eph = 100.f; + sensor_gps.epv = 100.f; + sensor_gps.hdop = 100.f; + sensor_gps.vdop = 100.f; } - pthread_mutex_unlock(&_node_mutex); + sensor_gps.timestamp = hrt_absolute_time(); + sensor_gps.timestamp_sample = timestamp; + sensor_gps.time_utc_usec = 0; + sensor_gps.device_id = device_id.devid; + sensor_gps.latitude_deg = latitude; + sensor_gps.longitude_deg = longitude; + sensor_gps.altitude_msl_m = altitude; + sensor_gps.altitude_ellipsoid_m = altitude; + sensor_gps.noise_per_ms = 0; + sensor_gps.jamming_indicator = 0; + sensor_gps.vel_m_s = sqrtf(vel_north * vel_north + vel_east * vel_east); + sensor_gps.vel_n_m_s = vel_north; + sensor_gps.vel_e_m_s = vel_east; + sensor_gps.vel_d_m_s = vel_down; + sensor_gps.cog_rad = atan2(vel_east, vel_north); + sensor_gps.timestamp_time_relative = 0; + sensor_gps.heading = NAN; + sensor_gps.heading_offset = NAN; + sensor_gps.heading_accuracy = 0; + sensor_gps.automatic_gain_control = 0; + sensor_gps.jamming_state = 0; + sensor_gps.spoofing_state = 0; + sensor_gps.vel_ned_valid = true; + sensor_gps.satellites_used = _sim_gps_used.get(); + + _sensor_gps_pub.publish(sensor_gps); } + 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; @@ -1059,8 +1138,6 @@ void GZBridge::Run() return; } - pthread_mutex_lock(&_node_mutex); - if (_parameter_update_sub.updated()) { parameter_update_s pupdate; _parameter_update_sub.copy(&pupdate); @@ -1074,8 +1151,6 @@ void GZBridge::Run() } ScheduleDelayed(10_ms); - - pthread_mutex_unlock(&_node_mutex); } int GZBridge::print_status() diff --git a/src/modules/simulation/gz_bridge/GZBridge.hpp b/src/modules/simulation/gz_bridge/GZBridge.hpp index 32b445decded..122092bac377 100644 --- a/src/modules/simulation/gz_bridge/GZBridge.hpp +++ b/src/modules/simulation/gz_bridge/GZBridge.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2022-2023 PX4 Development Team. All rights reserved. + * Copyright (c) 2025 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -44,24 +44,26 @@ #include #include #include +#include #include + #include #include #include #include #include #include -#include #include #include +#include +#include +#include +#include #include #include #include #include -#include #include -#include -#include #include #include @@ -117,59 +119,18 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &scan); void laserScanCallback(const gz::msgs::LaserScan &scan); - /** - * @brief Call Entityfactory service - * - * @param req - * @return true - * @return false - */ bool callEntityFactoryService(const std::string &service, const gz::msgs::EntityFactory &req); - - - /** - * @brief Call scene info service - * - * @param service - * @param req - * @return true - * @return false - */ bool callSceneInfoMsgService(const std::string &service); - - /** - * @brief Call String service - * - * @param service - * @param req - * @return true - * @return false - */ bool callStringMsgService(const std::string &service, const gz::msgs::StringMsg &req); - - /** - * @brief Call Vector3d Service - * - * @param service - * @param req - * @return true - * @return false - */ bool callVector3dService(const std::string &service, const gz::msgs::Vector3d &req); - /** - * - * Convert a quaterion from FLU_to_ENU frames (ROS convention) - * to FRD_to_NED frames (PX4 convention) - * - * @param q_FRD_to_NED output quaterion in PX4 conventions - * @param q_FLU_to_ENU input quaterion in ROS conventions - */ + bool callPhysicsMsgService(const std::string &service, const gz::msgs::Physics &req); + static void rotateQuaternion(gz::math::Quaterniond &q_FRD_to_NED, const gz::math::Quaterniond q_FLU_to_ENU); - bool callPhysicsMsgService(const std::string &service, const gz::msgs::Physics &req); + void addRealisticGpsNoise(double &latitude, double &longitude, double &altitude, + float &vel_north, float &vel_east, float &vel_down); - // Subscriptions - uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; uORB::Publication _distance_sensor_pub{ORB_ID(distance_sensor)}; uORB::Publication _differential_pressure_pub{ORB_ID(differential_pressure)}; @@ -178,15 +139,17 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S uORB::Publication _attitude_ground_truth_pub{ORB_ID(vehicle_attitude_groundtruth)}; uORB::Publication _gpos_ground_truth_pub{ORB_ID(vehicle_global_position_groundtruth)}; uORB::Publication _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)}; - uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; - uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; - uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; - uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; + uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + uORB::PublicationMulti _sensor_accel_pub{ORB_ID(sensor_accel)}; + uORB::PublicationMulti _sensor_gyro_pub{ORB_ID(sensor_gyro)}; + uORB::PublicationMulti _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)}; GZMixingInterfaceESC _mixing_interface_esc{_node, _node_mutex}; GZMixingInterfaceServo _mixing_interface_servo{_node, _node_mutex}; GZMixingInterfaceWheel _mixing_interface_wheel{_node, _node_mutex}; + GZGimbal _gimbal{_node, _node_mutex}; px4::atomic _world_time_us{0}; @@ -209,4 +172,22 @@ class GZBridge : public ModuleBase, public ModuleParams, public px4::S float _temperature{288.15}; // 15 degrees gz::transport::Node _node; + + // GPS noise model + float _gps_pos_noise_n = 0.0f; + float _gps_pos_noise_e = 0.0f; + float _gps_pos_noise_d = 0.0f; + float _gps_vel_noise_n = 0.0f; + float _gps_vel_noise_e = 0.0f; + float _gps_vel_noise_d = 0.0f; + const float _pos_noise_amplitude = 0.8f; // Position noise amplitude [m] + const float _pos_random_walk = 0.01f; // Position random walk coefficient + const float _pos_markov_time = 0.95f; // Position Markov process coefficient + const float _vel_noise_amplitude = 0.05f; // Velocity noise amplitude [m/s] + const float _vel_noise_density = 0.2f; // Velocity noise process density + const float _vel_markov_time = 0.85f; // Velocity Markov process coefficient + + DEFINE_PARAMETERS( + (ParamInt) _sim_gps_used + ) };