#!/bin/bash
# This script launches the necessary ROS components to streamline the monitor
# testing process. Note that the user will need to manually run the chum_play
# command, 
# ie. //data/chum/tools:chum_play --uri "chum://20190620T170217-kitt_03?t=/driving/mofo/imu,/driving/mofo/gps_primary,/driving/mofo/gps_secondary,/vehicle/measurements,/radar_driver/radar_cluster_proto" --publish_clock


USAGE="ros_monitor_tester <run_id>"

TMP_CONFIG_DIR="/tmp/current_config"
CONFIG_DIR="/etc/zoox/current_config"

if [ $# -ne 1 ]; then
  echo "$USAGE"
  exit 1;
fi

### Setup ###
# Create array to store PIDs of launched processes
active_pids=()
keep_running=true

# Kill all processes on SIGINT
finish() {
  # Loop through PIDs in reverse order
  echo "Caught SIGTERM signal!"
  for (( idx=${#active_pids[@]}-1 ; idx >= 0 ; idx-- )) ; do
    term_pid="${active_pids[idx]}"
    echo "Terminating" "$term_pid"
    kill -INT "$term_pid"
  done
  keep_running=false
}
trap finish SIGINT 

### Configuration ###
bazel-run.sh //bin:get_hardware_config -- --output_dir "$TMP_CONFIG_DIR" --run_id "$1"
sudo mkdir -p "$CONFIG_DIR"
sudo cp "$TMP_CONFIG_DIR"/"$1"/* "$CONFIG_DIR"

### Launch ROS jobs ###
robot_name="$(echo $1 | cut -d - -f2)"

ZOOX_ROBOT_NAME="$robot_name" bazel-run.sh //bin:roscore & active_pids+=("$!")

# Set use_sim_time and confirm
bazel-run.sh //bin:rosparam -- set /use_sim_time true

# Run confirmation loop 
ret_val="$(bazel-run.sh //bin:rosparam -- get /use_sim_time)"
ret_code="$?"
until [ "$ret_code" -eq 0 ] && [ "$ret_val" ]
do
  sleep 1

  if ! "$keep_running" ; then
    exit 1;
  fi

  # Occasionally, need to re-trigger rosparam set
  bazel-run.sh //bin:rosparam -- set /use_sim_time true
  ret_val="$(bazel-run.sh //bin:rosparam -- get /use_sim_time)"
  ret_code="$?"
done
echo "use_sim_time set to true!"

# Launch rest of ROS jobs
bazel-run.sh //third_party/ros/diagnostics/diagnostic_aggregator:aggregator_node & active_pids+=("$!")
ZOOX_ROBOT_NAME="$robot_name" bazel-run.sh //vehicle/launch:vehicle_params_launch
ZOOX_ROBOT_NAME="$robot_name" bazel-run.sh //vehicle/launch:pose_locomofo_launch & active_pids+=("$!")

# Launch tools
bazel-run.sh //bin:rqt_robot_monitor & active_pids+=("$!")

### Wait ###
# Wait for execution to terminate
while "$keep_running"
do
  sleep 1
done
