bf

2026-03-25 12:37:08 +08:00
commit 5e1e355ffa
462 changed files with 92376 additions and 0 deletions
--- a/src/rm_upstart/README.md
+++ b/src/rm_upstart/README.md
@@ -0,0 +1,9 @@
+# rm_upstart
+
+看门狗程序及自启动服务
+
+## 看门狗
+
+每个节点都会以1s一次的频率发布heartbeat话题，看门狗程序(rm_watch_dog.sh)通过ros2 topic echo监听这些话题，如果有一个节点在TIMEOUT(10s)内都没有更新心跳数据，看门狗程序会重启它自己拉起的 bringup 进程树。
+
+不会再使用全局 `pkill -f ros`，因此不会误杀在其它终端手动启动的 ROS2 进程（例如 `foxglove_bridge`）。
--- a/src/rm_upstart/register_service.sh
+++ b/src/rm_upstart/register_service.sh
@@ -0,0 +1,57 @@
+#!/bin/bash
+
+# 获取当前用户的用户名和家目录
+USER_NAME=${SUDO_USER:-$(whoami)}
+HOME_DIR=$(eval echo ~$USER_NAME)
+SERVICE_NAME="rm_1229.service"
+SERVICE_PATH="/etc/systemd/system/$SERVICE_NAME"
+
+# 检查是否为 root 用户，因为修改 systemd 配置需要 root 权限
+if [ "$(id -u)" -ne 0 ]; then
+    echo "This script must be run as root."
+    exit 1
+fi
+
+# 复制脚本到目标路径
+chmod +x ./rm_clean_up.sh
+chmod +x ./rm_watch_dog.sh
+cp ./rm_clean_up.sh /usr/sbin/
+cp ./rm_watch_dog.sh /usr/sbin/
+
+# 动态创建 systemd service 文件内容
+read -r -d '' SERVICE_CONTENT << EOF
+[Unit]
+Description=YQ 2026 Vision Project
+After=network.target
+
+[Service]
+User=$USER_NAME
+Type=simple
+Environment=ROS_DOMAIN_ID=10
+ExecStart=/usr/sbin/rm_watch_dog.sh
+ExecStop=/usr/sbin/rm_clean_up.sh
+Restart=always
+TimeoutStopSec=10
+RestartSec=10s
+
+[Install]
+WantedBy=multi-user.target
+EOF
+
+# 创建/覆写 systemd service 文件
+echo "Creating systemd service file at $SERVICE_PATH..."
+echo "$SERVICE_CONTENT" > "$SERVICE_PATH"
+
+# 重新加载 systemd 配置
+echo "Reloading systemd daemon..."
+systemctl daemon-reload
+
+# 启用服务
+echo "Enabling service $SERVICE_NAME..."
+systemctl enable "$SERVICE_NAME"
+
+# 启动服务
+echo "Starting service $SERVICE_NAME..."
+systemctl start "$SERVICE_NAME"
+
+echo "Service $SERVICE_NAME has been registered and started."
--- a/src/rm_upstart/rm_clean_up.sh
+++ b/src/rm_upstart/rm_clean_up.sh
@@ -0,0 +1,34 @@
+#!/bin/bash
+
+export ROS_HOSTNAME=$(hostname)
+export ROS_HOME=${ROS_HOME:=$HOME_DIR/.ros}
+export ROS_LOG_DIR="/tmp"
+source /opt/ros/humble/setup.bash
+pkill rm_watch_dog.sh # 杀掉看门狗
+
+PID_FILE="$HOME/yq_2026/.rm_bringup.pid"
+
+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
+}
+
+if [[ -f "$PID_FILE" ]]; then
+	launch_pid=$(cat "$PID_FILE")
+	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"
+fi
+
+ros2 daemon stop
+ros2 daemon start
+sleep 1
--- a/src/rm_upstart/rm_watch_dog.sh
+++ b/src/rm_upstart/rm_watch_dog.sh
@@ -0,0 +1,156 @@
+#!/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
+