#!/bin/bash
# 检查到未定义的变量则停止执行并报错
set -u
source /home/cicv/work/pji_desktop/config.sh

# 检查参数数量
if [ "$#" -ne 1 ]; then
  echo "Error: Incorrect number of arguments"
  echo "Usage: $0 MAP_BAG_PATH"
  exit 1
fi

MAP_BAG_PATH=$1
#SIMULATION_PATH="/home/cicv/work/pji_desktop/simulation"
MAP2GAZEBO_LOG_PATH="$SIMULATION_PATH/logs/run_map2gazebo.log"
GAZEBO_WORLD_LOG_PATH="$SIMULATION_PATH/logs/gazebo_world.log"
MAP_STL_PATH="$SIMULATION_PATH/catkin_map2gazebo/src/models/map/meshes/map.stl"
MAP2GAZEBO_START_FLAG="map2gazebo running"
GAZEBO_WORLD_END_FLAG="Exported STL"

# 激活conda环境
source activate simulation

# 清除环境
## kill ros节点
rosnode kill --all
## 删除历史日志
if [ -f "$MAP2GAZEBO_LOG_PATH" ]; then
  safe-rm "$MAP2GAZEBO_LOG_PATH"
fi
## 删除旧STL文件
if [ -f "$MAP_STL_PATH" ]; then
  safe-rm "$MAP_STL_PATH"
fi
sleep 0.5

# 执行命令
command="cd $SIMULATION_PATH/catkin_map2gazebo && safe-rm -r build devel && catkin_make && source devel/setup.bash && roslaunch --screen map2gazebo map2gazebo.launch"
nohup script -q -c bash -c "$command" > "$MAP2GAZEBO_LOG_PATH" 2>&1 &
disown

# 检查脚本运行状态
tail -F "$MAP2GAZEBO_LOG_PATH" | while read LINE; do
    # 检查脚本是否启动
    if echo "$LINE" | grep -q "$MAP2GAZEBO_START_FLAG"; then # 已启动
      echo "Service map2gazebo started."
      sleep 0.5
      # 播包 (含/map话题)
      rosbag play "$MAP_BAG_PATH"
      sleep 0.5
    fi

    # 检查STL文件是否生成
    if echo "$LINE" | grep -q "$GAZEBO_WORLD_END_FLAG"; then # 已生成
      # 清除环境
      ## kill上面启动的脚本
      pkill -f "map2gazebo"
      ## 关闭gazebo
      pkill -f "gazebo"
      echo "Service map2gazebo stopped."

      # 执行命令
      command="cd $SIMULATION_PATH/catkin_map2gazebo && safe-rm -r build devel && catkin_make && source devel/setup.bash && roslaunch --screen map2gazebo gazebo_world.launch"
      nohup script -q -c bash -c "$command" > "$GAZEBO_WORLD_LOG_PATH" 2>&1 &
      disown
    fi
done