From 782e7562eb4523446f9c5c7bb5dcafea37a4ff74 Mon Sep 17 00:00:00 2001
From: haitomatic <hai.to@unikie.com>
Date: Fri, 20 Oct 2023 11:50:16 +0000
Subject: [PATCH] sitl: add support for gz sim with ssrc models and px4 sitl
 build/launch

---
 .../init.d-posix/airframes/4401_gz_ssrc_fog_x |  70 +++++++++++
 .../airframes/4430_gz_ssrc_striver_mini       | 109 ++++++++++++++++++
 .../airframes/4440_gz_ssrc_skywalker_x8       |  68 +++++++++++
 .../airframes/50005_gz_ssrc_scout_mini_rover  |  72 ++++++++++++
 .../init.d-posix/airframes/CMakeLists.txt     |   4 +
 .../init.d-posix/px4-rc.simulator             | 101 +++++++++++-----
 packaging/Dockerfile.sitl_gzsim               |  76 ++++++++++++
 packaging/build_px4_sitl_gzsim.sh             |   7 ++
 packaging/entrypoint_sitl_gzsim.sh            |  36 ++++++
 9 files changed, 514 insertions(+), 29 deletions(-)
 create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4401_gz_ssrc_fog_x
 create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_striver_mini
 create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/4440_gz_ssrc_skywalker_x8
 create mode 100644 ROMFS/px4fmu_common/init.d-posix/airframes/50005_gz_ssrc_scout_mini_rover
 create mode 100644 packaging/Dockerfile.sitl_gzsim
 create mode 100755 packaging/build_px4_sitl_gzsim.sh
 create mode 100755 packaging/entrypoint_sitl_gzsim.sh

diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4401_gz_ssrc_fog_x b/ROMFS/px4fmu_common/init.d-posix/airframes/4401_gz_ssrc_fog_x
new file mode 100644
index 000000000000..5b014bdad352
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4401_gz_ssrc_fog_x
@@ -0,0 +1,70 @@
+#!/bin/sh
+#
+# @name Gazebo x500
+#
+# @type Quadrotor x
+#
+
+. ${R}etc/init.d/rc.mc_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=holybro-x500}
+
+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 CA_AIRFRAME 0
+param set-default CA_ROTOR_COUNT 4
+
+param set-default CA_ROTOR0_PX 0.175
+param set-default CA_ROTOR0_PY 0.175
+param set-default CA_ROTOR0_KM  0.05
+
+param set-default CA_ROTOR1_PX -0.175
+param set-default CA_ROTOR1_PY -0.175
+param set-default CA_ROTOR1_KM  0.05
+
+param set-default CA_ROTOR2_PX 0.175
+param set-default CA_ROTOR2_PY -0.175
+param set-default CA_ROTOR2_KM -0.05
+
+param set-default CA_ROTOR3_PX -0.175
+param set-default CA_ROTOR3_PY 0.175
+param set-default CA_ROTOR3_KM -0.05
+
+param set-default SIM_GZ_FUNC1 101
+param set-default SIM_GZ_FUNC2 102
+param set-default SIM_GZ_FUNC3 103
+param set-default SIM_GZ_FUNC4 104
+
+param set-default SIM_GZ_MIN1 150
+param set-default SIM_GZ_MIN2 150
+param set-default SIM_GZ_MIN3 150
+param set-default SIM_GZ_MIN4 150
+
+param set-default SIM_GZ_MAX1 1000
+param set-default SIM_GZ_MAX2 1000
+param set-default SIM_GZ_MAX3 1000
+param set-default SIM_GZ_MAX4 1000
+
+param set-default MPC_THR_HOVER 0.60
+
+# extra
+param set COM_RCL_EXCEPT 4
+param set NAV_DLL_ACT 0
+param set NAV_RCL_ACT 0
+param set MAV_0_BROADCAST 1
+param set IMU_GYRO_CUTOFF 60
+param set IMU_DGYRO_CUTOFF 30
+param set MC_ROLLRATE_P 0.14
+param set MC_PITCHRATE_P 0.14
+param set MC_ROLLRATE_I 0.3
+param set MC_PITCHRATE_I 0.3
+param set MC_ROLLRATE_D 0.004
+param set MC_PITCHRATE_D 0.004
+param set BAT_N_CELLS 4
+param set SDLOG_MODE 0
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_striver_mini b/ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_striver_mini
new file mode 100644
index 000000000000..7484028ff7d2
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4430_gz_ssrc_striver_mini
@@ -0,0 +1,109 @@
+#!/bin/sh
+#
+# @name Striver mini VTOL
+#
+# @type Standard VTOL
+# @class VTOL
+#
+
+. ${R}etc/init.d/rc.vtol_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=striver_mini}
+
+param set-default SIM_GZ_EN 1
+
+# Control allocator parameters
+param set-default CA_AIRFRAME 2
+param set-default CA_ROTOR_COUNT 5
+param set-default CA_ROTOR0_PX 0.37
+param set-default CA_ROTOR0_PY 0.42
+param set-default CA_ROTOR1_PX -0.41
+param set-default CA_ROTOR1_PY -0.42
+param set-default CA_ROTOR2_PX 0.37
+param set-default CA_ROTOR2_PY -0.42
+param set-default CA_ROTOR2_KM -0.05
+param set-default CA_ROTOR3_PX -0.41
+param set-default CA_ROTOR3_PY 0.42
+param set-default CA_ROTOR3_KM -0.05
+param set-default CA_ROTOR4_AX 1.0
+param set-default CA_ROTOR4_AZ 0.0
+param set-default CA_SV_CS_COUNT 5
+param set-default CA_SV_CS0_TYPE 1
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TYPE 2
+param set-default CA_SV_CS1_TRQ_R -0.5
+param set-default CA_SV_CS2_TYPE 3
+param set-default CA_SV_CS2_TRQ_P 1.0
+param set-default CA_SV_CS3_TYPE 3
+param set-default CA_SV_CS3_TRQ_P -1.0
+param set-default CA_SV_CS4_TYPE 4
+param set-default CA_SV_CS4_TRQ_Y 1.0
+
+# PWM functions
+param set-default PWM_MAIN_FUNC1 101
+param set-default PWM_MAIN_FUNC2 102
+param set-default PWM_MAIN_FUNC3 103
+param set-default PWM_MAIN_FUNC4 104
+param set-default PWM_AUX_FUNC1 201
+param set-default PWM_AUX_FUNC2 202
+param set-default PWM_AUX_FUNC3 203
+param set-default PWM_AUX_FUNC4 204
+param set-default PWM_AUX_FUNC5 205
+param set-default PWM_AUX_FUNC6 105
+
+# Start airspeed sensor driver
+ms4525do start -X -f 500
+
+# Airspeed parameters
+param set-default ASPD_DO_CHECKS 15
+param set-default FW_AIRSPD_MAX 22.0
+param set-default FW_AIRSPD_MIN 15.0
+param set-default FW_AIRSPD_STALL 12.0
+param set-default FW_AIRSPD_TRIM 18.0
+
+# Battery parameters
+param set-default BAT1_N_CELLS 4
+param set-default BAT1_V_CHARGED 4.2
+param set-default BAT1_V_EMPTY 3.6
+param set-default BAT1_V_DIV 18.1
+
+# Disable internal magnetometer
+param set CAL_MAG0_PRIO 0
+
+# Return mode return altitude
+param set-default RTL_RETURN_ALT 30.0
+
+# Fixed wing specific
+param set-default FW_MAN_R_MAX 60.0
+param set-default FW_RR_IMAX 0.4000
+param set-default FW_YR_IMAX 0.4000
+
+# VTOL specific
+# VTOL type
+param set-default VT_TYPE 2
+
+# Airspeed at which we can start blending both fw and mc controls.
+param set-default VT_ARSP_BLEND 10
+
+# Airspeed at which we can switch to fw mode
+param set-default VT_ARSP_TRANS 12
+
+# Back-transition duration
+param set-default VT_B_TRANS_DUR 4
+param set-default VT_B_TRANS_RAMP 3
+
+# Transition duration
+param set-default VT_F_TRANS_DUR 6
+param set-default VT_F_TRANS_THR 1
+
+# VTOL takeoff
+param set-default VTO_LOITER_ALT 20
+
+# QuadChute altitude (transition to quad mode as a failsafe)
+param set-default VT_FW_MIN_ALT 5
+
+# QuadChute angle limits
+param set-default VT_FW_QC_P 35
+param set-default VT_FW_QC_R 60
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/4440_gz_ssrc_skywalker_x8 b/ROMFS/px4fmu_common/init.d-posix/airframes/4440_gz_ssrc_skywalker_x8
new file mode 100644
index 000000000000..ecb60a69618d
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/4440_gz_ssrc_skywalker_x8
@@ -0,0 +1,68 @@
+#!/bin/sh
+#
+# @name SSRC Skywalker X8
+#
+# @type Flying Wing
+# @class Plane
+#
+
+. /etc/init.d/rc.fw_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=skywalker_x8}
+
+param set-default SIM_GZ_EN 1
+
+# Control allocator parameters
+param set-default CA_AIRFRAME 1
+param set-default CA_ROTOR_COUNT 1
+param set-default CA_SV_CS_COUNT 2
+param set-default CA_SV_CS0_TYPE 5
+param set-default CA_SV_CS0_TRQ_P 0.5
+param set-default CA_SV_CS0_TRQ_R -0.5
+param set-default CA_SV_CS1_TYPE 6
+param set-default CA_SV_CS1_TRQ_P 0.5
+param set-default CA_SV_CS1_TRQ_R 0.5
+
+# PWM
+param set-default PWM_MAIN_FUNC1 201
+param set-default PWM_MAIN_FUNC2 202
+param set-default PWM_MAIN_FUNC4 101
+param set-default PWM_AUX_FUNC1 201
+param set-default PWM_AUX_FUNC2 202
+param set-default PWM_AUX_FUNC4 101
+
+# Airspeed parameters
+sdp3x start -X -f 400
+param set-default ASPD_DO_CHECKS 15
+param set-default FW_AIRSPD_MAX 22.0
+param set-default FW_AIRSPD_MIN 14.0
+param set-default FW_AIRSPD_STALL 12.0
+param set-default FW_AIRSPD_TRIM 18.0
+
+# Battery parameters
+param set-default BAT1_N_CELLS 4
+param set-default BAT1_R_INTERNAL 0.0050
+param set-default BAT1_V_EMPTY 3.6000
+param set-default BAT1_V_LOAD_DROP 0.1000
+
+param set-default BAT2_R_INTERNAL 0.0050
+param set-default BAT2_V_EMPTY 3.6000
+param set-default BAT2_V_LOAD_DROP 0.1000
+
+# Disable internal magnetometer
+param set CAL_MAG0_PRIO 0
+
+# Launch detection
+param set-default FW_LAUN_DETCN_ON 1
+
+# Maximum manual roll angle
+param set-default FW_MAN_R_MAX 60.0
+
+# Rate control
+param set-default FW_RR_IMAX 0.4000
+param set-default FW_YR_IMAX 0.4000
+
+# Misc
+param set-default RTL_RETURN_ALT 30.0
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/50005_gz_ssrc_scout_mini_rover b/ROMFS/px4fmu_common/init.d-posix/airframes/50005_gz_ssrc_scout_mini_rover
new file mode 100644
index 000000000000..1a441448594c
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/50005_gz_ssrc_scout_mini_rover
@@ -0,0 +1,72 @@
+#!/bin/sh
+#
+# @name SSRC SCOUT MINI UGV
+#
+# @url https://global.agilex.ai/products/scout-mini
+#
+# @type Rover
+# @class Rover
+#
+
+. ${R}etc/init.d/rc.rover_defaults
+
+PX4_SIMULATOR=${PX4_SIMULATOR:=gz}
+PX4_GZ_WORLD=${PX4_GZ_WORLD:=default}
+PX4_SIM_MODEL=${PX4_SIM_MODEL:=scout_mini}
+
+param set-default SIM_GZ_EN 1
+
+param set-default CA_AIRFRAME 6
+
+param set-default SENS_EN_GPSSIM 1
+param set-default SENS_EN_BAROSIM 1
+param set-default SENS_EN_MAGSIM 1
+
+param set IMU_GYRO_CUTOFF 60
+param set IMU_DGYRO_CUTOFF 30
+
+param set-default SYS_HAS_BARO 0
+
+param set-default BAT1_N_CELLS 4
+
+param set-default MIS_TAKEOFF_ALT 0.01
+
+param set-default NAV_ACC_RAD 0.5     # reached when within 0.5m of waypoint
+
+# Enable Airspeed check circuit breaker because Rovers will have no airspeed sensor
+param set-default CBRK_AIRSPD_CHK 162128
+
+# EKF2
+param set-default EKF2_GBIAS_INIT 0.01
+param set-default EKF2_ANGERR_INIT 0.01
+param set-default EKF2_MAG_TYPE 1
+param set-default EKF2_REQ_SACC 1.0
+param set-default EKF2_REQ_VDRIFT 0.4
+param set-default EKF2_REQ_HDRIFT 0.2
+
+
+#################################
+# Rover Position Control Module #
+#################################
+
+param set-default GND_SP_CTRL_MODE 1
+param set-default GND_L1_DIST 5
+param set-default GND_L1_PERIOD 3
+param set-default GND_THR_CRUISE 1
+param set-default GND_THR_MAX 1
+
+# Because this is differential drive, it can make a turn with radius 0.
+# This corresponds to a turn angle of pi radians.
+# If a special case is made for differential-drive, this will need to change.
+param set-default GND_MAX_ANG 3.142
+param set-default GND_WHEEL_BASE 0.45
+
+# TODO: Set to -1.0, to allow reversing. This will require many changes in the codebase
+# to support negative throttle.
+param set-default GND_THR_MIN 0
+param set-default GND_SPEED_P 0.4
+param set-default GND_SPEED_I 1
+param set-default GND_SPEED_D 0.001
+param set-default GND_SPEED_IMAX 1
+param set-default GND_SPEED_MAX 1
+param set-default GND_SPEED_THR_SC 1
diff --git a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
index a588010d1f2e..e2d2d315101f 100644
--- a/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
+++ b/ROMFS/px4fmu_common/init.d-posix/airframes/CMakeLists.txt
@@ -79,6 +79,10 @@ px4_add_romfs_files(
 	4004_gz_standard_vtol
 	4005_gz_x500_vision
 	4006_gz_px4vision
+	4401_gz_ssrc_fog_x
+	4430_gz_ssrc_striver_mini
+	4440_gz_ssrc_skywalker_x8
+	50005_gz_ssrc_scout_mini_rover
 
 	6011_gazebo-classic_typhoon_h480
 	6011_gazebo-classic_typhoon_h480.post
diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
index 1f4693d4c1b3..e5e2ab5f1441 100644
--- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
+++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
@@ -38,44 +38,50 @@ if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0"
 
 elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; then
 
-	# source generated gz_env.sh for GZ_SIM_RESOURCE_PATH
-	if [ -f ./gz_env.sh ]; then
-		. ./gz_env.sh
+	# allow starting of gz sim optionally
+	if [ "$(param show -q SIM_GZ_RUN_GZSIM)" -eq "1" ];
+	then
+		# source generated gz_env.sh for GZ_SIM_RESOURCE_PATH
+		if [ -f ./gz_env.sh ]; then
+			. ./gz_env.sh
 
-	elif [ -f ../gz_env.sh ]; then
-		. ../gz_env.sh
-	fi
+		elif [ -f ../gz_env.sh ]; then
+			. ../gz_env.sh
+		fi
 
-	# "gz sim" only avaiilable in Garden and later
-	GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
-	if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
-	then
-		# "gz sim" from Garden on
-		gz_command="gz"
-		gz_sub_command="sim"
-	else
-		echo "ERROR [init] Gazebo gz please install gz-garden"
-		exit 1
-	fi
+		# "gz sim" only avaiilable in Garden and later
+		GZ_SIM_VERSIONS=$(gz sim --versions 2>&1)
+		if [ $? -eq 0 ] && [ "${GZ_SIM_VERSIONS}" != "" ]
+		then
+			# "gz sim" from Garden on
+			gz_command="gz"
+			gz_sub_command="sim"
+		else
+			echo "ERROR [init] Gazebo gz please install gz-garden"
+			exit 1
+		fi
 
-	# look for running ${gz_command} gazebo world
-	gz_world=$( ${gz_command} topic -l | grep -m 1 -e "/world/.*/clock" | sed 's/\/world\///g; s/\/clock//g' )
+		# look for running ${gz_command} gazebo 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_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
+		# shellcheck disable=SC2153
+		if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
 
-		echo "INFO  [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
+			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" &
+			${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 &
-		fi
+			if [ -z "${HEADLESS}" ]; then
+				# HEADLESS not set, starting gui
+				${gz_command} ${gz_sub_command} -g &
+			fi
 
+		else
+			echo "INFO  [init] gazebo already running world: ${gz_world}"
+			PX4_GZ_WORLD=${gz_world}
+		fi
 	else
-		echo "INFO  [init] gazebo already running world: ${gz_world}"
-		PX4_GZ_WORLD=${gz_world}
+		echo "INFO  [init] gazebo run disabled"
 	fi
 
 	# start gz_bridge
@@ -116,6 +122,43 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" -eq "1" ]; th
 			exit 1
 		fi
 
+	elif [ -n "${PX4_GZ_MODEL}" ] && [ -n "${PX4_GZ_MODEL_NAME}" ]; then
+		# model type and name specified, gz_bridge will spawn model with name
+
+		if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
+			# 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
+			echo "WARN  [init] PX4_GZ_MODEL_POSE not set, spawning at origin."
+			model_pose="0,0,0,0,0,0"
+		fi
+
+		# start gz bridge with pose arg.
+		if gz_bridge start -p "${model_pose}" -m "${PX4_GZ_MODEL}" -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
+			if param compare -s SENS_EN_BAROSIM 1
+			then
+				sensor_baro_sim start
+			fi
+			if param compare -s SENS_EN_GPSSIM 1
+			then
+				sensor_gps_sim start
+			fi
+			if param compare -s SENS_EN_MAGSIM 1
+			then
+				sensor_mag_sim start
+			fi
+			if param compare -s SENS_EN_ARSPDSIM 1
+			then
+				sensor_airspeed_sim start
+			fi
+
+		else
+			echo "ERROR [init] gz_bridge failed to start"
+			exit 1
+		fi
+
 	elif [ -n "${PX4_GZ_MODEL_NAME}" ] && [ -z "${PX4_GZ_MODEL}" ]; then
 		# model name specificed, gz_bridge will attach to existing model
 
diff --git a/packaging/Dockerfile.sitl_gzsim b/packaging/Dockerfile.sitl_gzsim
new file mode 100644
index 000000000000..4f4d2de396fc
--- /dev/null
+++ b/packaging/Dockerfile.sitl_gzsim
@@ -0,0 +1,76 @@
+FROM docker.io/ros:humble-ros-base as builder
+
+# Install git for pulling the base repository
+RUN apt update
+RUN apt install -y \
+	git \
+	curl \
+	lsb-release \
+	gnupg
+
+RUN curl http://packages.osrfoundation.org/gazebo.key | apt-key add -
+RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
+
+# Install build dependencies
+RUN apt update
+RUN DEBIAN_FRONTEND=noninteractive TZ=Etc/UTC apt install -y \
+	libgz-transport12 \
+	libgz-transport12-dev \
+	astyle \
+	build-essential \
+	cmake \
+	cppcheck \
+	file \
+	g++ \
+	gcc \
+	gdb \
+	git \
+	lcov \
+	libfuse2 \
+	libxml2-dev \
+	libxml2-utils \
+	make \
+	ninja-build \
+	python3 \
+	python3-dev \
+	python3-pip \
+	python3-setuptools \
+	python3-wheel \
+	rsync \
+	shellcheck \
+	unzip \
+	ros-humble-fastrtps \
+	ros-humble-rmw-fastrtps-cpp \
+	fastddsgen
+
+# Checkout the px4 version and build it
+RUN git clone -b rebase_sitl_fixes https://github.com/tiiuae/px4-firmware
+RUN python3 -m pip install -r px4-firmware/Tools/setup/requirements.txt
+COPY build_px4_sitl_gzsim.sh ./build.sh
+RUN ./build.sh
+
+#  ▲               runtime ──┐
+#  └── build                 ▼
+
+FROM docker.io/ros:humble-ros-base
+
+RUN apt update
+RUN apt install -y \
+	curl \
+	lsb-release \
+	gnupg \
+	ros-humble-fastrtps \
+	ros-humble-rmw-fastrtps-cpp
+
+RUN curl http://packages.osrfoundation.org/gazebo.key | apt-key add -
+RUN sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list'
+
+RUN apt update
+RUN DEBIAN_FRONTEND=noninteractive TZ=Etc/UTC apt install -y libgz-transport12 dnsutils
+
+WORKDIR /px4_sitl
+
+COPY --from=builder /px4-firmware/build/px4_sitl_default /px4_sitl
+COPY ./entrypoint_sitl_gzsim.sh /entrypoint.sh
+
+CMD ["/entrypoint.sh"]
diff --git a/packaging/build_px4_sitl_gzsim.sh b/packaging/build_px4_sitl_gzsim.sh
new file mode 100755
index 000000000000..5ee8050ace18
--- /dev/null
+++ b/packaging/build_px4_sitl_gzsim.sh
@@ -0,0 +1,7 @@
+#!/bin/bash
+
+source /opt/ros/humble/setup.sh
+
+pushd px4-firmware
+	make px4_sitl_default
+popd
diff --git a/packaging/entrypoint_sitl_gzsim.sh b/packaging/entrypoint_sitl_gzsim.sh
new file mode 100755
index 000000000000..a13919f04d48
--- /dev/null
+++ b/packaging/entrypoint_sitl_gzsim.sh
@@ -0,0 +1,36 @@
+#!/bin/bash
+#
+
+export PATH=/px4_sitl/bin:$PATH
+
+case $PX4_VEHICLE_TYPE in
+  mc)
+    export PX4_SYS_AUTOSTART=4401
+    ;;
+  rover)
+    export PX4_SYS_AUTOSTART=50006
+    ;;
+  vtol)
+    export PX4_SYS_AUTOSTART=4430
+    ;;
+  fw)
+    export PX4_SYS_AUTOSTART=4440
+    ;;
+  uuv)
+    export PX4_SYS_AUTOSTART=4403
+    ;;
+  *)
+    echo "ERROR: unknown vehicle type: $PX4_VEHICLE_TYPE"
+    exit 1
+    ;;
+esac
+
+export PX4_GZ_MODEL_NAME=$DRONE_DEVICE_ID
+export PX4_GZ_WORLD=${PX4_GZ_WORLD:-default}
+export GZ_PARTITION=sim
+export GZ_RELAY=$(dig +short gazebo-server)
+export GZ_IP=$(hostname -i)
+
+source /opt/ros/humble/setup.sh
+
+/px4/bin/px4 -d -s /px4/etc/init.d-posix/rcS -w /px4