From 9e8b530ad92207de902868e1bf8cdd4e29bdd3df Mon Sep 17 00:00:00 2001 From: Jacob Dahl <37091262+dakejahl@users.noreply.github.com> Date: Tue, 25 Mar 2025 09:49:03 -0800 Subject: [PATCH] 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 --- .../px4fmu_common/init.d-posix/CMakeLists.txt | 5 +- ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim | 180 ++++++++++++ .../px4fmu_common/init.d-posix/px4-rc.jmavsim | 14 + .../init.d-posix/px4-rc.mavlinksim | 27 ++ .../px4fmu_common/init.d-posix/px4-rc.params | 5 - .../px4fmu_common/init.d-posix/px4-rc.sihsim | 38 +++ .../init.d-posix/px4-rc.simulator | 270 +----------------- ROMFS/px4fmu_common/init.d-posix/rcS | 3 - Tools/simulation/gz | 2 +- 9 files changed, 272 insertions(+), 272 deletions(-) create mode 100644 ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim create mode 100644 ROMFS/px4fmu_common/init.d-posix/px4-rc.jmavsim create mode 100644 ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim delete mode 100644 ROMFS/px4fmu_common/init.d-posix/px4-rc.params create mode 100644 ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim diff --git a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt index 1ba7ccde27..0772ccdc7c 100644 --- a/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt +++ b/ROMFS/px4fmu_common/init.d-posix/CMakeLists.txt @@ -35,8 +35,11 @@ add_subdirectory(airframes) px4_add_romfs_files( px4-rc.mavlink - px4-rc.params px4-rc.simulator + px4-rc.gzsim + px4-rc.jmavsim + px4-rc.mavlinksim + px4-rc.sihsim rc.replay rcS ) diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim new file mode 100644 index 0000000000..9cb534c57b --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.gzsim @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.jmavsim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.jmavsim new file mode 100644 index 0000000000..bb488d2c52 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.jmavsim @@ -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)) diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim new file mode 100644 index 0000000000..f2d571062c --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.mavlinksim @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.params b/ROMFS/px4fmu_common/init.d-posix/px4-rc.params deleted file mode 100644 index d05f50146a..0000000000 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.params +++ /dev/null @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim b/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim new file mode 100644 index 0000000000..3e10bd631d --- /dev/null +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.sihsim @@ -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 diff --git a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator index 03218e5813..100e0a67ca 100644 --- a/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator +++ b/ROMFS/px4fmu_common/init.d-posix/px4-rc.simulator @@ -8,272 +8,18 @@ param set-default IMU_INTEG_RATE 250 param set-default COM_MODE_ARM_CHK 1 if [ "$PX4_SIMULATOR" = "sihsim" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "0" ]; then - - 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 + # Run SIH + . px4-rc.sihsim elif [ "$PX4_SIMULATOR" = "gz" ] || [ "$(param show -q SIM_GZ_EN)" = "1" ]; then - - echo "INFO [init] Gazebo simulator" - - 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 + # Run Gazebo (gz) + . px4-rc.gzsim elif [ "$PX4_SIM_MODEL" = "jmavsim_iris" ] || [ "$(param show -q SYS_AUTOSTART)" -eq "10017" ]; then - - 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)) + # Run jMAVSim + . px4-rc.jmavsim else - # otherwise start simulator (mavlink) module - - # 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 - + # Run simulator_mavlink module for interface with gazebo-classic + . px4-rc.mavlinksim fi diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS index c53dbbd46a..7114c054bf 100644 --- a/ROMFS/px4fmu_common/init.d-posix/rcS +++ b/ROMFS/px4fmu_common/init.d-posix/rcS @@ -227,9 +227,6 @@ then exit 1 fi -#user defined params for instances can be in PATH -. px4-rc.params - dataman start # only start the simulator if not in replay mode, as both control the lockstep time diff --git a/Tools/simulation/gz b/Tools/simulation/gz index 6c18846a4c..6caa6d54fa 160000 --- a/Tools/simulation/gz +++ b/Tools/simulation/gz @@ -1 +1 @@ -Subproject commit 6c18846a4c7f9fe786840a29bf4e3237f908611b +Subproject commit 6caa6d54fa95b0141c9974bbd836b0f6fe0f26ee