#!/bin/bash
# 检查到未定义的变量则停止执行并报错
set -u

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

BUILD_MAP_NAME=$1
SIMULATION_PATH="/home/cicv/work/pji_desktop/simulation"

# 发布自定义起点 - gazebo
command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish initialPose  1 1 0 0 0 0"
# 清除环境
pkill -f "goal_publish/initialPose"
sleep 1
echo 'bash -c '\""$command"\"
nohup bash -c "$command" >/dev/null 2>&1 &
sleep 5

# 发布自定义起点 - rviz
command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish initialPose_rviz  1 1 0 0 0 0"
# 清除环境
pkill -f "goal_publish/initialPose_rviz"
sleep 1
echo 'bash -c '\""$command"\"
nohup bash -c "$command" >/dev/null 2>&1 &
# 这里必须设置不少于5s的延迟时间，否则rviz中的起点设置会出现问题
sleep 5

# 动态加载障碍物
command="cd $SIMULATION_PATH/pji_work && source devel/setup.bash && rosrun test demo $SIMULATION_PATH/merge_obstacles_data/merged_obstacles_new_0517_1.csv; exec bash"
gnome-terminal --tab -e 'bash -c '\""$command"\"
sleep 0.5
echo 'bash -c '"$command"

# 发布自定义终点 - 1
command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish goal 3.26 4.65 0.00537 0 0 0"
# 清除环境
pkill -f "goal_publish/goal"
pkill -f "goal_publish/pub_node"
sleep 1
# 开始录包
nohup rosbag record /imu /odom /obstacle_detection /sys_info -o "$SIMULATION_PATH"/evaluation_bag/example_0821_1.bag  >/dev/null 2>&1 &
echo 'bash -c '\""$command"\"
nohup bash -c "$command" >/dev/null 2>&1 &
sleep 10
# 结束录包
pkill -f "rosbag record"

# 发布自定义终点 - 2
command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish goal 2.47 -1.17 0.00304 0 0 0"
# 清除环境
pkill -f "goal_publish/goal"
pkill -f "goal_publish/pub_node"
sleep 1
# 开始录包
nohup rosbag record /imu /odom /obstacle_detection /sys_info -o "$SIMULATION_PATH"/evaluation_bag/example_0821_2.bag  >/dev/null 2>&1 &
echo 'bash -c '\""$command"\"
nohup bash -c "$command" >/dev/null 2>&1 &
sleep 15
# 结束录包
pkill -f "rosbag record"

# 发布自定义终点 - 3
command="cd $SIMULATION_PATH/catkin_ws && catkin_make && source devel/setup.bash && rosrun goal_publish goal 8.51 3.32 0.00705 0 0 0"
# 清除环境
pkill -f "goal_publish/goal"
pkill -f "goal_publish/pub_node"
sleep 1
echo 'bash -c '\""$command"\"
# 开始录包
nohup rosbag record /imu /odom /obstacle_detection /sys_info -o "$SIMULATION_PATH"/evaluation_bag/example_0821_3.bag  >/dev/null 2>&1 &
nohup bash -c "$command" >/dev/null 2>&1 &
sleep 20
# 结束录包
pkill -f "rosbag record"

# 发布默认终点并录包
command="cd $SIMULATION_PATH/catkin_ws && source devel/setup.bash && rosrun goal_publish pub_node $SIMULATION_PATH/data/build_map_bag/$BUILD_MAP_NAME $SIMULATION_PATH/evaluation_bag/example_0821_4.bag; exec bash"
# 清除环境
pkill -f "goal_publish/goal" # 下发自定义/默认终点前必须先清除goal，不能只清除pubnode
pkill -f "goal_publish/pub_node"
sleep 1
echo 'bash -c '\""$command"\"
nohup bash -c "$command" >/dev/null 2>&1 &
sleep 10
# 结束录包
pkill -f "goal_publish/pub_node"