#!/bin/bash
# watch_dog.sh

TIMEOUT=10  # 设定超时时间为10秒
NAMESPACE="" # 命名空间 例如 "/infantry_3" 注意要有"/"；留空则自动从 rm_bringup/config/launch_params.yaml 读取
CORE_NODE_NAMES=("armor_detector" "armor_solver" "serial_driver")  # 固定监控节点
CAMERA_NODE_NAME="hik_camera"  # 相机节点默认名（视频模式会改为 video_player）
NODE_NAMES=()
USER="$(whoami)" #用户名
HOME_DIR=$(eval echo ~$USER)
WORKING_DIR="$HOME_DIR/yq_2026" # 代码目录
LAUNCH_FILE="rm_bringup bringup.launch.py" # launch 文件
OUTPUT_FILE="$WORKING_DIR/screen.output" # 终端输出记录文件
PID_FILE="$WORKING_DIR/.rm_bringup.pid" # 记录由看门狗启动的 bringup 主进程 PID

rmw="rmw_fastrtps_cpp" #RMW
export RMW_IMPLEMENTATION="$rmw" # RMW实现

# ROS Domain ID (默认 10，可通过环境变量覆盖)
export ROS_DOMAIN_ID=${ROS_DOMAIN_ID:=10}

export ROS_HOSTNAME=$(hostname)
export ROS_HOME=${ROS_HOME:=$HOME_DIR/.ros}
export ROS_LOG_DIR="/tmp"

source /opt/ros/humble/setup.bash
source $WORKING_DIR/install/setup.bash

# 如果未手动指定 NAMESPACE，则从 bringup 的 launch_params.yaml 自动读取
if [[ -z "$NAMESPACE" ]]; then
  launch_params_yaml="$WORKING_DIR/src/rm_bringup/config/launch_params.yaml"
  if [[ -f "$launch_params_yaml" ]]; then
    readarray -t _launch_vals < <(python3 - "$launch_params_yaml" << 'PY'
import sys
import yaml

path = sys.argv[1]
with open(path, "r", encoding="utf-8") as file:
    data = yaml.safe_load(file) or {}

namespace = data.get("namespace", "")
if namespace is None:
    namespace = ""
video_play = bool(data.get("video_play", False))
print(str(namespace).strip())
print("true" if video_play else "false")
PY
)
    NAMESPACE="${_launch_vals[0]}"
    if [[ "${_launch_vals[1]}" == "true" ]]; then
      CAMERA_NODE_NAME="video_player"
    fi
  fi
fi

NODE_NAMES=("${CORE_NODE_NAMES[@]}" "$CAMERA_NODE_NAME")

# 规范化命名空间
NAMESPACE=$(echo "$NAMESPACE" | tr -d "\r" | xargs)
NAMESPACE="${NAMESPACE%/}"
if [[ -n "$NAMESPACE" ]] && [[ "${NAMESPACE:0:1}" != "/" ]]; then
  NAMESPACE="/$NAMESPACE"
fi
if [[ "$NAMESPACE" == "/" ]]; then
  NAMESPACE=""
fi

rmw_config=""
if [[ "$rmw" == "rmw_fastrtps_cpp" ]]
then
  if [[ ! -z $rmw_config ]]
  then
    export FASTRTPS_DEFAULT_PROFILES_FILE=$rmw_config
  fi
elif [[ "$rmw" == "rmw_cyclonedds_cpp" ]]
then
  if [[ ! -z $rmw_config ]]
  then
    export CYCLONEDDS_URI=$rmw_config
  fi
fi

function bringup() {
    source /opt/ros/humble/setup.bash
    source $WORKING_DIR/install/setup.bash
    nohup ros2 launch $LAUNCH_FILE > "$OUTPUT_FILE" 2>&1 &
  launch_pid=$!
  echo "$launch_pid" > "$PID_FILE"
}

function kill_process_tree() {
  local pid="$1"
  local child

  for child in $(pgrep -P "$pid" 2>/dev/null); do
    kill_process_tree "$child"
  done

  kill -TERM "$pid" 2>/dev/null || true
}

function stop_bringup() {
  local launch_pid=""

  if [[ -f "$PID_FILE" ]]; then
    launch_pid=$(cat "$PID_FILE")
  fi

  if [[ -n "$launch_pid" ]] && kill -0 "$launch_pid" 2>/dev/null; then
    kill_process_tree "$launch_pid"
    sleep 2
    kill -KILL "$launch_pid" 2>/dev/null || true
  fi

  rm -f "$PID_FILE"
}

function restart() {
  stop_bringup
    ros2 daemon stop
    ros2 daemon start
    bringup
}

bringup
sleep $TIMEOUT
sleep $TIMEOUT

# 监控每个节点的心跳
while true; do
    for node in "${NODE_NAMES[@]}"; do
    if [[ -n "$NAMESPACE" ]]; then
      topic="$NAMESPACE/$node/heartbeat"
    else
      topic="/$node/heartbeat"
    fi
        echo "- Check $node"
    if ros2 topic list 2>/dev/null | grep -q -F "$topic" 2>/dev/null; then
      data_value=$(timeout 10 ros2 topic echo "$topic" --once | grep -o "data: [0-9]*" | awk '{print $2}' 2>/dev/null)
            if [ ! -z "$data_value" ]; then
                echo "    $node is OK! Heartbeat Count: $data_value"
            else
                echo "    Heartbeat lost for $topic, restarting all nodes..."
                restart
                break 
            
            fi
        else
            echo "    Heartbeat topic $topic does not exist, restarting all nodes..."
            restart
            break
        fi
    done
    sleep $TIMEOUT
done

