#!/usr/bin/env bash
set -euo pipefail

if [[ $# -ne 2 ]]; then
  echo "Usage: $0 log_prefix /path/to/output_perception_timing.log"
  exit 1
fi

log_prefix=$1
output_log_file=$2

# Set up the ros parameters.
logger/rcv_core_launch "${log_prefix}.tar.gz"
# Filter old localizer and PO2 messages. We need vision messages for online
# perception.
sideband_bag="${log_prefix}".bag
if [ -f "${sideband_bag}" ]; then
  filtered_sideband_bag="/tmp/filtered.bag"
  base/ros/bag_filter \
          --exclude /localizer6dof/pose,/driving/PerceptionObstacles2 \
          --input "${sideband_bag}" \
          --output "${filtered_sideband_bag}"
  sideband_bag="${filtered_sideband_bag}"
fi
# Start bag playback.
rosrun offline_log_and_playback raw_playback "${log_prefix}".{velodyne,applanix}.bag "${filtered_sideband_bag}" &
sleep 3
# Stuff needs to be run to run perception.
roslaunch rcv raw_playback.launch &
sleep 3
# Launch the online localizer
roslaunch rcv localizer_6dof.launch &
sleep 3
# Launch the online perception.
roslaunch rcv perception.launch extra_args:="--vmodule=timer=1 -alsologtostderr" &> "${output_log_file}" &
# Launch rviz.
roslaunch rcv rviz.launch &
# Wait for bag playback to finish and then kill all nodes.
wait %1
rosnode kill -a
