#!/bin/bash
###################################################################################################################################################
#   Author: Zhang Shuai                                                                                                                           #
#   Date:   2019-05-23                                                                                                                            #
#   Description:    launch a MPI Gazebo simulation task                                                                                           #
#   Usage:  ./MPI_Gazebo_launch.sh [ros packdge's name] [launch file's name] [workspace's path] [world's path and name] [hostfile] [shell's path] #
#   Example:    ./MPI_Gazebo_launch.sh rosplane_sim fixedwing_text_three_100_withoutfast_mpi.launch ~/zhangshuai/DA                               #
#               ~/zhangshuai/DA/src/da/rosplane/rosplane_sim/worlds/flying_mpi_100.world hostfile ~/Git/Gazebo_Distributed_MPI/mpi_run            #
###################################################################################################################################################

packdge_name=$1
launch_file_name=$2
workspace_path=$3
world_file=$4
host_file=$5
shell_path=$6

echo $packdge_name
echo $launch_file_name
echo $workspace_path
echo $world_file
echo $host_file
echo $shell_path

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
for((i=0;i<host_count;i++))
do
    ssh -f -n ${hosts[$i]} "source $workspace_path/devel/setup.bash;
    export ROS_HOSTNAME=localhost;
    roscore;
    echo roscore_done!;
    rosparam set /use_sim_time true;
    rosparam get /use_sim_time"
    echo "${hosts[$i]}:roscore_good"
done

sleep 5s

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

sleep 10s

#登入各节点roslaunch加载模型
for((i=0;i<host_count;i++))
do
    ssh -f -n ${hosts[$i]} "source $workspace_path/devel/setup.bash;
    echo ++++++++++${hosts[$i]}++++++++++++;
    env | grep LD_LIBRARY;
    echo ++++++++++${hosts[$i]}++++++++++++;
    roslaunch $packdge_name $launch_file_name;
    echo roslaunch_done!"
    echo "${hosts[$i]}:roslaunch_good"
done

sleep 100s

#登入各节点开始仿真
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.目前方式比较暴力，后续改进。）
for((i=0;i<host_count;i++))
do
    ssh -f -n ${hosts[$i]} > /dev/null 2>&1 "cd $shell_path;
    ./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