#!/bin/bash
#########################################################################
#   Author: Zhang Shuai                                                 #
#   Date:   2019-05-23                                                  #
#   Description:    launch a MPI Gazebo simulation task                 #
#   Usage:  ./MPI_Gazebo_launch.sh [hostfile] [world's path and name]   #
#########################################################################

host_file=$1
world_file=$2

host_count=0
hosts=()

#获取hostfile文件中的host名字
while read line
do
    if [ -n "$line" ]
    then
        temp=${line% *}
        hosts[$host_count]=$temp
        let host_count=$host_count+1
    fi
done < $host_file

echo "host_count:$host_count"

for((i=0;i<host_count;i++))
do
    echo "host_name:${hosts[$i]}"
done

#登入各节点启动roscore    （source还有改进空间，ROS运行的工作目录应该作为参数传进来）
for((i=0;i<host_count;i++))
do
    ssh -f -n ${hosts[$i]} "source ~/catkin_ws/devel/setup.bash;
    roscore;
    echo roscore_done!"
    echo "${hosts[$i]}:roscore_good"
done

sleep 5s

#登入第一个节点启动MPI的gzserver，并成为各自节点上的ROS node
ssh -f -n ${hosts[0]} "cd ~/MPI;
mpiexec -hostfile hostfile -np 1 mygzserver $world_file : -np 1 mygzserver $world_file;
echo mpiexec_done!"
echo "${hosts[0]}:mpiexec_good"

sleep 20s

#登入各节点roslaunch加载模型（source还有改进空间，ROS运行的工作目录应该作为参数传进来）
for((i=0;i<host_count;i++))
do
    ssh -f -n ${hosts[$i]} "source ~/catkin_ws/devel/setup.bash;
    roslaunch hector_quadrotor_gazebo hector_2_single_gazebo_mpi_spawn_test.launch;
    echo roslaunch_done!"
    echo "${hosts[$i]}:roslaunch_good"
done

sleep 20s

#登入各节点开始仿真
for((i=0;i<host_count;i++))
do
    ssh -f -n ${hosts[$i]} "gz world -p 0;
    echo start_done!;
    exit"
    echo "${hosts[$i]}:start_good"
done

sleep 5m

#登入各节点kill相关进程以结束仿真（1.目前方式比较暴力，后续改进；2.cd ~/MPI 还要进行灵活性优化）
for((i=0;i<host_count;i++))
do
    ssh -f -n ${hosts[$i]} > /dev/null 2>&1 "cd ~/MPI;
    ./kill.sh;
    ./kill.sh;
    ./kill.sh;
    ./kill.sh;
    ./kill.sh;
    ./kill.sh;
    ./kill.sh;
    ./kill.sh;
    ./kill.sh;
    exit"
    echo "${hosts[$i]}:close_good"
done

echo simulation_done!

exit 0