gazebo_sitl_multiple_run.sh
3.54 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
#!/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/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" "iris_rtps" "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/instance_$n"
[ ! -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_${MODEL}_${N} -s etc/init.d-posix/rcS >out.log 2>err.log &
python3 ${src_path}/Tools/sitl_gazebo/scripts/jinja_gen.py ${src_path}/Tools/sitl_gazebo/models/${MODEL}/${MODEL}.sdf.jinja ${src_path}/Tools/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.0
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}${LABEL}
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/setup_gazebo.bash ${src_path} ${src_path}/build/${target}
echo "Starting gazebo"
gzserver ${src_path}/Tools/sitl_gazebo/worlds/${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 ${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
spawn_model ${target_vehicle} $n $target_x $target_y
m=$(($m + 1))
n=$(($n + 1))
done
done
fi
trap "cleanup" SIGINT SIGTERM EXIT
echo "Starting gazebo client"
gzclient