gz: Refactor GZBridge and px4-rc.simulator (#24421)

* disable SENS_EN_GPSSIM for all gz airframes

* add GPS + noise to GZBridge

* remove mutex from gz callbacks. Callbacks run synchronously after sim update step and run() loop does not share resources.

* remove hrt check in callbacks

* format

* remove param set-default for already default params

* update submodule

* remove unnecessary comments

* overhaul of the GZBridge and px4-rc.simulator script

* remove arg

* shellcheck disable

* add bus/address

* start gz_bridge before adjusting sim speed or camera follow

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

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
 

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
 

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

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
 

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

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
 

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

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

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
 

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
 

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?

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

ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator

@@ -73,13 +73,13 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 			exit 1
 		fi
 
-		# look for running ${gz_command} gazebo world
+		# Look for an already running world
 		gz_world=$( ${gz_command} topic -l | grep -m 1 -e "^/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
 
 		# shellcheck disable=SC2153
 		if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
 
-			# source generated gz_env.sh for GZ_SIM_RESOURCE_PATH
+			# Setup gz environment variables
 			if [ -f ./gz_env.sh ]; then
 				. ./gz_env.sh
 
@@ -87,62 +87,124 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 				. ../gz_env.sh
 			fi
 
-			echo "INFO  [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
-
+			# Start gazebo with default world
+			echo "INFO  [init] Starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
 			${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
 
 			if [ -z "${HEADLESS}" ]; then
-				# HEADLESS not set, starting gui
-				${gz_command} ${gz_sub_command} -g &
+				echo "INFO  [init] Starting gz gui"
+				${gz_command} ${gz_sub_command} -g > /dev/null 2>&1 &
 			fi
-
 		else
-			# Gazebo is already running, do not start the simulator, nor the GUI
+			# Gazebo is already running
 			echo "INFO  [init] gazebo already running world: ${gz_world}"
 			PX4_GZ_WORLD=${gz_world}
 		fi
+
 	else
 		echo "INFO  [init] Standalone PX4 launch, waiting for Gazebo"
 	fi
 
-	# start gz_bridge
+	# Start gz_bridge - either spawn a model or connect to existing one
 	if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
-		# model specified, gz_bridge will spawn model
+		# Spawn a model
+		MODEL_NAME="${PX4_SIM_MODEL#*gz_}"
+		MODEL_NAME_INSTANCE="${MODEL_NAME}_${px4_instance}"
 
+		POSE_ARG=""
 		if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
-			# model pose provided: [x, y, z, roll, pitch, yaw]
+			pos_x=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $1}')
+			pos_y=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $2}')
+			pos_z=$(echo "${PX4_GZ_MODEL_POSE}" | awk -F',' '{print $3}')
+			pos_x=${pos_x:-0}
+			pos_y=${pos_y:-0}
+			pos_z=${pos_z:-0}
 
-			# Clean potential input line formatting.
-			model_pose="$( echo "${PX4_GZ_MODEL_POSE}" | sed -e 's/^[ \t]*//; s/[ \t]*$//; s/,/ /g; s/  / /g; s/ /,/g' )"
-			echo "INFO  [init] PX4_GZ_MODEL_POSE set, spawning at: ${model_pose}"
-
-		else
-			# model pose not provided, origin will be used
-
-			echo "WARN  [init] PX4_GZ_MODEL_POSE not set, spawning at origin."
-			model_pose="0,0,0,0,0,0"
+			POSE_ARG=", pose: { position: { x: ${pos_x}, y: ${pos_y}, z: ${pos_z} } }"
+			echo "INFO  [init] Spawning model at position: ${pos_x} ${pos_y} ${pos_z}"
 		fi
 
-		# start gz bridge with pose arg.
-		if ! gz_bridge start -p "${model_pose}" -m "${PX4_SIM_MODEL#*gz_}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
+		# Spawn model
+		${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \
+				--reptype gz.msgs.Boolean --timeout 1000 \
+				--req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1
+
+		# Start gz_bridge
+		if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${MODEL_NAME_INSTANCE}"; then
 			echo "ERROR [init] gz_bridge failed to start and spawn model"
 			exit 1
 		fi
 
-	elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then
-		# model name specificed, gz_bridge will attach to existing model
+		# Set physics parameters for faster-than-realtime simulation if needed
+		check_scene_info() {
+			SERVICE_INFO=$(${gz_command} service -i --service "/world/${PX4_GZ_WORLD}/scene/info" 2>&1)
+			if echo "$SERVICE_INFO" | grep -q "Service providers"; then
+				return 0
+			else
+				return 1
+			fi
+		}
 
+		ATTEMPTS=30
+		while [ $ATTEMPTS -gt 0 ]; do
+			if check_scene_info; then
+				echo "INFO  [init] Gazebo world is ready"
+				break
+			fi
+			ATTEMPTS=$((ATTEMPTS-1))
+			if [ $ATTEMPTS -eq 0 ]; then
+				echo "ERROR [init] Timed out waiting for Gazebo world"
+				exit 1
+			fi
+			echo "INFO  [init] Waiting for Gazebo world..."
+			sleep 1
+		done
+
+		if [ -n "${PX4_SIM_SPEED_FACTOR}" ]; then
+			echo "INFO  [init] Setting simulation speed factor: ${PX4_SIM_SPEED_FACTOR}"
+			${gz_command} service -s "/world/${PX4_GZ_WORLD}/set_physics" --reqtype gz.msgs.Physics \
+						--reptype gz.msgs.Boolean --timeout 1000 \
+						--req "real_time_factor: ${PX4_SIM_SPEED_FACTOR}" > /dev/null 2>&1
+		fi
+
+
+		# Set up camera to follow the model if requested
+		if [ -n "${PX4_GZ_FOLLOW}" ]; then
+
+			# Wait for model to spawn
+			sleep 1
+
+			echo "INFO  [init] Setting camera to follow ${MODEL_NAME_INSTANCE}"
+
+			# Set camera to follow the model
+			${gz_command} service -s "/gui/follow" --reqtype gz.msgs.StringMsg \
+					--reptype gz.msgs.Boolean --timeout 5000 \
+					--req "data: \"${MODEL_NAME_INSTANCE}\"" > /dev/null 2>&1
+
+			# Set default camera offset if not specified
+			follow_x=${PX4_GZ_FOLLOW_OFFSET_X:--2.0}
+			follow_y=${PX4_GZ_FOLLOW_OFFSET_Y:--2.0}
+			follow_z=${PX4_GZ_FOLLOW_OFFSET_Z:-2.0}
+
+			# Set camera offset
+			${gz_command} service -s "/gui/follow/offset" --reqtype gz.msgs.Vector3d \
+					--reptype gz.msgs.Boolean --timeout 5000 \
+					--req "x: ${follow_x}, y: ${follow_y}, z: ${follow_z}" > /dev/null 2>&1
+
+			echo "INFO  [init] Camera follow offset set to ${follow_x}, ${follow_y}, ${follow_z}"
+		fi
+
+	elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then
+		# Connect to existing model
 		echo "INFO  [init] PX4_GZ_MODEL_NAME set, PX4 will attach to existing model"
-		if ! gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then
+		if ! gz_bridge start -w "${PX4_GZ_WORLD}" -n "${PX4_GZ_MODEL_NAME}"; then
 			echo "ERROR [init] gz_bridge failed to start and attach to existing model"
 			exit 1
 		fi
-
 	else
-		echo "ERROR [init] failed to pass only PX4_GZ_MODEL_NAME or PX4_SIM_MODEL"
+		echo "ERROR [init] failed to pass either PX4_GZ_MODEL_NAME or PX4_SIM_MODEL"
 		exit 1
 	fi
-
 	# Start the sensor simulator modules
 	if param compare -s SENS_EN_BAROSIM 1
 	then

Tools/simulation/gz

@@ -1 +1 @@
-Subproject commit 183cbee9e2250d1ca5517cd76c5d9a7e47cc0b7a
+Subproject commit 23170a91255d99aea8960d1101541afce0f209d9

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 <px4_platform_common/module_params.h>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/drivers/device/Device.hpp>
 #include <lib/geo/geo.h>
+
 #include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionInterval.hpp>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/distance_sensor.h>
-#include <lib/drivers/device/Device.hpp>
 #include <uORB/topics/sensor_accel.h>
 #include <uORB/topics/sensor_gyro.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/obstacle_distance.h>
+#include <uORB/topics/wheel_encoders.h>
 #include <uORB/topics/vehicle_angular_velocity.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
-#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>
@@ -81,7 +83,7 @@ using namespace time_literals;
 class GZBridge : public ModuleBase<GZBridge>, public ModuleParams, public px4::ScheduledWorkItem
 {
 public:
-	GZBridge(const char *world, const char *name, const char *model, const char *pose_str);
+	GZBridge(const std::string &world, const std::string &model_name);
 	~GZBridge() override;
 
 	/** @see ModuleBase */
@@ -98,78 +100,26 @@ public:
 	/** @see ModuleBase::print_status() */
 	int print_status() override;
 
-	uint64_t world_time_us() const { return _world_time_us.load(); }
-
 private:
 
 	void Run() override;
 
-	bool updateClock(const uint64_t tv_sec, const uint64_t tv_nsec);
+	void clockCallback(const gz::msgs::Clock &msg);
+	void airspeedCallback(const gz::msgs::AirSpeed &msg);
+	void barometerCallback(const gz::msgs::FluidPressure &msg);
+	void imuCallback(const gz::msgs::IMU &msg);
+	void poseInfoCallback(const gz::msgs::Pose_V &msg);
+	void odometryCallback(const gz::msgs::OdometryWithCovariance &msg);
+	void navSatCallback(const gz::msgs::NavSat &msg);
+	void laserScantoLidarSensorCallback(const gz::msgs::LaserScan &msg);
+	void laserScanCallback(const gz::msgs::LaserScan &msg);
 
-	void clockCallback(const gz::msgs::Clock &clock);
-
-	void airspeedCallback(const gz::msgs::AirSpeed &air_speed);
-	void barometerCallback(const gz::msgs::FluidPressure &air_pressure);
-	void imuCallback(const gz::msgs::IMU &imu);
-	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);
-
-	/**
-	 * @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
-	*/
 	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_s>          _distance_sensor_pub{ORB_ID(distance_sensor)};
 	uORB::Publication<differential_pressure_s>    _differential_pressure_pub{ORB_ID(differential_pressure)};
@@ -178,23 +128,21 @@ private:
 	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)};
 	uORB::Publication<vehicle_local_position_s>   _lpos_ground_truth_pub{ORB_ID(vehicle_local_position_groundtruth)};
-	uORB::PublicationMulti<sensor_baro_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
 
-	uORB::PublicationMulti<sensor_accel_s> _sensor_accel_pub{ORB_ID(sensor_accel)};
-	uORB::PublicationMulti<sensor_gyro_s>  _sensor_gyro_pub{ORB_ID(sensor_gyro)};
-	uORB::PublicationMulti<vehicle_odometry_s> _visual_odometry_pub{ORB_ID(vehicle_visual_odometry)};
+	uORB::PublicationMulti<sensor_gps_s>          _sensor_gps_pub{ORB_ID(sensor_gps)};
+	uORB::PublicationMulti<sensor_baro_s>         _sensor_baro_pub{ORB_ID(sensor_baro)};
+	uORB::PublicationMulti<sensor_accel_s>        _sensor_accel_pub{ORB_ID(sensor_accel)};
+	uORB::PublicationMulti<sensor_gyro_s>         _sensor_gyro_pub{ORB_ID(sensor_gyro)};
+	uORB::PublicationMulti<vehicle_odometry_s>    _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};
+	GZMixingInterfaceESC   _mixing_interface_esc{_node};
+	GZMixingInterfaceServo _mixing_interface_servo{_node};
+	GZMixingInterfaceWheel _mixing_interface_wheel{_node};
 
-	px4::atomic<uint64_t> _world_time_us{0};
-
-	pthread_mutex_t _node_mutex;
+	GZGimbal _gimbal{_node};
 
 	MapProjection _pos_ref{};
-	double _alt_ref{}; // starting altitude reference
+	double _alt_ref{};
 
 	matrix::Vector3d _position_prev{};
 	matrix::Vector3d _velocity_prev{};
@@ -203,10 +151,26 @@ private:
 
 	const std::string _world_name;
 	const std::string _model_name;
-	const std::string _model_sim;
-	const std::string _model_pose;
 
 	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<px4::params::SIM_GPS_USED>) _sim_gps_used
+	)
 };

src/modules/simulation/gz_bridge/GZGimbal.cpp

@@ -49,6 +49,8 @@ bool GZGimbal::init(const std::string &world_name, const std::string &model_name
 		return false;
 	}
 
+	pthread_mutex_init(&_node_mutex, nullptr);
+
 	updateParameters();
 
 	ScheduleOnInterval(200_ms); // @5Hz

src/modules/simulation/gz_bridge/GZGimbal.hpp

@@ -27,18 +27,17 @@ using namespace time_literals;
 class GZGimbal : public px4::ScheduledWorkItem, public ModuleParams
 {
 public:
-	GZGimbal(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
+	GZGimbal(gz::transport::Node &node) :
 		px4::ScheduledWorkItem(MODULE_NAME "-gimbal", px4::wq_configurations::rate_ctrl),
 		ModuleParams(nullptr),
-		_node(node),
-		_node_mutex(node_mutex)
+		_node(node)
 	{}
 
 private:
 	friend class GZBridge;
 
 	gz::transport::Node &_node;
-	pthread_mutex_t &_node_mutex;
+	pthread_mutex_t _node_mutex;
 
 	uORB::Subscription _gimbal_device_set_attitude_sub{ORB_ID(gimbal_device_set_attitude)};
 	uORB::Subscription _gimbal_controls_sub{ORB_ID(gimbal_controls)};

src/modules/simulation/gz_bridge/GZMixingInterfaceESC.cpp

@@ -55,6 +55,8 @@ bool GZMixingInterfaceESC::init(const std::string &model_name)
 
 	_esc_status_pub.advertise();
 
+	pthread_mutex_init(&_node_mutex, nullptr);
+
 	ScheduleNow();
 
 	return true;

src/modules/simulation/gz_bridge/GZMixingInterfaceESC.hpp

@@ -50,10 +50,9 @@ class GZMixingInterfaceESC : public OutputModuleInterface
 public:
 	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
 
-	GZMixingInterfaceESC(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
+	GZMixingInterfaceESC(gz::transport::Node &node) :
 		OutputModuleInterface(MODULE_NAME "-actuators-esc", px4::wq_configurations::rate_ctrl),
-		_node(node),
-		_node_mutex(node_mutex)
+		_node(node)
 	{}
 
 	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
@@ -77,7 +76,7 @@ private:
 	void motorSpeedCallback(const gz::msgs::Actuators &actuators);
 
 	gz::transport::Node &_node;
-	pthread_mutex_t &_node_mutex;
+	pthread_mutex_t _node_mutex;
 
 	MixingOutput _mixing_output{"SIM_GZ_EC", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 

src/modules/simulation/gz_bridge/GZMixingInterfaceServo.cpp

@@ -121,6 +121,8 @@ bool GZMixingInterfaceServo::init(const std::string &model_name)
 		_angular_range_rad.push_back(max_val - min_val);
 	}
 
+	pthread_mutex_init(&_node_mutex, nullptr);
+
 	ScheduleNow();
 
 	return true;

src/modules/simulation/gz_bridge/GZMixingInterfaceServo.hpp

@@ -44,10 +44,9 @@
 class GZMixingInterfaceServo : public OutputModuleInterface
 {
 public:
-	GZMixingInterfaceServo(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
+	GZMixingInterfaceServo(gz::transport::Node &node) :
 		OutputModuleInterface(MODULE_NAME "-actuators-servo", px4::wq_configurations::rate_ctrl),
-		_node(node),
-		_node_mutex(node_mutex)
+		_node(node)
 	{}
 
 	bool updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS],
@@ -83,7 +82,7 @@ private:
 	float get_servo_angle_min(const size_t index);
 
 	gz::transport::Node &_node;
-	pthread_mutex_t &_node_mutex;
+	pthread_mutex_t _node_mutex;
 
 	MixingOutput _mixing_output{"SIM_GZ_SV", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};
 

src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.cpp

@@ -51,6 +51,8 @@ bool GZMixingInterfaceWheel::init(const std::string &model_name)
 		return false;
 	}
 
+	pthread_mutex_init(&_node_mutex, nullptr);
+
 	_wheel_encoders_pub.advertise();
 
 	ScheduleNow();

src/modules/simulation/gz_bridge/GZMixingInterfaceWheel.hpp

@@ -51,10 +51,9 @@ class GZMixingInterfaceWheel : public OutputModuleInterface
 public:
 	static constexpr int MAX_ACTUATORS = MixingOutput::MAX_ACTUATORS;
 
-	GZMixingInterfaceWheel(gz::transport::Node &node, pthread_mutex_t &node_mutex) :
+	GZMixingInterfaceWheel(gz::transport::Node &node) :
 		OutputModuleInterface(MODULE_NAME "-actuators-wheel", px4::wq_configurations::rate_ctrl),
-		_node(node),
-		_node_mutex(node_mutex)
+		_node(node)
 	{}
 
 	bool updateOutputs(bool stop_wheels, uint16_t outputs[MAX_ACTUATORS],
@@ -78,7 +77,7 @@ private:
 	void wheelSpeedCallback(const gz::msgs::Actuators &actuators);
 
 	gz::transport::Node &_node;
-	pthread_mutex_t &_node_mutex;
+	pthread_mutex_t _node_mutex;
 
 	MixingOutput _mixing_output{"SIM_GZ_WH", MAX_ACTUATORS, *this, MixingOutput::SchedulingPolicy::Auto, false, false};