You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
132 lines
3.5 KiB
132 lines
3.5 KiB
#!/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
|
|
|