#!/bin/bash

source /opt/ros/$ROS_DISTRO/setup.bash
gz sim -v -r $WORLD_FILE_NAME &
while true
do
  topics=$(gz topic -l)
  echo $topics
  if [[ $topics == *"/world/$WORLD_NAME"* ]]
  then
    break
  fi
  sleep 1
done

if [[ -z $SIM_DRONE_COUNT ]]
then
  exit 0
fi

cd /ardupilot_gazebo/models/$DRONE_MODEL
for (( j=0; j<$SIM_DRONE_COUNT; j++ ));
do
    xacro -v fdm_port_in:=$(($FDM_PORT_IN + ($j * 10))) model.xacro.sdf -o model.sdf
    value=$(</ardupilot_gazebo/models/$DRONE_MODEL/model.sdf)
    #-string is better than using -file option. File is not up to date in the next iteration.
    ros2 run ros_gz_sim create -world $WORLD_NAME -string "$value" -name spiri-$j -x $j  -y 0 -z 0.195 ##Possible read from text file for formation of drones.
done
exec "$@"