removal of PX4_GZ_MODEL env variable and fix of ground glitching (#22400

) Removal of PX4_GZ_MODEL env variable and fix of ground glitching Signed-off-by: frederik <[email protected]> Co-authored-by: frederik <[email protected]> Co-authored-by: Beniamino Pozzan <[email protected]>
PX4 · Nov 29, 2023 · f00d97d · f00d97d
1 parent 59abab8
commit f00d97d
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 31 deletions.
diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator
@@ -92,7 +92,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 	fi
 
 	# start gz_bridge
-	if [ -n "${PX4_GZ_MODEL}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
+	if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
 		# model specified, gz_bridge will spawn model
 
 		if [ -n "${PX4_GZ_MODEL_POSE}" ]; then
@@ -106,7 +106,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 		fi
 
 		# start gz bridge with pose arg.
-		if gz_bridge start -p "${model_pose}" -m "${PX4_GZ_MODEL}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
+		if gz_bridge start -p "${model_pose}" -m "${PX4_SIM_MODEL#*gz_}" -w "${PX4_GZ_WORLD}" -i "${px4_instance}"; then
 			if param compare -s SENS_EN_BAROSIM 1
 			then
 				sensor_baro_sim start
@@ -129,7 +129,7 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 			exit 1
 		fi
 
-	elif [ -n "${PX4_GZ_MODEL_NAME}" ] && [ -z "${PX4_GZ_MODEL}" ]; then
+	elif [ -n "${PX4_GZ_MODEL_NAME}" ]; then
 		# model name specificed, gz_bridge will attach to existing model
 
 		if gz_bridge start -n "${PX4_GZ_MODEL_NAME}" -w "${PX4_GZ_WORLD}"; then
@@ -155,35 +155,8 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
 			exit 1
 		fi
 
-	elif [ -n "${PX4_SIM_MODEL}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ] && [ -z "${PX4_GZ_MODEL}" ]; then
-
-		echo "WARN  [init] PX4_GZ_MODEL_NAME or PX4_GZ_MODEL not set using PX4_SIM_MODEL."
-
-		if gz_bridge start -m "${PX4_SIM_MODEL#*gz_}" -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
-
 	else
-		echo "ERROR [init] failed to pass only PX4_GZ_MODEL_NAME or PX4_GZ_MODEL"
+		echo "ERROR [init] failed to pass only PX4_GZ_MODEL_NAME or PX4_SIM_MODEL"
 		exit 1
 	fi
 

diff --git a/src/modules/simulation/gz_bridge/GZBridge.cpp b/src/modules/simulation/gz_bridge/GZBridge.cpp
@@ -95,6 +95,12 @@ int GZBridge::init()
 				model_pose_v.push_back(0.0);
 			}
 
+			// If model position z is less equal than 0, move above floor to prevent floor glitching
+			if (model_pose_v[2] <= 0.0) {
+				PX4_INFO("Model position z is less or equal 0.0, moving upwards");
+				model_pose_v[2] = 1.0;
+			}
+
 			gz::msgs::Pose *p = req.mutable_pose();
 			gz::msgs::Vector3d *position = p->mutable_position();
 			position->set_x(model_pose_v[0]);