Standalone px4 stable

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

@@ -59,36 +59,40 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; 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
+	# Only start up Gazebo if STANDALONE set to false
+	if [ "$STANDALONE" != '1' ]; then
 
-	# 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' )
+		# "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
 
-	# shellcheck disable=SC2153
-	if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
+		# 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' )
 
-		echo "INFO  [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
+		# shellcheck disable=SC2153
+		if [ -z "${gz_world}" ] && [ -n "${PX4_GZ_WORLDS}" ] && [ -n "${PX4_GZ_WORLD}" ]; then
 
-		${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
+			echo "INFO  [init] starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
 
-		if [ -z "${HEADLESS}" ]; then
-			# HEADLESS not set, starting gui
-			${gz_command} ${gz_sub_command} -g &
-		fi
+			${gz_command} ${gz_sub_command} --verbose=1 -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
 
-	else
-		echo "INFO  [init] gazebo already running world: ${gz_world}"
-		PX4_GZ_WORLD=${gz_world}
+			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
 	fi
 
 	# start gz_bridge

src/modules/simulation/gz_bridge/GZBridge.cpp

@@ -98,7 +98,7 @@ int GZBridge::init()
 			// 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;
+				model_pose_v[2] = 0.5;
 			}
 
 			gz::msgs::Pose *p = req.mutable_pose();
@@ -122,15 +122,44 @@ int GZBridge::init()
 		bool result;
 		std::string create_service = "/world/" + _world_name + "/create";
 
-		if (_node.Request(create_service, req, 1000, rep, result)) {
-			if (!rep.data() || !result) {
-				PX4_ERR("EntityFactory service call failed");
-				return PX4_ERROR;
+		bool gz_called = false;
+		// Check if STANDALONE has been set.
+		char *standalone_val = std::getenv("STANDALONE");
+
+		if ((standalone_val != nullptr) && (std::strcmp(standalone_val, "1") == 0)) {
+			// Check if Gazebo has been called and if not attempt to reconnect.
+			while (gz_called == false) {
+				if (_node.Request(create_service, req, 1000, rep, result)) {
+					if (!rep.data() || !result) {
+						PX4_ERR("EntityFactory service call failed");
+						return PX4_ERROR;
+
+					} else {
+						gz_called = true;
+					}
+				}
+
+				// If Gazebo has not been called, wait 2 seconds and try again.
+				else {
+					printf("WARN  [gz_bridge] Service call timed out as Gazebo has not been detected. \n");
+					system_usleep(2000000);
+				}
 			}
+		}
+
+
+		// If STANDALONE has not been set, do not retry to reconnect.
+		else {
+			if (_node.Request(create_service, req, 1000, rep, result)) {
+				if (!rep.data() || !result) {
+					PX4_ERR("EntityFactory service call failed");
+					return PX4_ERROR;
+				}
 
-		} else {
-			PX4_ERR("Service call timed out");
-			return PX4_ERROR;
+			} else {
+				PX4_ERR("Service call timed out");
+				return PX4_ERROR;
+			}
 		}
 	}