@@ -73,76 +73,139 @@ elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
7373 exit 1
7474 fi
7575
76- # look for running ${gz_command} gazebo world
76+ # Look for an already running world
7777 gz_world=$( ${gz_command} topic -l | grep -m 1 -e " ^/world/.*/clock" | sed ' s/\/world\///g; s/\/clock//g' )
7878
7979 # shellcheck disable=SC2153
8080 if [ -z " ${gz_world} " && [ -n " ${PX4_GZ_WORLD} " ; then
8181
82- # source generated gz_env.sh for GZ_SIM_RESOURCE_PATH
82+ # Setup gz environment variables
8383 if [ -f ./gz_env.sh ]; then
8484 . ./gz_env.sh
8585
8686 elif [ -f ../gz_env.sh ]; then
8787 . ../gz_env.sh
8888 fi
8989
90- echo " INFO [init] starting gazebo with world: ${PX4_GZ_WORLDS} /${PX4_GZ_WORLD} .sdf"
90+ echo " INFO [init] Starting gazebo with world: ${PX4_GZ_WORLDS} /${PX4_GZ_WORLD} .sdf"
9191
92- ${gz_command} ${gz_sub_command} --verbose=1 -r -s " ${PX4_GZ_WORLDS} /${PX4_GZ_WORLD} .sdf" &
92+ ${gz_command} ${gz_sub_command} --verbose=${GZ_VERBOSE := 1} " ${PX4_GZ_WORLDS} /${PX4_GZ_WORLD} .sdf" &
9393
9494 if [ -z " ${HEADLESS} " ; then
95- # HEADLESS not set, starting gui
96- ${gz_command} ${gz_sub_command} -g &
95+ echo " INFO [init] Starting gz gui"
96+ ${gz_command} ${gz_sub_command} -g > /dev/null 2>&1 &
9797 fi
98-
9998 else
100- # Gazebo is already running, do not start the simulator, nor the GUI
99+ # Gazebo is already running
101100 echo " INFO [init] gazebo already running world: ${gz_world} "
102101 PX4_GZ_WORLD=${gz_world}
103102 fi
103+
104104 else
105105 echo " INFO [init] Standalone PX4 launch, waiting for Gazebo"
106106 fi
107107
108- # start gz_bridge
108+ # Start gz_bridge - either spawn a model or connect to existing one
109109 if [ -n " ${PX4_SIM_MODEL#* gz_} " && [ -z " ${PX4_GZ_MODEL_NAME} " ; then
110- # model specified, gz_bridge will spawn model
110+ # Spawn a model
111+ MODEL_NAME=" ${PX4_SIM_MODEL#* gz_} "
112+ MODEL_NAME_INSTANCE=" ${MODEL_NAME} _${px4_instance} "
111113
114+ POSE_ARG=" "
112115 if [ -n " ${PX4_GZ_MODEL_POSE} " ; then
113- # model pose provided: [x, y, z, roll, pitch, yaw]
116+ pos_x=$( echo " ${PX4_GZ_MODEL_POSE} " | awk -F' ,' ' {print $1}' )
117+ pos_y=$( echo " ${PX4_GZ_MODEL_POSE} " | awk -F' ,' ' {print $2}' )
118+ pos_z=$( echo " ${PX4_GZ_MODEL_POSE} " | awk -F' ,' ' {print $3}' )
119+ pos_x=${pos_x:- 0}
120+ pos_y=${pos_y:- 0}
121+ pos_z=${pos_z:- 0}
122+
123+ POSE_ARG=" , pose: { position: { x: ${pos_x} , y: ${pos_y} , z: ${pos_z} } }"
124+ echo " INFO [init] Spawning model at position: ${pos_x} ${pos_y} ${pos_z} "
125+ fi
114126
115- # Clean potential input line formatting.
116- model_pose=" $( echo " ${PX4_GZ_MODEL_POSE} " | sed -e ' s/^[ \t]*//; s/[ \t]*$//; s/,/ /g; s/ / /g; s/ /,/g' ) "
117- echo " INFO [init] PX4_GZ_MODEL_POSE set, spawning at: ${model_pose} "
127+ echo " INFO [init] Spawning model"
128+ # Spawn model
129+ ${gz_command} service -s " /world/${PX4_GZ_WORLD} /create"
130+ --reptype gz.msgs.Boolean --timeout 5000 \
131+ --req " sdf_filename: \" ${PX4_GZ_MODELS} /${MODEL_NAME} \" , name: \" ${MODEL_NAME_INSTANCE} \" , allow_renaming: false${POSE_ARG} " > /dev/null 2>&1
118132
119- else
120- # model pose not provided, origin will be used
133+ # Start gz_bridge
134+ if ! gz_bridge start -w " ${PX4_GZ_WORLD} " " ${MODEL_NAME_INSTANCE} " ; then
135+ echo " ERROR [init] gz_bridge failed to start and spawn model"
136+ exit 1
137+ fi
138+
139+ # Set physics parameters for faster-than-realtime simulation if needed
140+ check_scene_info () {
141+ SERVICE_INFO=$( ${gz_command} service -i --service " /world/${PX4_GZ_WORLD} /scene/info" 2>&1 )
142+ if echo " $SERVICE_INFO " | grep -q " Service providers" ; then
143+ return 0
144+ else
145+ return 1
146+ fi
147+ }
121148
122- echo " WARN [init] PX4_GZ_MODEL_POSE not set, spawning at origin."
123- model_pose=" 0,0,0,0,0,0"
149+ ATTEMPTS=30
150+ while [ $ATTEMPTS -gt 0 ]; do
151+ if check_scene_info; then
152+ echo " INFO [init] Gazebo world is ready"
153+ break
154+ fi
155+ ATTEMPTS=$(( ATTEMPTS- 1 ))
156+ if [ $ATTEMPTS -eq 0 ]; then
157+ echo " ERROR [init] Timed out waiting for Gazebo world"
158+ exit 1
159+ fi
160+ echo " INFO [init] Waiting for Gazebo world..."
161+ sleep 1
162+ done
163+
164+ if [ -n " ${PX4_SIM_SPEED_FACTOR} " ; then
165+ echo " INFO [init] Setting simulation speed factor: ${PX4_SIM_SPEED_FACTOR} "
166+ ${gz_command} service -s " /world/${PX4_GZ_WORLD} /set_physics"
167+ --reptype gz.msgs.Boolean --timeout 5000 \
168+ --req " real_time_factor: ${PX4_SIM_SPEED_FACTOR} " > /dev/null 2>&1
124169 fi
125170
126- # start gz bridge with pose arg.
127- if ! gz_bridge start -p " ${model_pose} " " ${PX4_SIM_MODEL#* gz_} " " ${PX4_GZ_WORLD} " " ${px4_instance} " ; then
128- echo " ERROR [init] gz_bridge failed to start and spawn model"
129- exit 1
171+
172+ # Set up camera to follow the model if requested
173+ if [ -n " ${PX4_GZ_FOLLOW} " ; then
174+
175+ # Wait for model to spawn
176+ sleep 1
177+
178+ echo " INFO [init] Setting camera to follow ${MODEL_NAME_INSTANCE} "
179+
180+ # Set camera to follow the model
181+ ${gz_command} service -s " /gui/follow"
182+ --reptype gz.msgs.Boolean --timeout 5000 \
183+ --req " data: \" ${MODEL_NAME_INSTANCE} \" " > /dev/null 2>&1
184+
185+ # Set default camera offset if not specified
186+ follow_x=${PX4_GZ_FOLLOW_OFFSET_X:- -2.0}
187+ follow_y=${PX4_GZ_FOLLOW_OFFSET_Y:- -2.0}
188+ follow_z=${PX4_GZ_FOLLOW_OFFSET_Z:- 2.0}
189+
190+ # Set camera offset
191+ ${gz_command} service -s " /gui/follow/offset"
192+ --reptype gz.msgs.Boolean --timeout 5000 \
193+ --req " x: ${follow_x} , y: ${follow_y} , z: ${follow_z} " > /dev/null 2>&1
194+
195+ echo " INFO [init] Camera follow offset set to ${follow_x} , ${follow_y} , ${follow_z} "
130196 fi
131197
132198 elif [ -n " ${PX4_GZ_MODEL_NAME} " ; then
133- # model name specificed, gz_bridge will attach to existing model
134-
199+ # Connect to existing model
135200 echo " INFO [init] PX4_GZ_MODEL_NAME set, PX4 will attach to existing model"
136- if ! gz_bridge start -n " ${PX4_GZ_MODEL_NAME } " w " ${PX4_GZ_WORLD } " ; then
201+ if ! gz_bridge start -w " ${PX4_GZ_WORLD } " n " ${PX4_GZ_MODEL_NAME } " ; then
137202 echo " ERROR [init] gz_bridge failed to start and attach to existing model"
138203 exit 1
139204 fi
140-
141205 else
142- echo " ERROR [init] failed to pass only PX4_GZ_MODEL_NAME or PX4_SIM_MODEL"
206+ echo " ERROR [init] failed to pass either PX4_GZ_MODEL_NAME or PX4_SIM_MODEL"
143207 exit 1
144208 fi
145-
146209 # Start the sensor simulator modules
147210 if param compare -s SENS_EN_BAROSIM 1
148211 then
0 commit comments