#!/bin/bash
###############################################################################
# Planner Node Restart Obsbag Test
###############################################################################
# Overview
# Below is a listing of steps to be performed to test how long it takes to
# restart the planner_node in the best case scenario.
###############################################################################
trap "trap - SIGTERM && kill -- -$$" SIGINT SIGTERM EXIT
# 1) Start simulation
#bazel run //sim/obstacle_bag:obsbag2 run chum://20200228T224457-kitt_05 start_absolute:=1582931342.0 offline:=true &
#${ZOOX_WORKSPACE_ROOT}/sim/sdl/test_sdl.sh //sim/sim_tests/simple:straight
#sim/sdl/ros_visualize_sdl.sh //sim/sdl/test_scenarios:ov_pedestrian --simulator_args='rviz:=false sim_args:="--params-kv sim/metrics=true"' &
sim/sdl/ros_visualize_sdl.sh //sim/sdl/test_scenarios:self_healing_planner --simulator_args='rviz:=false sim_args:="--params-kv sim/metrics=true"' &
#${ZOOX_WORKSPACE_ROOT}/sim/sdl/ros_visualize_sdl.sh //sim/sdl/test_scenarios:self_healing_planner --simulator_args="rviz:=false" &
# 2) Check kAutonomousManualReady in stepsvehicle/common/proto/vehicle_platform_measurements.proto
sleep 120
bazel run //experimental/pcrandell:simple_ros_listener -- /planner/decision/reproducibility 120 readiness 2
if [$? -ne 0]
then
  echo 'Test FAILED -- Vehicle did not enter kAutonomous on vehicle initialization'
  exit 1
fi
# 3) Kill planner
TEMP=$(ps -eo args | grep planner_node | grep bazel)
killall -11 planner_node_private
# 4) Restart planner
$TEMP &
# 5) Record timestamp for planner restart begin
BEGIN=`date +%s`
# 5) Record timestamp for planner restart begin
# 6) Wait for up to 60 seconds kAutonomousManualReady in stepsvehicle/common/proto/vehicle_platform_measurements.proto
bazel run //experimental/pcrandell:simple_ros_listener -- /planner/decision/reproducibility 30 readiness 2
if [$? -ne 0]
then
  echo 'Test FAILED -- Vehicle did not transition back to kAutonomous after planner restart'
  exit 3
fi
# 7) Record timestamp for planner restart end
END=`date +%s`
# 8) Verify end-begin <= 30 seconds
echo "Planner took `expr $END - $BEGIN` seconds ##########################"
sleep 200
