#!/usr/bin/env bash
# Avoid creating the long-lived Fast DDS participant before the robot network
# interfaces and the vendor ROS graph are available. A participant created
# while the configured interface whitelist is empty does not recover later.
set -eo pipefail

source /opt/ros/jazzy/setup.bash
if [[ -f /home/nvidia/xos/setup.bash ]]; then
  source /home/nvidia/xos/setup.bash
fi
if [[ -f /opt/robot_tele_server/install/setup.bash ]]; then
  source /opt/robot_tele_server/install/setup.bash
fi

required_topic="/hric/robot/rl_state"
wait_seconds="${TG3_ROS_READY_WAIT_SECONDS:-120}"
retry_seconds="${TG3_ROS_READY_RETRY_SECONDS:-2}"
deadline=$((SECONDS + wait_seconds))

while ((SECONDS < deadline)); do
  # Capture first: grep -q in a pipe can close stdout early and make ros2
  # report SIGPIPE under pipefail even though the topic was found.
  topic_list="$(
    timeout 8 ros2 topic list --no-daemon --spin-time 3 2>/dev/null || true
  )"
  if grep -Fxq "$required_topic" <<<"$topic_list"; then
    echo "ROS discovery ready: $required_topic"
    exit 0
  fi
  sleep "$retry_seconds"
done

echo "ROS discovery did not expose $required_topic within ${wait_seconds}s" >&2
exit 1