#!/bin/bash
set -eo pipefail

source /opt/ros/humble/setup.bash
source /workspaces/vlm_office/install/setup.bash

export TURTLEBOT3_MODEL="${TURTLEBOT3_MODEL:-burger}"
export GZ_SIM_RESOURCE_PATH="/workspaces/vlm_office/src/turtlebot3_simulations/turtlebot3_gazebo/models/turtlebot3_office:/workspaces/vlm_office/src/turtlebot3_simulations/turtlebot3_gazebo/models${GZ_SIM_RESOURCE_PATH:+:$GZ_SIM_RESOURCE_PATH}"

HEADLESS="${HEADLESS:-false}"
ENABLE_RVIZ="${ENABLE_RVIZ:-true}"

PIDS=()

cleanup() {
  for pid in "${PIDS[@]:-}"; do
    kill "$pid" 2>/dev/null || true
  done
  wait || true
}

trap cleanup EXIT INT TERM

# Gazebo Sim 8 + TurtleBot3 + ros_gz_bridge (same ROS topics as Classic: /scan /odom /cmd_vel /tf /clock)
ros2 launch turtlebot3_gazebo turtlebot3_office_gz.launch.py \
  robot_model:="$TURTLEBOT3_MODEL" \
  x_pose:=0.0 y_pose:=0.0 use_sim_time:=true headless:="$HEADLESS" \
  show_rviz:=false start_slam:=false &
PIDS+=($!)

sleep 14

ros2 launch nav2_bringup navigation_launch.py use_sim_time:=True &
PIDS+=($!)

sleep 10

ros2 launch slam_toolbox online_async_launch.py use_sim_time:=True &
PIDS+=($!)

sleep 10

if [ "$ENABLE_RVIZ" = "true" ]; then
  ros2 run rviz2 rviz2 -d /opt/ros/humble/share/nav2_bringup/rviz/nav2_default_view.rviz &
  PIDS+=($!)
  sleep 3
fi

ros2 launch explore_lite explore.launch.py use_sim_time:=true &
PIDS+=($!)

wait