fix(script): improve error handling and early termination on failure

Author: kluge7

tests/waypoint_navigation/simulator_test.sh

@@ -1,74 +1,148 @@
-#!/bin/bash
-set -e
+#!/usr/bin/env bash
+set -euo pipefail
+
+###########################
+# CONFIG & ENVIRONMENT
+###########################
+SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+WORKSPACE=~/ros2_ws
+LOG_DIR="${SCRIPT_DIR}/logs"
+mkdir -p "$LOG_DIR"
 
-# Load ROS 2 environment
 echo "Setting up ROS 2 environment..."
-. /opt/ros/humble/setup.sh
-. ~/ros2_ws/install/setup.bash
+# Temporarily disable -u to avoid unbound variable errors
+set +u
+source "/opt/ros/humble/setup.sh"
+source "${WORKSPACE}/install/setup.bash"
+set -u
+
 export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/usr/local/lib
 
-# Get the directory of this script dynamically
-SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" && pwd )"
+###########################
+# GLOBAL CLEANUP HANDLER
+###########################
+SIM_PID=""
+ORCA_PID=""
+CONTROLLER_PID=""
+FILTER_PID=""
+TAIL_PIDS=()
+
+cleanup() {
+  echo "Cleanup: stopping all background processes..."
+  # Kill each background process by PID, if it exists
+  for p in "$SIM_PID" "$ORCA_PID" "$CONTROLLER_PID" "$FILTER_PID"; do
+    if [[ -n "$p" ]] && kill -0 "$p" 2>/dev/null; then
+      echo "Killing process $p"
+      kill -TERM "$p" || true
+    fi
+  done
+
+  # Kill all tail processes
+  for tpid in "${TAIL_PIDS[@]}"; do
+    if [[ -n "$tpid" ]] && kill -0 "$tpid" 2>/dev/null; then
+      kill -9 "$tpid" || true
+    fi
+  done
 
-# Launch Stonefish Simulator
-setsid ros2 launch stonefish_sim simulation_nogpu.launch.py &
-SIM_PID=$!
+  # Make sure everything is truly dead
+  wait || true
+}
+trap cleanup EXIT
+
+###########################
+# HELPER: LAUNCH & MONITOR
+###########################
+launch_and_monitor() {
+  local logfile="$1"; shift
+  local launch_cmd=("$@")
+
+  "${launch_cmd[@]}" &> "$logfile" &
+  local child_pid=$!
+
+  tail -F "$logfile" | while read -r line; do
+    if [[ "$line" == *"[ERROR]"* ]]; then
+      echo "[FATAL] Detected error in ${logfile}:"
+      echo "$line"
+      kill -TERM "$child_pid" 2>/dev/null || true
+      exit 1
+    fi
+  done &
+  local tail_pid=$!
+
+  TAIL_PIDS+=("$tail_pid")
+  echo "$child_pid"
+}
+
+###########################
+# 1. LAUNCH SIMULATOR
+###########################
+echo "Launching Stonefish Simulator..."
+SIM_LOG="${LOG_DIR}/simulator.log"
+SIM_PID="$(launch_and_monitor "$SIM_LOG" ros2 launch stonefish_sim simulation_nogpu.launch.py)"
 echo "Launched simulator with PID: $SIM_PID"
 
 echo "Waiting for simulator to start..."
-timeout 30s bash -c '
-    while ! ros2 topic list | grep -q "/orca/odom"; do
-        sleep 1
-    done || true'
+timeout 30s bash -c 'while ! ros2 topic list | grep -q "/orca/odom"; do sleep 1; done'
 echo "Simulator started"
 
-# Wait for odometry data
 echo "Waiting for odom data..."
 timeout 10s ros2 topic echo /orca/odom --once
 echo "Got odom data"
 
-# Launch ORCA Simulation
-setsid ros2 launch stonefish_sim orca_sim.launch.py &
-ORCA_PID=$!
+###########################
+# 2. LAUNCH ORCA
+###########################
+echo "Launching ORCA simulator..."
+ORCA_LOG="${LOG_DIR}/orca.log"
+ORCA_PID="$(launch_and_monitor "$ORCA_LOG" ros2 launch stonefish_sim orca_sim.launch.py)"
 echo "Launched orca with PID: $ORCA_PID"
 
 echo "Waiting for sim interface to start..."
-timeout 30s bash -c 'until ros2 topic list | grep -q "/orca/pose"; do sleep 1; done'
+timeout 30s bash -c 'while ! ros2 topic list | grep -q "/orca/pose"; do sleep 1; done'
 echo "Simulator started"
 
-# Wait for pose data
 echo "Waiting for pose data..."
 timeout 10s ros2 topic echo /orca/pose --once
 echo "Got pose data"
 
-# Launch Controllers
-setsid ros2 launch dp_adapt_backs_controller dp_adapt_backs_controller.launch.py &
-CONTROLLER_PID=$!
+###########################
+# 3. LAUNCH CONTROLLERS
+###########################
+echo "Launching DP Adaptive Backstepping Controller..."
+CONTROLLER_LOG="${LOG_DIR}/dp_adapt.log"
+CONTROLLER_PID="$(launch_and_monitor "$CONTROLLER_LOG" ros2 launch dp_adapt_backs_controller dp_adapt_backs_controller.launch.py)"
 echo "Launched controller with PID: $CONTROLLER_PID"
 
-setsid ros2 launch reference_filter_dp reference_filter.launch.py &
-FILTER_PID=$!
+echo "Launching reference_filter..."
+FILTER_LOG="${LOG_DIR}/filter.log"
+FILTER_PID="$(launch_and_monitor "$FILTER_LOG" ros2 launch reference_filter_dp reference_filter.launch.py)"
 echo "Launched filter with PID: $FILTER_PID"
 
-# Set operation mode
+###########################
+# 4. PUBLISH MSGS TO START
+###########################
 echo "Turning off killswitch and setting operation mode to autonomous mode"
 ros2 topic pub /orca/killswitch std_msgs/msg/Bool "{data: false}" -1
 ros2 topic pub /orca/operation_mode std_msgs/msg/String "{data: 'autonomous mode'}" -1
 
-# Send waypoint goal
-echo "Sending goal"
+echo "Sending goal..."
 python3 "$SCRIPT_DIR/send_goal.py"
 
-# Check if goal reached
-echo "Checking if goal reached"
+###########################
+# 5. CHECK GOAL
+###########################
+echo "Checking if goal reached..."
 python3 "$SCRIPT_DIR/check_goal.py"
-
 if [ $? -ne 0 ]; then
-    echo "Test failed: Drone did not reach goal."
-    exit 1
+  echo "Test failed: Drone did not reach goal."
+  exit 1
 else
-    echo "Test passed: Drone reached goal."
+  echo "Test passed: Drone reached goal."
 fi
 
-# Terminate processes
-kill -TERM -"$SIM_PID" -"$ORCA_PID" -"$CONTROLLER_PID" -"$FILTER_PID"
+###########################
+# 6. CLEAN SHUTDOWN
+###########################
+echo "Terminating all processes..."
+cleanup
+echo "All done!"