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

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

IMAGE_NAME=$1
CONTAINER_NAME=$2
WORLD_NAME="map.world"
MAP_PGM_NAME="map.pgm"
MAP_YAML_NAME="map.yaml"
MAP_BAG_NAME="origin_map.bag"
SIMULATION_PATH="/home/cicv/work/pji_desktop/simulation"

# 容器依赖服务配置
## 激活conda环境
source activate simulation

#  /home/cicv/work/pji_desktop/simulation/data/build_map_bag/build_map.bag \
## 默认起点配置
python "$SIMULATION_PATH"/catkin_ws/src/goal_publish/script/send_initial_pos_0521.py \
  "$SIMULATION_PATH"/data/map_bag/"$MAP_BAG_NAME" \
  "$SIMULATION_PATH"/catkin_ws/src/SimulationEnvs/launch/env_node.launch \
  "$SIMULATION_PATH"/catkin_ws/src/Navigation/launch/amcl.launch

## 启动仿真环境
### 选择场景文件
#### 修改env_node.launch中的world_name参数
xmlstarlet ed -u "/launch/include/arg[@name='world_name']/@value" \
  -v '$(find SimulationEnvs)/worlds/'"$WORLD_NAME" "$SIMULATION_PATH"/catkin_ws/src/SimulationEnvs/launch/env_node.launch | sponge "$SIMULATION_PATH"/catkin_ws/src/SimulationEnvs/launch/env_node.launch # -v后面如果有$必须为单引号才会被当作字符串
### 选择地图
#### 修改map_server.launch中的map参数
xmlstarlet ed -u "/launch/arg[@name='map']/@default" \
  -v "$MAP_YAML_NAME" "$SIMULATION_PATH"/catkin_ws/src/Navigation/launch/map_server.launch | sponge "$SIMULATION_PATH"/catkin_ws/src/Navigation/launch/map_server.launch
#### 修改map.yaml中的pgm路径
MAP_PGM_PATH="$SIMULATION_PATH"/catkin_ws/src/Navigation/map/"$MAP_PGM_NAME"
#echo '.image = '\""$map_pgm_path"\"
yq e '.image = '\""$MAP_PGM_PATH"\" -i "$SIMULATION_PATH"/catkin_ws/src/Navigation/map/"$MAP_YAML_NAME"
### 启动仿真
cd "$SIMULATION_PATH"/catkin_ws
safe-rm -r build devel
catkin_make
source devel/setup.bash
#### 清除过时任务
pkill -f "map2gazebo"
pkill -f "gazebo"
pkill -f "rviz"
pkill -f "SimulationEnvs"
pkill -f "Navigation"
rosnode kill --all
sleep 1
nohup roslaunch SimulationEnvs env_node.launch > "$SIMULATION_PATH"/logs/env_node.launch.log 2>&1 &
sleep 1
disown
nohup roslaunch Navigation navigation.launch > "$SIMULATION_PATH"/logs/navigation.launch.log 2>&1 &
disown

# 检查容器是否存在
CONTAINER_ID=$(docker ps -aqf name=^"${CONTAINER_NAME}"$)
if [ -n "$CONTAINER_ID" ]; then
    echo "Container $CONTAINER_NAME already exists，removing..."
    docker rm -f "$CONTAINER_ID"
    echo "Container removed"
fi

# 开启容器
echo "Start container..."
## 创建容器
docker run -itd --name "$CONTAINER_NAME" --net=host -v "$SIMULATION_PATH"/data/pjirobot/data:/root/pjirobot/data  "$IMAGE_NAME" /bin/bash

## 检查容器是否已启动
CONTAINER_ID=$(docker ps -aqf name=^"${CONTAINER_NAME}"$)
if [ -n "$CONTAINER_ID" ]; then
    echo "Container $CONTAINER_NAME started"
fi

# 容器初始配置
## 容器内路径参数
MAP_SERVICE_PATH="/home/load_map_service"
PJI_NAV_PATH="/home/pji_nav"
PJI_ENGINE_PATH="/home/pji_engine"
## 启动加载地图服务
command="cd $MAP_SERVICE_PATH && source devel/setup.bash && nohup rosrun load_map load_map > load_map.log 2>&1 &"
docker exec "$CONTAINER_NAME" /bin/bash -c "$command"
sleep 0.5
## 启动导航节点
command="cd $PJI_NAV_PATH && source install/setup.bash && nohup roslaunch pji_navigation navigation.launch >/dev/null 2>&1 &"
docker exec "$CONTAINER_NAME" /bin/bash -c "$command"
sleep 0.5
## 启动engine节点
command="cd $PJI_ENGINE_PATH && source install/setup.bash && nohup roslaunch pji_launch pji_engine.launch >/dev/null 2>&1 &"
docker exec "$CONTAINER_NAME" /bin/bash -c "$command"
#sleep 0.5
echo "done"