mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 11:06:04 +08:00
sim: refactor startup scripts and fix gz sim standalone (#24600)
* sim: refactor px4-rc.simulator into sim specific startup scripts. Fix gz sim for standalone mode. * shellcheck disable=SC2154
This commit is contained in:
@@ -35,8 +35,11 @@ add_subdirectory(airframes)
|
|||||||
|
|
||||||
px4_add_romfs_files(
|
px4_add_romfs_files(
|
||||||
px4-rc.mavlink
|
px4-rc.mavlink
|
||||||
px4-rc.params
|
|
||||||
px4-rc.simulator
|
px4-rc.simulator
|
||||||
|
px4-rc.gzsim
|
||||||
|
px4-rc.jmavsim
|
||||||
|
px4-rc.mavlinksim
|
||||||
|
px4-rc.sihsim
|
||||||
rc.replay
|
rc.replay
|
||||||
rcS
|
rcS
|
||||||
)
|
)
|
||||||
|
|||||||
@@ -0,0 +1,180 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
# shellcheck disable=SC2154
|
||||||
|
|
||||||
|
echo "INFO [init] Gazebo simulator"
|
||||||
|
|
||||||
|
# Enforce minimum gz version as Harmonic (gz-sim8)
|
||||||
|
MIN_GZ_VERSION="8.0.0"
|
||||||
|
GZ_SIM_VERSION=$(gz sim --versions 2>/dev/null | head -n 1 | tr -d ' ')
|
||||||
|
|
||||||
|
if [ -z "$GZ_SIM_VERSION" ]; then
|
||||||
|
echo "ERROR [init] Gazebo gz sim not found. Please install gz-harmonic"
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
|
||||||
|
# Use sort compare, check that MIN_GZ_VERSION is ordered last
|
||||||
|
if [ "$(printf '%s\n' "$GZ_SIM_VERSION" "$MIN_GZ_VERSION" | sort -V | head -n1)" = "$MIN_GZ_VERSION" ]; then
|
||||||
|
gz_command="gz"
|
||||||
|
gz_sub_command="sim"
|
||||||
|
|
||||||
|
# Specify render engine if `GZ_SIM_RENDER_ENGINE` is set
|
||||||
|
# (for example, if you want to use Ogre 1.x instead of Ogre 2.x):
|
||||||
|
if [ -n "${PX4_GZ_SIM_RENDER_ENGINE}" ]; then
|
||||||
|
echo "INFO [init] Setting Gazebo render engine to '${PX4_GZ_SIM_RENDER_ENGINE}'!"
|
||||||
|
gz_sub_command="${gz_sub_command} --render-engine ${PX4_GZ_SIM_RENDER_ENGINE}"
|
||||||
|
fi
|
||||||
|
else
|
||||||
|
echo "ERROR [init] Gazebo gz sim version is too old ($GZ_SIM_VERSION). Minimum required version is $MIN_GZ_VERSION"
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
|
||||||
|
# If not standalone
|
||||||
|
if [ -z "${PX4_GZ_STANDALONE}" ]; then
|
||||||
|
|
||||||
|
# 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
|
||||||
|
|
||||||
|
# Setup gz environment variables
|
||||||
|
if [ -f ./gz_env.sh ]; then
|
||||||
|
. ./gz_env.sh
|
||||||
|
|
||||||
|
elif [ -f ../gz_env.sh ]; then
|
||||||
|
. ../gz_env.sh
|
||||||
|
fi
|
||||||
|
|
||||||
|
echo "INFO [init] Starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
|
||||||
|
|
||||||
|
${gz_command} ${gz_sub_command} --verbose=${GZ_VERBOSE:=1} -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
|
||||||
|
|
||||||
|
if [ -z "${HEADLESS}" ]; then
|
||||||
|
echo "INFO [init] Starting gz gui"
|
||||||
|
${gz_command} ${gz_sub_command} -g > /dev/null 2>&1 &
|
||||||
|
fi
|
||||||
|
|
||||||
|
else
|
||||||
|
# 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
|
||||||
|
|
||||||
|
# Wait for Gazebo world to be ready before proceeding
|
||||||
|
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
|
||||||
|
|
||||||
|
# Start gz_bridge - either spawn a model or connect to existing one
|
||||||
|
if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
|
||||||
|
# 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
|
||||||
|
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}
|
||||||
|
|
||||||
|
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
|
||||||
|
|
||||||
|
echo "INFO [init] Spawning model"
|
||||||
|
|
||||||
|
# Spawn model
|
||||||
|
${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \
|
||||||
|
--reptype gz.msgs.Boolean --timeout 5000 \
|
||||||
|
--req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}/model.sdf\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1
|
||||||
|
|
||||||
|
# Wait for model to spawn
|
||||||
|
sleep 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
|
||||||
|
|
||||||
|
# Set physics parameters for faster-than-realtime simulation if needed
|
||||||
|
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 5000 \
|
||||||
|
--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
|
||||||
|
|
||||||
|
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 -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 either PX4_GZ_MODEL_NAME or PX4_SIM_MODEL"
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
|
|
||||||
|
# NOTE: Only for rover_mecanum and spacecraft_2d. All other models have
|
||||||
|
# the magnetometer sensor in the model.sdf.
|
||||||
|
if param compare -s SENS_EN_MAGSIM 1
|
||||||
|
then
|
||||||
|
sensor_mag_sim start
|
||||||
|
fi
|
||||||
|
# NOTE: new gz has airspeed sensor, remove once added
|
||||||
|
if param compare -s SENS_EN_ARSPDSIM 1
|
||||||
|
then
|
||||||
|
sensor_airspeed_sim start
|
||||||
|
fi
|
||||||
@@ -0,0 +1,14 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
# shellcheck disable=SC2154
|
||||||
|
|
||||||
|
echo "INFO [init] jMAVSim simulator"
|
||||||
|
|
||||||
|
if jps | grep -i jmavsim; then
|
||||||
|
kill "$(jps | grep -i jmavsim | awk '{print $1}')" || true
|
||||||
|
sleep 1
|
||||||
|
fi
|
||||||
|
|
||||||
|
param set IMU_INTEG_RATE 250
|
||||||
|
./jmavsim_run.sh -l -r 250 &
|
||||||
|
|
||||||
|
simulator_mavlink start -h localhost $((4560+px4_instance))
|
||||||
@@ -0,0 +1,27 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
# shellcheck disable=SC2154
|
||||||
|
|
||||||
|
# EKF2 specifics
|
||||||
|
param set-default EKF2_GPS_DELAY 10
|
||||||
|
param set-default EKF2_MULTI_IMU 3
|
||||||
|
param set-default SENS_IMU_MODE 0
|
||||||
|
|
||||||
|
simulator_tcp_port=$((4560+px4_instance))
|
||||||
|
|
||||||
|
# Check if PX4_SIM_HOSTNAME environment variable is empty
|
||||||
|
# If empty check if PX4_SIM_HOST_ADDR environment variable is empty
|
||||||
|
# If both are empty use localhost for simulator
|
||||||
|
if [ -z "${PX4_SIM_HOSTNAME}" ]; then
|
||||||
|
|
||||||
|
if [ -z "${PX4_SIM_HOST_ADDR}" ]; then
|
||||||
|
echo "INFO [init] PX4_SIM_HOSTNAME: localhost"
|
||||||
|
simulator_mavlink start -c $simulator_tcp_port
|
||||||
|
else
|
||||||
|
echo "INFO [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOST_ADDR}"
|
||||||
|
simulator_mavlink start -t "${PX4_SIM_HOST_ADDR}" "${simulator_tcp_port}"
|
||||||
|
fi
|
||||||
|
|
||||||
|
else
|
||||||
|
echo "INFO [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOSTNAME}"
|
||||||
|
simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}"
|
||||||
|
fi
|
||||||
@@ -1,5 +0,0 @@
|
|||||||
#!/bin/sh
|
|
||||||
# shellcheck disable=SC2154
|
|
||||||
|
|
||||||
#param set-default MAV_SYS_ID $((px4_instance+1))
|
|
||||||
#param set-default IMU_INTEG_RATE 250
|
|
||||||
@@ -0,0 +1,38 @@
|
|||||||
|
#!/bin/sh
|
||||||
|
|
||||||
|
echo "INFO [init] SIH simulator"
|
||||||
|
|
||||||
|
if [ -n "${PX4_HOME_LAT}" ]; then
|
||||||
|
param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
|
||||||
|
fi
|
||||||
|
|
||||||
|
if [ -n "${PX4_HOME_LON}" ]; then
|
||||||
|
param set SIH_LOC_LON0 ${PX4_HOME_LON}
|
||||||
|
fi
|
||||||
|
if [ -n "${PX4_HOME_ALT}" ]; then
|
||||||
|
param set SIH_LOC_H0 ${PX4_HOME_ALT}
|
||||||
|
fi
|
||||||
|
|
||||||
|
if simulator_sih start; 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_AGPSIM 1
|
||||||
|
then
|
||||||
|
sensor_agp_sim start
|
||||||
|
fi
|
||||||
|
|
||||||
|
else
|
||||||
|
echo "ERROR [init] simulator_sih failed to start"
|
||||||
|
exit 1
|
||||||
|
fi
|
||||||
@@ -8,272 +8,18 @@ param set-default IMU_INTEG_RATE 250
|
|||||||
param set-default COM_MODE_ARM_CHK 1
|
param set-default COM_MODE_ARM_CHK 1
|
||||||
|
|
||||||
if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then
|
if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then
|
||||||
|
# Run SIH
|
||||||
echo "INFO [init] SIH simulator"
|
. px4-rc.sihsim
|
||||||
|
|
||||||
if [ -n "${PX4_HOME_LAT}" ]; then
|
|
||||||
param set SIH_LOC_LAT0 ${PX4_HOME_LAT}
|
|
||||||
fi
|
|
||||||
|
|
||||||
if [ -n "${PX4_HOME_LON}" ]; then
|
|
||||||
param set SIH_LOC_LON0 ${PX4_HOME_LON}
|
|
||||||
fi
|
|
||||||
if [ -n "${PX4_HOME_ALT}" ]; then
|
|
||||||
param set SIH_LOC_H0 ${PX4_HOME_ALT}
|
|
||||||
fi
|
|
||||||
|
|
||||||
if simulator_sih start; 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_AGPSIM 1
|
|
||||||
then
|
|
||||||
sensor_agp_sim start
|
|
||||||
fi
|
|
||||||
|
|
||||||
else
|
|
||||||
echo "ERROR [init] simulator_sih failed to start"
|
|
||||||
exit 1
|
|
||||||
fi
|
|
||||||
|
|
||||||
elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
|
elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then
|
||||||
|
# Run Gazebo (gz)
|
||||||
echo "INFO [init] Gazebo simulator"
|
. px4-rc.gzsim
|
||||||
|
|
||||||
if [ -z "${PX4_GZ_STANDALONE}" ]; then
|
|
||||||
|
|
||||||
# Enforce minimum gz version as Harmonic (gz-sim8)
|
|
||||||
MIN_GZ_VERSION="8.0.0"
|
|
||||||
GZ_SIM_VERSION=$(gz sim --versions 2>/dev/null | head -n 1 | tr -d ' ')
|
|
||||||
|
|
||||||
if [ -z "$GZ_SIM_VERSION" ]; then
|
|
||||||
echo "ERROR [init] Gazebo gz sim not found. Please install gz-harmonic"
|
|
||||||
exit 1
|
|
||||||
fi
|
|
||||||
|
|
||||||
# Use sort compare, check that MIN_GZ_VERSION is ordered last
|
|
||||||
if [ "$(printf '%s\n' "$GZ_SIM_VERSION" "$MIN_GZ_VERSION" | sort -V | head -n1)" = "$MIN_GZ_VERSION" ]; then
|
|
||||||
gz_command="gz"
|
|
||||||
gz_sub_command="sim"
|
|
||||||
|
|
||||||
# Specify render engine if `GZ_SIM_RENDER_ENGINE` is set
|
|
||||||
# (for example, if you want to use Ogre 1.x instead of Ogre 2.x):
|
|
||||||
if [ -n "${PX4_GZ_SIM_RENDER_ENGINE}" ]; then
|
|
||||||
echo "INFO [init] Setting Gazebo render engine to '${PX4_GZ_SIM_RENDER_ENGINE}'!"
|
|
||||||
gz_sub_command="${gz_sub_command} --render-engine ${PX4_GZ_SIM_RENDER_ENGINE}"
|
|
||||||
fi
|
|
||||||
else
|
|
||||||
echo "ERROR [init] Gazebo gz sim version is too old ($GZ_SIM_VERSION). Minimum required version is $MIN_GZ_VERSION"
|
|
||||||
exit 1
|
|
||||||
fi
|
|
||||||
|
|
||||||
# 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
|
|
||||||
|
|
||||||
# Setup gz environment variables
|
|
||||||
if [ -f ./gz_env.sh ]; then
|
|
||||||
. ./gz_env.sh
|
|
||||||
|
|
||||||
elif [ -f ../gz_env.sh ]; then
|
|
||||||
. ../gz_env.sh
|
|
||||||
fi
|
|
||||||
|
|
||||||
echo "INFO [init] Starting gazebo with world: ${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf"
|
|
||||||
|
|
||||||
${gz_command} ${gz_sub_command} --verbose=${GZ_VERBOSE:=1} -r -s "${PX4_GZ_WORLDS}/${PX4_GZ_WORLD}.sdf" &
|
|
||||||
|
|
||||||
if [ -z "${HEADLESS}" ]; then
|
|
||||||
echo "INFO [init] Starting gz gui"
|
|
||||||
${gz_command} ${gz_sub_command} -g > /dev/null 2>&1 &
|
|
||||||
fi
|
|
||||||
|
|
||||||
# Wait for Gazebo world to be ready before proceeding
|
|
||||||
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
|
|
||||||
|
|
||||||
else
|
|
||||||
# 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 - either spawn a model or connect to existing one
|
|
||||||
if [ -n "${PX4_SIM_MODEL#*gz_}" ] && [ -z "${PX4_GZ_MODEL_NAME}" ]; then
|
|
||||||
# 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
|
|
||||||
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}
|
|
||||||
|
|
||||||
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
|
|
||||||
|
|
||||||
echo "INFO [init] Spawning model"
|
|
||||||
|
|
||||||
# Spawn model
|
|
||||||
${gz_command} service -s "/world/${PX4_GZ_WORLD}/create" --reqtype gz.msgs.EntityFactory \
|
|
||||||
--reptype gz.msgs.Boolean --timeout 5000 \
|
|
||||||
--req "sdf_filename: \"${PX4_GZ_MODELS}/${MODEL_NAME}/model.sdf\", name: \"${MODEL_NAME_INSTANCE}\", allow_renaming: false${POSE_ARG}" > /dev/null 2>&1
|
|
||||||
|
|
||||||
# Wait for model to spawn
|
|
||||||
sleep 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
|
|
||||||
|
|
||||||
# Set physics parameters for faster-than-realtime simulation if needed
|
|
||||||
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 5000 \
|
|
||||||
--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
|
|
||||||
|
|
||||||
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 -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 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
|
|
||||||
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
|
|
||||||
if param compare -s SENS_EN_AGPSIM 1
|
|
||||||
then
|
|
||||||
sensor_agp_sim start
|
|
||||||
fi
|
|
||||||
|
|
||||||
elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "10017" ]; then
|
elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "10017" ]; then
|
||||||
|
# Run jMAVSim
|
||||||
echo "INFO [init] jMAVSim simulator"
|
. px4-rc.jmavsim
|
||||||
|
|
||||||
if jps | grep -i jmavsim; then
|
|
||||||
kill "$(jps | grep -i jmavsim | awk '{print $1}')" || true
|
|
||||||
sleep 1
|
|
||||||
fi
|
|
||||||
|
|
||||||
param set IMU_INTEG_RATE 250
|
|
||||||
./jmavsim_run.sh -l -r 250 &
|
|
||||||
|
|
||||||
simulator_mavlink start -h localhost $((4560+px4_instance))
|
|
||||||
|
|
||||||
else
|
else
|
||||||
# otherwise start simulator (mavlink) module
|
# Run simulator_mavlink module for interface with gazebo-classic
|
||||||
|
. px4-rc.mavlinksim
|
||||||
# EKF2 specifics
|
|
||||||
param set-default EKF2_GPS_DELAY 10
|
|
||||||
param set-default EKF2_MULTI_IMU 3
|
|
||||||
param set-default SENS_IMU_MODE 0
|
|
||||||
|
|
||||||
simulator_tcp_port=$((4560+px4_instance))
|
|
||||||
|
|
||||||
# Check if PX4_SIM_HOSTNAME environment variable is empty
|
|
||||||
# If empty check if PX4_SIM_HOST_ADDR environment variable is empty
|
|
||||||
# If both are empty use localhost for simulator
|
|
||||||
if [ -z "${PX4_SIM_HOSTNAME}" ]; then
|
|
||||||
|
|
||||||
if [ -z "${PX4_SIM_HOST_ADDR}" ]; then
|
|
||||||
echo "INFO [init] PX4_SIM_HOSTNAME: localhost"
|
|
||||||
simulator_mavlink start -c $simulator_tcp_port
|
|
||||||
else
|
|
||||||
echo "INFO [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOST_ADDR}"
|
|
||||||
simulator_mavlink start -t "${PX4_SIM_HOST_ADDR}" "${simulator_tcp_port}"
|
|
||||||
fi
|
|
||||||
|
|
||||||
else
|
|
||||||
echo "INFO [init] PX4_SIM_HOSTNAME: ${PX4_SIM_HOSTNAME}"
|
|
||||||
simulator_mavlink start -h "${PX4_SIM_HOSTNAME}" "${simulator_tcp_port}"
|
|
||||||
fi
|
|
||||||
|
|
||||||
fi
|
fi
|
||||||
|
|||||||
@@ -227,9 +227,6 @@ then
|
|||||||
exit 1
|
exit 1
|
||||||
fi
|
fi
|
||||||
|
|
||||||
#user defined params for instances can be in PATH
|
|
||||||
. px4-rc.params
|
|
||||||
|
|
||||||
dataman start
|
dataman start
|
||||||
|
|
||||||
# only start the simulator if not in replay mode, as both control the lockstep time
|
# only start the simulator if not in replay mode, as both control the lockstep time
|
||||||
|
|||||||
+1
-1
Submodule Tools/simulation/gz updated: 6c18846a4c...6caa6d54fa
Reference in New Issue
Block a user