simulation organization and cleanup

- new modules/simulation directory to collect all simulators and related modules
 - new Tools/simulation directory to collect and organize scattered simulation submodules, scripts, etc
 - simulation module renamed to simulator_mavlink
 - sih renamed to simulator_sih (not a great name, but I wanted to be clear it was a simulator)
 - ignition_simulator renamed to simulator_ignition_bridge
 - large sitl_target.cmake split by simulation option and in some cases pushed to appropriate modules
 - sitl targets broken down to what's actually available (eg jmavsim only has 1 model and 1 world)
 - new Gazebo consistently referred to as Ignition for now (probably the least confusing thing until we fully drop Gazebo classic support someday)

Tools/simulation/flightgear/sitl_run.sh

@@ -0,0 +1,47 @@
+#!/usr/bin/env bash
+
+set -e
+
+if [ "$#" -lt 4 ]; then
+	echo usage: sitl_run.sh sitl_bin model src_path build_path
+	exit 1
+fi
+
+sitl_bin="$1"
+model="$2"
+src_path="$3"
+build_path="$4"
+
+echo SITL ARGS
+
+echo sitl_bin: $sitl_bin
+echo model: $model
+echo src_path: $src_path
+echo build_path: $build_path
+
+rootfs="$build_path/rootfs" # this is the working directory
+mkdir -p "$rootfs"
+
+export PX4_SIM_MODEL=${model}
+
+echo "FG setup"
+cd "${src_path}/Tools/simulation/flightgear/flightgear_bridge/"
+./FG_run.py models/${model}.json 0
+"${build_path}/build_flightgear_bridge/flightgear_bridge" 0 `./get_FGbridge_params.py "models/"${model}".json"` &
+FG_BRIDGE_PID=$!
+
+pushd "$rootfs" >/dev/null
+
+# Do not exit on failure now from here on because we want the complete cleanup
+set +e
+
+sitl_command="\"$sitl_bin\" \"$build_path\"/etc"
+
+echo SITL COMMAND: $sitl_command
+
+eval $sitl_command
+
+popd >/dev/null
+
+kill $FG_BRIDGE_PID
+kill -9 `cat /tmp/px4fgfspid_0`

Tools/simulation/gazebo/setup_gazebo.bash

@@ -0,0 +1,25 @@
+#!/bin/bash
+#
+# Setup environment to make PX4 visible to Gazebo.
+#
+# Note, this is not necessary if using a ROS catkin workspace with the px4
+# package as the paths are exported.
+#
+# License: according to LICENSE.md in the root directory of the PX4 Firmware repository
+
+if [ "$#" != 2 ]; then
+    echo -e "usage: source setup_gazebo.bash src_dir build_dir\n"
+    return 1
+fi
+
+SRC_DIR=$1
+BUILD_DIR=$2
+
+# setup Gazebo env and update package path
+export GAZEBO_PLUGIN_PATH=$GAZEBO_PLUGIN_PATH:${BUILD_DIR}/build_gazebo
+export GAZEBO_MODEL_PATH=$GAZEBO_MODEL_PATH:${SRC_DIR}/Tools/simulation/gazebo/sitl_gazebo/models
+export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:${BUILD_DIR}/build_gazebo
+
+echo -e "GAZEBO_PLUGIN_PATH $GAZEBO_PLUGIN_PATH"
+echo -e "GAZEBO_MODEL_PATH $GAZEBO_MODEL_PATH"
+echo -e "LD_LIBRARY_PATH $LD_LIBRARY_PATH"

Tools/simulation/gazebo/sitl_multiple_run.sh

@@ -0,0 +1,140 @@
+#!/bin/bash
+# run multiple instances of the 'px4' binary, with the gazebo SITL simulation
+# It assumes px4 is already built, with 'make px4_sitl_default gazebo'
+
+# The simulator is expected to send to TCP port 4560+i for i in [0, N-1]
+# For example gazebo can be run like this:
+#./Tools/simulation/gazebo/sitl_multiple_run.sh -n 10 -m iris
+
+function cleanup() {
+	pkill -x px4
+	pkill gzclient
+	pkill gzserver
+}
+
+function spawn_model() {
+	MODEL=$1
+	N=$2 #Instance Number
+	X=$3
+	Y=$4
+	X=${X:=0.0}
+	Y=${Y:=$((3*${N}))}
+
+	SUPPORTED_MODELS=("iris" "plane" "standard_vtol" "rover" "r1_rover" "typhoon_h480")
+	if [[ " ${SUPPORTED_MODELS[*]} " != *"$MODEL"* ]];
+	then
+		echo "ERROR: Currently only vehicle model $MODEL is not supported!"
+		echo "       Supported Models: [${SUPPORTED_MODELS[@]}]"
+		trap "cleanup" SIGINT SIGTERM EXIT
+		exit 1
+	fi
+
+	working_dir="$build_path/rootfs/$n"
+	[ ! -d "$working_dir" ] && mkdir -p "$working_dir"
+
+	pushd "$working_dir" &>/dev/null
+	echo "starting instance $N in $(pwd)"
+	$build_path/bin/px4 -i $N -d "$build_path/etc" >out.log 2>err.log &
+	python3 ${src_path}/Tools/simulation/gazebo/sitl_gazebo/scripts/jinja_gen.py ${src_path}/Tools/simulation/gazebo/sitl_gazebo/models/${MODEL}/${MODEL}.sdf.jinja ${src_path}/Tools/simulation/gazebo/sitl_gazebo --mavlink_tcp_port $((4560+${N})) --mavlink_udp_port $((14560+${N})) --mavlink_id $((1+${N})) --gst_udp_port $((5600+${N})) --video_uri $((5600+${N})) --mavlink_cam_udp_port $((14530+${N})) --output-file /tmp/${MODEL}_${N}.sdf
+
+	echo "Spawning ${MODEL}_${N} at ${X} ${Y}"
+
+	gz model --spawn-file=/tmp/${MODEL}_${N}.sdf --model-name=${MODEL}_${N} -x ${X} -y ${Y} -z 0.83
+
+	popd &>/dev/null
+
+}
+
+if [ "$1" == "-h" ] || [ "$1" == "--help" ]
+then
+	echo "Usage: $0 [-n <num_vehicles>] [-m <vehicle_model>] [-w <world>] [-s <script>]"
+	echo "-s flag is used to script spawning vehicles e.g. $0 -s iris:3,plane:2"
+	exit 1
+fi
+
+while getopts n:m:w:s:t:l: option
+do
+	case "${option}"
+	in
+		n) NUM_VEHICLES=${OPTARG};;
+		m) VEHICLE_MODEL=${OPTARG};;
+		w) WORLD=${OPTARG};;
+		s) SCRIPT=${OPTARG};;
+		t) TARGET=${OPTARG};;
+		l) LABEL=_${OPTARG};;
+	esac
+done
+
+num_vehicles=${NUM_VEHICLES:=3}
+world=${WORLD:=empty}
+target=${TARGET:=px4_sitl_default}
+vehicle_model=${VEHICLE_MODEL:="iris"}
+export PX4_SIM_MODEL=${vehicle_model}
+
+echo ${SCRIPT}
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+src_path="$SCRIPT_DIR/../../.."
+
+build_path=${src_path}/build/${target}
+mavlink_udp_port=14560
+mavlink_tcp_port=4560
+
+echo "killing running instances"
+pkill -x px4 || true
+
+sleep 1
+
+source ${src_path}/Tools/simulation/gazebo/setup_gazebo.bash ${src_path} ${src_path}/build/${target}
+
+# To use gazebo_ros ROS2 plugins
+if [[ -n "$ROS_VERSION" ]] && [ "$ROS_VERSION" == "2" ]; then
+	ros_args="-s libgazebo_ros_init.so -s libgazebo_ros_factory.so"
+else
+	ros_args=""
+fi
+
+echo "Starting gazebo"
+gzserver ${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/${world}.world --verbose $ros_args &
+sleep 5
+
+n=0
+if [ -z ${SCRIPT} ]; then
+	if [ $num_vehicles -gt 255 ]
+	then
+		echo "Tried spawning $num_vehicles vehicles. The maximum number of supported vehicles is 255"
+		exit 1
+	fi
+
+	while [ $n -lt $num_vehicles ]; do
+		spawn_model ${vehicle_model} $n
+		n=$(($n + 1))
+	done
+else
+	IFS=,
+	for target in ${SCRIPT}; do
+		target="$(echo "$target" | tr -d ' ')" #Remove spaces
+		target_vehicle=$(echo $target | cut -f1 -d:)
+		target_number=$(echo $target | cut -f2 -d:)
+		target_x=$(echo $target | cut -f3 -d:)
+		target_y=$(echo $target | cut -f4 -d:)
+
+		if [ $n -gt 255 ]
+		then
+			echo "Tried spawning $n vehicles. The maximum number of supported vehicles is 255"
+			exit 1
+		fi
+
+		m=0
+		while [ $m -lt ${target_number} ]; do
+			export PX4_SIM_MODEL=${target_vehicle}
+			spawn_model ${target_vehicle}${LABEL} $n $target_x $target_y
+			m=$(($m + 1))
+			n=$(($n + 1))
+		done
+	done
+
+fi
+trap "cleanup" SIGINT SIGTERM EXIT
+
+echo "Starting gazebo client"
+gzclient

Tools/simulation/gazebo/sitl_run.sh

@@ -0,0 +1,180 @@
+#!/usr/bin/env bash
+set -e
+
+if [ "$#" -lt 6 ]; then
+	echo usage: sitl_run.sh sitl_bin debugger model world src_path build_path
+	exit 1
+fi
+
+if [[ -n "$DONT_RUN" ]]; then
+	echo "Not running simulation (DONT_RUN is set)."
+	exit 0
+fi
+
+sitl_bin="$1"
+debugger="$2"
+model="$3"
+world="$4"
+src_path="$5"
+build_path="$6"
+
+echo SITL ARGS
+
+echo sitl_bin: $sitl_bin
+echo debugger: $debugger
+echo model: $model
+echo world: $world
+echo src_path: $src_path
+echo build_path: $build_path
+
+rootfs="$build_path/rootfs" # this is the working directory
+mkdir -p "$rootfs"
+
+# To disable user input
+if [[ -n "$NO_PXH" ]]; then
+	no_pxh=-d
+else
+	no_pxh=""
+fi
+
+# To disable user input
+if [[ -n "$VERBOSE_SIM" ]]; then
+	verbose="--verbose"
+else
+	verbose=""
+fi
+
+# Disable follow mode
+if [[ "$PX4_NO_FOLLOW_MODE" != "1" ]]; then
+    follow_mode="--gui-client-plugin libgazebo_user_camera_plugin.so"
+else
+    follow_mode=""
+fi
+
+# To use gazebo_ros ROS2 plugins
+if [[ -n "$ROS_VERSION" ]] && [ "$ROS_VERSION" == "2" ]; then
+	ros_args="-s libgazebo_ros_init.so -s libgazebo_ros_factory.so"
+else
+	ros_args=""
+fi
+
+if [ "$model" == "" ] || [ "$model" == "none" ]; then
+	echo "empty model, setting iris as default"
+	model="iris"
+fi
+
+# kill process names that might stil
+# be running from last time
+pkill -x gazebo || true
+
+export PX4_SIM_MODEL=${model}
+export PX4_SIM_WORLD=${world}
+
+SIM_PID=0
+
+if [ -x "$(command -v gazebo)" ]; then
+	# Get the model name
+	model_name="${model}"
+	# Check if a 'modelname-gen.sdf' file exist for the models using jinja and generating the SDF files
+	if [ -f "${src_path}/Tools/simulation/gazebo/sitl_gazebo/models/${model}/${model}-gen.sdf" ]; then
+		model_name="${model}-gen"
+	fi
+
+	# Set the plugin path so Gazebo finds our model and sim
+	source "$src_path/Tools/simulation/gazebo/setup_gazebo.bash" "${src_path}" "${build_path}"
+	if [ -z $PX4_SITL_WORLD ]; then
+		#Spawn predefined world
+		if [ "$world" == "none" ]; then
+			if [ -f ${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/${model}.world ]; then
+				echo "empty world, default world ${model}.world for model found"
+				world_path="${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/${model}.world"
+			else
+				echo "empty world, setting empty.world as default"
+				world_path="${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/empty.world"
+			fi
+		else
+			#Spawn empty world if world with model name doesn't exist
+			world_path="${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/${world}.world"
+		fi
+	else
+		if [ -f ${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/${PX4_SITL_WORLD}.world ]; then
+			# Spawn world by name if exists in the worlds directory from environment variable
+			world_path="${src_path}/Tools/simulation/gazebo/sitl_gazebo/worlds/${PX4_SITL_WORLD}.world"
+		else
+			# Spawn world from environment variable with absolute path
+			world_path="$PX4_SITL_WORLD"
+		fi
+	fi
+	gzserver $verbose $world_path $ros_args &
+	SIM_PID=$!
+
+	# Check all paths in ${GAZEBO_MODEL_PATH} for specified model
+	IFS_bak=$IFS
+	IFS=":"
+	for possible_model_path in ${GAZEBO_MODEL_PATH}; do
+		if [ -z $possible_model_path ]; then
+			continue
+		fi
+		# trim \r from path
+		possible_model_path=$(echo $possible_model_path | tr -d '\r')
+		if test -f "${possible_model_path}/${model}/${model}.sdf" ; then
+			modelpath=$possible_model_path
+			break
+		fi
+	done
+	IFS=$IFS_bak
+
+	if [ -z $modelpath ]; then
+		echo "Model ${model} not found in model path: ${GAZEBO_MODEL_PATH}"
+		exit 1
+	else
+		echo "Using: ${modelpath}/${model}/${model}.sdf"
+	fi
+
+	while gz model --verbose --spawn-file="${modelpath}/${model}/${model_name}.sdf" --model-name=${model} -x 1.01 -y 0.98 -z 0.83 2>&1 | grep -q "An instance of Gazebo is not running."; do
+		echo "gzserver not ready yet, trying again!"
+		sleep 1
+	done
+
+	if [[ -n "$HEADLESS" ]]; then
+		echo "not running gazebo gui"
+	else
+		# gzserver needs to be running to avoid a race. Since the launch
+		# is putting it into the background we need to avoid it by backing off
+		sleep 3
+		nice -n 20 gzclient --verbose $follow_mode &
+		GUI_PID=$!
+	fi
+else
+	echo "You need to have gazebo simulator installed!"
+	exit 1
+fi
+
+pushd "$rootfs" >/dev/null
+
+# Do not exit on failure now from here on because we want the complete cleanup
+set +e
+
+sitl_command="\"$sitl_bin\" $no_pxh \"$build_path\"/etc"
+
+echo SITL COMMAND: $sitl_command
+
+if [ "$debugger" == "lldb" ]; then
+	eval lldb -- $sitl_command
+elif [ "$debugger" == "gdb" ]; then
+	eval gdb --args $sitl_command
+elif [ "$debugger" == "valgrind" ]; then
+	eval valgrind --track-origins=yes --leak-check=full -v $sitl_command
+elif [ "$debugger" == "callgrind" ]; then
+	eval valgrind --tool=callgrind -v $sitl_command
+else
+	eval $sitl_command
+fi
+
+popd >/dev/null
+
+
+kill -9 $SIM_PID
+if [[ ! -n "$HEADLESS" ]]; then
+	kill -9 $GUI_PID
+fi

Tools/simulation/ignition/worlds/default.sdf

@@ -1,6 +1,6 @@
 <?xml version="1.0" encoding="UTF-8"?>
 <sdf version="1.9">
-   <world name="empty">
+   <world name="default">
       <physics type="ode">
          <max_step_size>0.004</max_step_size>
          <real_time_factor>1.0</real_time_factor>

Tools/simulation/jmavsim/jmavsim_run.sh

@@ -0,0 +1,76 @@
+#!/usr/bin/env bash
+
+set -e
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+cd "$SCRIPT_DIR/jMAVSim"
+
+port=4560
+extra_args=
+baudrate=921600
+device=
+ip="127.0.0.1"
+protocol="tcp"
+while getopts ":b:d:u:p:qsr:f:i:loat" opt; do
+	case $opt in
+		b)
+			baudrate=$OPTARG
+			;;
+		d)
+			device="$OPTARG"
+			;;
+		u)
+			protocol="udp"
+			;;
+		i)
+			ip="$OPTARG"
+			;;
+		p)
+			port=$OPTARG
+			;;
+		q)
+			extra_args="$extra_args -qgc"
+			;;
+		s)
+			extra_args="$extra_args -sdk"
+			;;
+		r)
+			extra_args="$extra_args -r $OPTARG"
+			;;
+		l)
+			extra_args="$extra_args -lockstep"
+			;;
+		o)
+			extra_args="$extra_args -disponly"
+			;;
+		a)
+			extra_args="$extra_args -fw" # aircraft model
+			;;
+		t)
+			extra_args="$extra_args -ts" # tailsitter model
+			;;
+		\?)
+			echo "Invalid option: -$OPTARG" >&2
+			exit 1
+			;;
+	esac
+done
+
+if [ "$device" == "" ]; then
+	if [ "$protocol" == "tcp" ]; then
+		device="-tcp $ip:$port"
+	else
+		device="-udp $port"
+	fi
+else
+	device="-serial $device $baudrate"
+fi
+
+if [ "$HEADLESS" = "1" ]; then
+    extra_args="$extra_args -no-gui"
+fi
+
+ant create_run_jar copy_res
+cd out/production
+
+java -XX:GCTimeRatio=20 -Djava.ext.dirs= -jar jmavsim_run.jar $device $extra_args

Tools/simulation/jmavsim/sitl_run.sh

@@ -0,0 +1,67 @@
+#!/usr/bin/env bash
+
+set -e
+
+if [ "$#" -lt 3 ]; then
+	echo usage: sitl_run.sh sitl_bin debugger src_path build_path
+	exit 1
+fi
+
+sitl_bin="$1"
+debugger="$2"
+src_path="$3"
+build_path="$4"
+
+echo SITL ARGS
+
+echo sitl_bin: $sitl_bin
+echo debugger: $debugger
+echo src_path: $src_path
+echo build_path: $build_path
+
+rootfs="$build_path/rootfs" # this is the working directory
+mkdir -p "$rootfs"
+
+# To disable user input
+if [[ -n "$NO_PXH" ]]; then
+	no_pxh=-d
+else
+	no_pxh=""
+fi
+
+jmavsim_pid=`ps aux | grep java | grep "\-jar jmavsim_run.jar" | awk '{ print $2 }'`
+if [ -n "$jmavsim_pid" ]; then
+	kill $jmavsim_pid
+fi
+
+export PX4_SIM_MODEL="iris"
+
+# Start Java simulator
+"$src_path"/Tools/simulation/jmavsim/jmavsim_run.sh -r 250 -l &
+SIM_PID=$!
+
+pushd "$rootfs" >/dev/null
+
+# Do not exit on failure now from here on because we want the complete cleanup
+set +e
+
+sitl_command="\"$sitl_bin\" $no_pxh \"$build_path\"/etc"
+
+echo SITL COMMAND: $sitl_command
+
+if [ "$debugger" == "lldb" ]; then
+	eval lldb -- $sitl_command
+elif [ "$debugger" == "gdb" ]; then
+	eval gdb --args $sitl_command
+elif [ "$debugger" == "valgrind" ]; then
+	eval valgrind --track-origins=yes --leak-check=full -v $sitl_command
+elif [ "$debugger" == "callgrind" ]; then
+	eval valgrind --tool=callgrind -v $sitl_command
+else
+	eval $sitl_command
+fi
+
+popd >/dev/null
+
+pkill -9 -P $SIM_PID
+kill -9 $SIM_PID

Tools/simulation/jsbsim/sitl_run.sh

@@ -0,0 +1,96 @@
+#!/usr/bin/env bash
+
+
+set -e
+
+if [ "$#" -lt 5 ]; then
+	echo usage: sitl_run.sh sitl_bin model world src_path build_path
+	exit 1
+fi
+
+if [[ -n "$DONT_RUN" ]]; then
+	echo "Not running simulation (DONT_RUN is set)."
+	exit 0
+fi
+
+sitl_bin="$1"
+model="$2"
+world="$3"
+src_path="$4"
+build_path="$5"
+
+echo SITL ARGS
+
+echo sitl_bin: $sitl_bin
+echo model: $model
+echo world: $world
+echo src_path: $src_path
+echo build_path: $build_path
+
+rootfs="$build_path/rootfs" # this is the working directory
+mkdir -p "$rootfs"
+
+# To disable user input
+if [[ -n "$NO_PXH" ]]; then
+	no_pxh=-d
+else
+	no_pxh=""
+fi
+
+export PX4_SIM_MODEL=${model}
+export PX4_SIM_WORLD=${world}
+
+# This is needed for aircraft namespace mapping
+# Need more architectural discussions to make this more scalable
+case "$model" in
+        rascal)
+            MODEL_NAME="Rascal110-JSBSim"
+            ;;
+        malolo)
+            MODEL_NAME="Malolo1"
+            ;;
+        quadrotor_x)
+            MODEL_NAME="quadrotor_x"
+            ;;
+        hexarotor_x)
+            MODEL_NAME="hexarotor_x"
+            ;;
+        *)
+            echo "Unknown Model"
+            exit 1
+
+esac
+
+export JSBSIM_AIRCRAFT_MODEL="$MODEL_NAME"
+
+if [[ -n "$HEADLESS" ]]; then
+	echo "not running flightgear gui"
+else
+	export FG_AIRCRAFT="${SRC_DIR}/Tools/simulation/jsbsim/jsbsim_bridge/models"
+
+	fgfs --fdm=null \
+		--native-fdm=socket,in,60,,5550,udp \
+		--aircraft=$JSBSIM_AIRCRAFT_MODEL \
+		--airport=${world} \
+		--disable-hud \
+		--disable-ai-models &> /dev/null &
+	FGFS_PID=$!
+fi
+
+"${build_path}/build_jsbsim_bridge/jsbsim_bridge" ${model} -s "${src_path}/Tools/simulation/jsbsim/jsbsim_bridge/scene/${world}.xml" 2> /dev/null &
+JSBSIM_PID=$!
+
+pushd "$rootfs" >/dev/null
+
+# Do not exit on failure now from here on because we want the complete cleanup
+set +e
+
+sitl_command="\"$sitl_bin\" $no_pxh \"$build_path\"/etc"
+
+echo SITL COMMAND: $sitl_command
+eval $sitl_command
+
+popd >/dev/null
+
+kill $JSBSIM_PID
+kill $FGFS_PID

Tools/simulation/sitl_multiple_run.sh

@@ -0,0 +1,35 @@
+#!/bin/bash
+# run multiple instances of the 'px4' binary, but w/o starting the simulator.
+# It assumes px4 is already built, with 'make px4_sitl_default'
+
+# The simulator is expected to send to TCP port 4560+i for i in [0, N-1]
+# For example jmavsim can be run like this:
+#./Tools/simulation/jmavsim/jmavsim_run.sh -p 4561 -l
+
+sitl_num=2
+[ -n "$1" ] && sitl_num="$1"
+
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+src_path="$SCRIPT_DIR/.."
+
+build_path=${src_path}/build/px4_sitl_default
+
+echo "killing running instances"
+pkill -x px4 || true
+
+sleep 1
+
+export PX4_SIM_MODEL=iris
+
+n=0
+while [ $n -lt $sitl_num ]; do
+	working_dir="$build_path/instance_$n"
+	[ ! -d "$working_dir" ] && mkdir -p "$working_dir"
+
+	pushd "$working_dir" &>/dev/null
+	echo "starting instance $n in $(pwd)"
+	$build_path/bin/px4 -i $n -d "$build_path/etc" >out.log 2>err.log &
+	popd &>/dev/null
+
+	n=$(($n + 1))
+done