Benjamin Perseghetti
4 years ago
committed by
Julian Kent
1 changed files with 200 additions and 0 deletions
@ -0,0 +1,200 @@
@@ -0,0 +1,200 @@
|
||||
#!/bin/bash |
||||
#Author: Benjamin Perseghetti |
||||
#Email: bperseghetti@rudislabs.com |
||||
# This script unifies running gazebo simulation for HITL and SITL |
||||
# You can run multiple instances of the 'px4' binary, with the gazebo SITL simulation |
||||
# This script assumes px4 is already built, with 'make px4_sitl_default gazebo' |
||||
# You can also run HITL with -h flag |
||||
# Generate world and/or model files with editable json -j [m (model), w (world), mw (model and world), or wm (world and model)] |
||||
|
||||
# The simulator in SITL is expected to send to TCP port 4560+i for i in [0, N-1] |
||||
# For example gazebo can be run like this for multiple SITL: |
||||
# ./Tools/gz_sim.sh -n 10 -m iris |
||||
# Or gazebo can be run like this for HITL: |
||||
# ./Tools/gz_sim.sh -h 1 -m standard_vtol |
||||
|
||||
function cleanup() { |
||||
echo "running the cleanup" |
||||
pkill -x px4 |
||||
pkill gazebo |
||||
pkill gzclient |
||||
pkill gzserver |
||||
} |
||||
|
||||
trap "cleanup" INT SIGINT SIGTERM EXIT |
||||
|
||||
function spawn_model() { |
||||
MODEL=$1 |
||||
N=$2 #Instance Number |
||||
WORLD_FILE=$3 |
||||
MJ=$4 |
||||
SITL_MODEL_NAME="${MODEL}_${N}" |
||||
sitl_path=${SCRIPT_DIR}/sitl_gazebo |
||||
jinja_model_script=${sitl_path}/scripts/jinja_model_gen.py |
||||
base_model="--base_model ${MODEL}" |
||||
model_json="--json_gen ${MJ}" |
||||
|
||||
if [ $hitl == true ]; then |
||||
python3 ${src_path}/Tools/boot_now.py "/dev/ttyACM0" |
||||
hil_mode="--hil_mode 1" |
||||
model_name="--model_name ${MODEL}" |
||||
hitl_launch_command="${model_json} ${base_model} ${hil_mode} ${model_name}" |
||||
echo "Generating: ${jinja_model_script} ${hitl_launch_command}" |
||||
python3 ${jinja_model_script} ${hitl_launch_command} |
||||
sleep 1 |
||||
source ${src_path}/Tools/setup_gazebo.bash ${src_path} ${src_path}/build/${target} |
||||
sleep 2 |
||||
gazebo ${sitl_path}/worlds/temp_${WORLD_FILE}.world --verbose |
||||
|
||||
else |
||||
mavlink_tcp="--mavlink_tcp_port $((4560+${N}))" |
||||
mavlink_udp="--mavlink_udp_port $((14560+${N}))" |
||||
model_name="--model_name ${SITL_MODEL_NAME}" |
||||
output_path="--output_path /tmp" |
||||
working_dir="$build_path/instance_$n" |
||||
sitl_launch_command="${model_json} ${base_model} ${mavlink_tcp} ${mavlink_udp} ${model_name} ${output_path}" |
||||
[ ! -d "$working_dir" ] && mkdir -p "$working_dir" |
||||
pushd "$working_dir" &>/dev/null |
||||
echo "starting instance $N in $(pwd)" |
||||
../bin/px4 -i $N -d "$build_path/etc" -w sitl_${SITL_MODEL_NAME} -s etc/init.d-posix/rcS >out.log 2>err.log & |
||||
python3 ${jinja_model_script} ${sitl_launch_command} |
||||
echo "Generating: ${jinja_model_script} ${sitl_launch_command}" |
||||
echo "Spawning ${SITL_MODEL_NAME}" |
||||
gz model --spawn-file=/tmp/${SITL_MODEL_NAME}.sdf --model-name=${SITL_MODEL_NAME} -x 0.0 -y $((3*${N})) -z 0.2 |
||||
popd &>/dev/null |
||||
fi |
||||
|
||||
} |
||||
|
||||
if [ "$1" == "--help" ]; then |
||||
echo "Usage: $0 [-n <num_vehicles>] [-m <vehicle_model>] [-h <run_hitl>] [-w <world>] [-s <script>] [-t <num_threads>] [-j <json_params>]" |
||||
echo "-s flag is used to script spawning vehicles e.g. $0 -s iris:3,plane:2" |
||||
echo "-h flag is used to launch a single vehicle in HITL mode" |
||||
echo "-t flag is used to set the number of ODE threads for the world" |
||||
echo "-j flag is used to enable json parameters from gen_params.json for the world (w), model (m), or both (wm or mw)" |
||||
exit 1 |
||||
fi |
||||
|
||||
while getopts n:m:h:w:s:t:j:p option |
||||
do |
||||
case "${option}" |
||||
in |
||||
n) NUM_VEHICLES=${OPTARG};; |
||||
m) VEHICLE_MODEL=${OPTARG};; |
||||
h) HITL=${OPTARG};; |
||||
w) WORLD=${OPTARG};; |
||||
s) SCRIPT=${OPTARG};; |
||||
t) THREADS=${OPTARG};; |
||||
j) JSON=${OPTARG};; |
||||
p) TARGET=${OPTARG};; |
||||
esac |
||||
done |
||||
|
||||
num_vehicles=${NUM_VEHICLES:=1} |
||||
world=${WORLD:=empty} |
||||
hitl=${HITL:=false} |
||||
threads=${THREADS:=1} |
||||
json_opts=${JSON:=0} |
||||
target=${TARGET:=px4_sitl_default} |
||||
system_threads=`grep -Pc '^processor\t' /proc/cpuinfo` |
||||
echo "Number of requested ODE threads: $((threads))" |
||||
echo "Max number of possilbe threads: $((system_threads))" |
||||
if [ $((threads)) -gt $((system_threads)) ]; then |
||||
threads=$system_threads |
||||
echo "Requested ODE thread count too high, set to system max of $threads threads." |
||||
elif [ $(( ${threads} )) -lt 1 ]; then |
||||
threads=1 |
||||
echo "Requested ODE thread count too low, set to $threads thread." |
||||
else |
||||
echo "Using $threads threads for ODE." |
||||
fi |
||||
|
||||
if [ "$json_opts" == "mw" ] || [ "$json_opts" == "wm" ]; then |
||||
echo "JSON used for both world and model generation" |
||||
wjson="1" |
||||
mjson="1" |
||||
elif [ "$json_opts" == "w" ]; then |
||||
echo "JSON used for world generation" |
||||
wjson="1" |
||||
mjson="0" |
||||
elif [ "$json_opts" == "m" ]; then |
||||
echo "JSON used for model generation" |
||||
mjson="1" |
||||
wjson="0" |
||||
else |
||||
wjson="0" |
||||
mjson="0" |
||||
fi |
||||
|
||||
if [ "$hitl" == "True" ] || [ "$hitl" == "1" ] || [ "$hitl" == "true" ]; then |
||||
hitl=true |
||||
else |
||||
hitl=false |
||||
fi |
||||
|
||||
export PX4_SIM_MODEL=${VEHICLE_MODEL:=iris} |
||||
|
||||
SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )" |
||||
src_path="$SCRIPT_DIR/.." |
||||
echo ${SCRIPT} |
||||
build_path=${src_path}/build/${target} |
||||
source ${src_path}/Tools/setup_gazebo.bash ${src_path} ${src_path}/build/${target} |
||||
sleep 1 |
||||
|
||||
sitl_path=${SCRIPT_DIR}/sitl_gazebo |
||||
world_name="--world_name ${world}" |
||||
jinja_world_script=${sitl_path}/scripts/jinja_world_gen.py |
||||
sitl_ode_threads="--ode_threads ${threads}" |
||||
world_json="--json_gen ${wjson}" |
||||
|
||||
if [ $hitl == true ]; then |
||||
hitl_model_name="--model_name ${PX4_SIM_MODEL}" |
||||
echo "HITL mode is currently turned on, disabling multiple vehicle spawn and script spawn." |
||||
echo "RUNNING: python3 $jinja_world_script $world_name $hitl_model_name $world_json" |
||||
python3 $jinja_world_script $world_name $hitl_model_name $world_json |
||||
echo "Generated temp_${world}.world" |
||||
spawn_model ${PX4_SIM_MODEL} 0 ${world} ${mjson} |
||||
|
||||
else |
||||
echo "killing running instances" |
||||
pkill -x px4 || true |
||||
echo "HITL mode is currently turned off." |
||||
echo "RUNNING: python3 $jinja_world_script $world_name $sitl_ode_threads $world_json" |
||||
python3 $jinja_world_script $world_name $sitl_ode_threads $world_json |
||||
echo "Generated temp_${world}.world" |
||||
echo "Starting gazebo: gzserver ${sitl_path}/worlds/temp_${world}.world --verbose" |
||||
gzserver ${sitl_path}/worlds/temp_${world}.world --verbose & |
||||
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 ${PX4_SIM_MODEL} $n ${world} ${mjson} |
||||
n=$(($n + 1)) |
||||
done |
||||
else |
||||
for target in ${SCRIPT}; do |
||||
target="$(echo "$target" | tr -d ' ')" #Remove spaces |
||||
target_vehicle="${target%:*}" |
||||
target_number="${target#*:}" |
||||
|
||||
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 |
||||
spawn_model ${PX4_SIM_MODEL} $n ${world} ${mjson} |
||||
m=$(($m + 1)) |
||||
n=$(($n + 1)) |
||||
done |
||||
done |
||||
fi |
||||
echo "Starting gazebo client" |
||||
gzclient |
||||
fi |
||||
Loading…
Reference in new issue