Add hivecore_robot_system submodule

.gitignore

@@ -21,4 +21,8 @@ camera_pose_data.json
 eye_to_hand_matrix.json
 hand_pose_data.json
 
-.vscode/
+.vscode/
+logs/
+voice_model/
+config/
+data/

.gitmodules

@@ -1,24 +1,30 @@
 [submodule "hivecore_robot_arm"]
 	path = hivecore_robot_arm
-	url = ssh://git@192.168.10.50:2222/HiveCoreRD/hivecore_robot_arm.git
+	url = ssh://git@47.110.133.3:2222/HiveCoreRD/hivecore_robot_arm.git
 [submodule "hivecore_robot_brain"]
 	path = hivecore_robot_brain
-	url = ssh://git@192.168.10.50:2222/HiveCoreRD/hivecore_robot_brain.git
+	url = ssh://git@47.110.133.3:2222/HiveCoreRD/hivecore_robot_brain.git
 [submodule "hivecore_robot_voice"]
 	path = hivecore_robot_voice
-	url = ssh://git@192.168.10.50:2222/HiveCoreRD/hivecore_robot_voice.git
+	url = ssh://git@47.110.133.3:2222/HiveCoreRD/hivecore_robot_voice.git
 [submodule "hivecore_robot_slam"]
 	path = hivecore_robot_slam
-	url = ssh://git@192.168.10.50:2222/HiveCoreRD/hivecore_robot_slam.git
+	url = ssh://git@47.110.133.3:2222/HiveCoreRD/hivecore_robot_slam.git
 [submodule "hivecore_robot_motion"]
 	path = hivecore_robot_motion
-	url = ssh://git@192.168.10.50:2222/HiveCoreRD/hivecore_robot_motion.git
+	url = ssh://git@47.110.133.3:2222/HiveCoreRD/hivecore_robot_motion.git
 [submodule "hivecore_robot_vision"]
 	path = hivecore_robot_vision
-	url = ssh://git@192.168.10.50:2222/HiveCoreRD/hivecore_robot_vision.git
+	url = ssh://git@47.110.133.3:2222/HiveCoreRD/hivecore_robot_vision.git
 [submodule "hivecore_robot_drivers"]
 	path = hivecore_robot_drivers
-	url = ssh://git@192.168.10.50:2222/HiveCoreRD/hivecore_robot_drivers.git
+	url = ssh://git@47.110.133.3:2222/HiveCoreRD/hivecore_robot_drivers.git
 [submodule "hivecore_robot_interfaces"]
 	path = hivecore_robot_interfaces
-	url = ssh://git@192.168.10.50:2222/HiveCoreRD/hivecore_robot_interfaces.git
+	url = ssh://git@47.110.133.3:2222/HiveCoreRD/hivecore_robot_interfaces.git
+[submodule "hivecore_robot_ctrlgui"]
+	path = hivecore_robot_ctrlgui
+	url = ssh://git@47.110.133.3:2222/HiveCoreRD/hivecore_robot_ctrl_gui.git
+[submodule "hivecore_robot_system"]
+	path = hivecore_robot_system
+	url = ssh://git@47.110.133.3:2222/HiveCoreRD/hivecore_robot_system.git

.vscode/settings.json

@@ -1,145 +0,0 @@
-{
-  "files.associations": {
-    "*.launch": "python",
-    "*.world": "xml",
-    "*.xacro": "xml",
-    "ostream": "cpp",
-    "cctype": "cpp",
-    "clocale": "cpp",
-    "cmath": "cpp",
-    "csignal": "cpp",
-    "cstdarg": "cpp",
-    "cstddef": "cpp",
-    "cstdio": "cpp",
-    "cstdlib": "cpp",
-    "cstring": "cpp",
-    "ctime": "cpp",
-    "cwchar": "cpp",
-    "cwctype": "cpp",
-    "any": "cpp",
-    "array": "cpp",
-    "atomic": "cpp",
-    "bit": "cpp",
-    "bitset": "cpp",
-    "chrono": "cpp",
-    "codecvt": "cpp",
-    "compare": "cpp",
-    "complex": "cpp",
-    "concepts": "cpp",
-    "condition_variable": "cpp",
-    "coroutine": "cpp",
-    "cstdint": "cpp",
-    "deque": "cpp",
-    "forward_list": "cpp",
-    "list": "cpp",
-    "map": "cpp",
-    "set": "cpp",
-    "string": "cpp",
-    "unordered_map": "cpp",
-    "unordered_set": "cpp",
-    "vector": "cpp",
-    "exception": "cpp",
-    "algorithm": "cpp",
-    "functional": "cpp",
-    "iterator": "cpp",
-    "memory": "cpp",
-    "memory_resource": "cpp",
-    "numeric": "cpp",
-    "optional": "cpp",
-    "random": "cpp",
-    "ratio": "cpp",
-    "regex": "cpp",
-    "source_location": "cpp",
-    "string_view": "cpp",
-    "system_error": "cpp",
-    "tuple": "cpp",
-    "type_traits": "cpp",
-    "utility": "cpp",
-    "slist": "cpp",
-    "fstream": "cpp",
-    "future": "cpp",
-    "initializer_list": "cpp",
-    "iomanip": "cpp",
-    "iosfwd": "cpp",
-    "iostream": "cpp",
-    "istream": "cpp",
-    "limits": "cpp",
-    "mutex": "cpp",
-    "new": "cpp",
-    "numbers": "cpp",
-    "semaphore": "cpp",
-    "sstream": "cpp",
-    "stdexcept": "cpp",
-    "stop_token": "cpp",
-    "streambuf": "cpp",
-    "thread": "cpp",
-    "cfenv": "cpp",
-    "cinttypes": "cpp",
-    "typeindex": "cpp",
-    "typeinfo": "cpp",
-    "valarray": "cpp",
-    "variant": "cpp",
-    "core": "cpp",
-    "charconv": "cpp",
-    "ranges": "cpp",
-    "shared_mutex": "cpp"
-  },
-  "C_Cpp_Runner.cCompilerPath": "gcc",
-  "C_Cpp_Runner.cppCompilerPath": "g++",
-  "C_Cpp_Runner.debuggerPath": "gdb",
-  "C_Cpp_Runner.cStandard": "",
-  "C_Cpp_Runner.cppStandard": "",
-  "C_Cpp_Runner.msvcBatchPath": "",
-  "C_Cpp_Runner.useMsvc": false,
-  "C_Cpp_Runner.warnings": [
-    "-Wall",
-    "-Wextra",
-    "-Wpedantic",
-    "-Wshadow",
-    "-Wformat=2",
-    "-Wcast-align",
-    "-Wconversion",
-    "-Wsign-conversion",
-    "-Wnull-dereference"
-  ],
-  "C_Cpp_Runner.msvcWarnings": [
-    "/W4",
-    "/permissive-",
-    "/w14242",
-    "/w14287",
-    "/w14296",
-    "/w14311",
-    "/w14826",
-    "/w44062",
-    "/w44242",
-    "/w14905",
-    "/w14906",
-    "/w14263",
-    "/w44265",
-    "/w14928"
-  ],
-  "C_Cpp_Runner.enableWarnings": true,
-  "C_Cpp_Runner.warningsAsError": false,
-  "C_Cpp_Runner.compilerArgs": [],
-  "C_Cpp_Runner.linkerArgs": [],
-  "C_Cpp_Runner.includePaths": [],
-  "C_Cpp_Runner.includeSearch": [
-    "*",
-    "**/*"
-  ],
-  "C_Cpp_Runner.excludeSearch": [
-    "**/build",
-    "**/build/**",
-    "**/.*",
-    "**/.*/**",
-    "**/.vscode",
-    "**/.vscode/**"
-  ],
-  "C_Cpp_Runner.useAddressSanitizer": false,
-  "C_Cpp_Runner.useUndefinedSanitizer": false,
-  "C_Cpp_Runner.useLeakSanitizer": false,
-  "C_Cpp_Runner.showCompilationTime": false,
-  "C_Cpp_Runner.useLinkTimeOptimization": false,
-  "C_Cpp_Runner.msvcSecureNoWarnings": false,
-  "cmake.sourceDirectory": "/home/demo/hivecore_robot_os1/hivecore_robot_arm/arm_keyboard_input"
-}

README.md

@@ -1 +1,120 @@
-# hivecore_robot_os1
+# 1. hivecore_robot_os1
+
+## 使用教程 
+### 编译
+```bash
+cd /home/demo/hivecore_robot_os1
+# 编译指定的包
+colcon build --packages-select <pkg_name>
+```
+### 运行
+
+1. 启动节点
+	```bash
+	# 启动 launch.conf 中配置的所有节点
+	./launch_robot.sh all
+
+	# 仅启动特定节点（支持多个，名称对应 launch.conf 中的 pkg 或 target）
+	./launch_robot.sh <node_name1> <node_name2> ...
+	# 例如: ./launch_robot.sh robot_control
+	```
+2. 控制终端
+   	```
+	打开浏览器，输入 http://192.168.0.39:8080/ 启动web服务，ip地址为运行设备的ip地址
+	```
+
+3. 杀掉节点
+   	```bash
+	# 杀掉所有相关节点进程
+   	./kill_robot.sh all
+
+	# 仅杀掉特定节点进程
+	./kill_robot.sh <node_name1> <node_name2> ...
+	# 例如: ./kill_robot.sh vision_detect
+	```
+4. 查看日志
+   	```
+	如果通过launch_robot.sh启动节点，日志存放在 /home/demo/hivecore_robot_os1/logs目录
+	如果命令行运行节点，日志存放在 /home/demo/.ros/log目录
+	```
+
+# 2. hivecore_robot_vision
+
+#### 安装教程
+
+1.  Gemini SDK:
+
+	> 安装 deb 依赖项:
+	```	
+	# ssume you have sourced ROS environment, same blow
+	sudo apt install libgflags-dev nlohmann-json3-dev  \
+	ros-$ROS_DISTRO-image-transport ros-${ROS_DISTRO}-image-transport-plugins ros-${ROS_DISTRO}-compressed-image-transport \
+	ros-$ROS_DISTRO-image-publisher ros-$ROS_DISTRO-camera-info-manager \
+	ros-$ROS_DISTRO-diagnostic-updater ros-$ROS_DISTRO-diagnostic-msgs ros-$ROS_DISTRO-statistics-msgs \
+	ros-$ROS_DISTRO-backward-ros libdw-dev
+	```
+	> 安装 udev 规则
+	```
+	cd  ~/workspace/src/orbbec_camera/scripts
+	sudo bash install_udev_rules.sh
+	sudo udevadm control --reload-rules && sudo udevadm trigger
+	```
+
+2.  RealSense SDK:
+
+	```
+	sudo apt install ros-humble-librealsense2*
+	```
+	
+3.  Dependency:
+
+	```
+	"idge=3.2.1"
+	"numpy>=1.23.0, <2.0.0",
+	"matplotlib>=3.3.0",
+	"opencv-python>=4.6.0",
+	"pillow>=7.1.2",
+	"pyyaml>=5.3.1",
+	"requests>=2.23.0",
+	"scipy>=1.4.1",
+	"torch>=2.7.1",
+	"torch>=1.8.0,!=2.4.0; sys_platform == 'win32'",
+	"torchvision>=0.9.0",
+	"tqdm>=4.64.0",
+	"psutil",
+	"py-cpuinfo",
+	"pandas>=1.1.4",
+	"open3d",
+	```
+
+#### 使用说明
+1. 启动相机和检测节点: 
+
+        ros2 launch detect_part camera_detect.launch.py
+
+	> camera_detect.launch.py 中可以修改部分参数:set_confidence,   output_boxes, output_masks, checkpoint_name
+
+	> 坐标系设置为：Z为图像深度，XY平面为图像成象平面
+
+2. 接收位置话题节点: 
+
+        ros2 run detect_part sub_pose_node
+
+3. 位置话题接口: 
+
+        interface/msg/PoseWithID.msg, PoseWithIDArray.msg
+		
+4. 话题名称: 
+	 
+        /pose/cv_detect_pose # 位置
+        /image/detect_image  # 目标识别图片
+		
+5. PoseWithID.msg：
+
+        string class_name
+        int32 class_id
+        geometry_msgs/Pose[] pose_list
+
+6. PoseWithIDArray.msg：
+
+        PoseWithID[] objects

cpu7.sh

@@ -1 +1,6 @@
-ros2 launch ethercat_control ethercat_node.launch.py
+#!/bin/bash
+SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" >/dev/null 2>&1 && pwd)"
+source /opt/ros/humble/setup.bash
+source "${SCRIPT_DIR}/install/setup.bash"
+
+ros2 launch ethercat_control ethercat_node.launch.py

cv.py

@@ -1,15 +0,0 @@
-import cv2
-import time
-cap = cv2.VideoCapture(2)  # 0表示第一个摄像头
-start=time.time()
-while True:
-    ret, frame = cap.read()
-    if not ret: break
-    end=time.time()
-    diff=end-start
-    print('cap img',1/diff)
-    start=end
-    ###cv2.imshow('USB Camera', frame)
-    if cv2.waitKey(1) == 27: break  # 按ESC退出
-cap.release()
-cv2.destroyAllWindows()

kill_robot.sh

@@ -1,10 +1,14 @@
 #!/usr/bin/env bash
 
-#   ./kill_robot.sh                 # kill ROS 2 processes from this workspace
+#   ./kill_robot.sh [node_names...] # kill specific nodes
+#   ./kill_robot.sh all             # kill all ROS 2 processes from this workspace
 #   ./kill_robot.sh -A              # kill all ROS 2 processes system-wide (dangerous)
 #   ./kill_robot.sh -n              # dry run (show PIDs only)
 #   ./kill_robot.sh -y              # no confirmation
 #   ./kill_robot.sh -v              # verbose logs
+#   ./kill_robot.sh -x node1,node2  # exclude nodes (do not kill)
+#   ./kill_robot.sh except node1 node2  # kill all except these nodes
+#   ./kill_robot.sh all -x rm_arm_node,vision_node  # example: kill all except listed nodes
 
 set -Eeuo pipefail
 
@@ -14,36 +18,108 @@ SCOPE_ALL=false
 DRY_RUN=false
 ASSUME_YES=false
 VERBOSE=false
+TARGET_NODES=()
+EXCLUDE_NODES=()
 
 log() { echo "[kill_ros2] $*"; }
 vlog() { $VERBOSE && echo "[kill_ros2][debug] $*" || true; }
 
-usage() {
-  cat <<EOF
-Usage: $(basename "$0") [options]
-Options:
-  -A        Kill all ROS 2 processes system-wide (not only this workspace)
-  -n        Dry run (list candidate PIDs and commands)
-  -y        Assume yes (no confirmation prompt)
-  -v        Verbose output
-  -h        Show this help and exit
-Env:
-  WS_DIR    Workspace directory (default: script directory)
-EOF
-}
-
-while getopts ":Anyvh" opt; do
-  case $opt in
-    A) SCOPE_ALL=true ;;
-    n) DRY_RUN=true ;;
-    y) ASSUME_YES=true ;;
-    v) VERBOSE=true ;;
-    h) usage; exit 0 ;;
-    :) echo "Option -$OPTARG requires an argument" >&2; exit 2 ;;
-    \?) echo "Unknown option: -$OPTARG" >&2; usage; exit 2 ;;
+# Parse arguments
+# Collect options first, remaining args are target nodes.
+while [[ $# -gt 0 ]]; do
+  case "$1" in
+    -A|--all-system)
+      SCOPE_ALL=true
+      shift
+      ;;
+    -n|--dry-run)
+      DRY_RUN=true
+      shift
+      ;;
+    -y|--yes)
+      ASSUME_YES=true
+      shift
+      ;;
+    -v|--verbose)
+      VERBOSE=true
+      shift
+      ;;
+    -x|--exclude)
+      shift
+      [[ $# -gt 0 ]] || { echo "Error: --exclude requires a value"; exit 1; }
+      IFS=',' read -r -a _excl <<<"$1"
+      EXCLUDE_NODES+=(${_excl[@]})
+      shift
+      ;;
+    except)
+      shift
+      while [[ $# -gt 0 ]]; do
+        EXCLUDE_NODES+=($1)
+        shift
+      done
+      ;;
+    -h|--help)
+      echo "Usage: $0 [OPTIONS] [NODE_NAMES... | all]"
+      echo "  all                  Kill ALL ROS 2 nodes associated with workspace (default behavior if no nodes specified AND 'all' is passed)"
+      echo "  NODE_NAMES...        List of node executable names or keywords to kill (e.g., 'rm_arm_node')"
+      echo "Options:"
+      echo "  -A, --all-system     Kill ALL ROS 2 processes system-wide (ignores workspace check)"
+      echo "  -n, --dry-run        Print PIDs that would be killed, but don't kill them"
+      echo "  -y, --yes            Skip confirmation prompt"
+      echo "  -v, --verbose        Enable verbose logging"
+      echo "  -x, --exclude LIST   Comma-separated node names/keywords to exclude"
+      echo "  except NODES...      Exclude nodes (kill all except these)"
+      exit 0
+      ;;
+    *)
+      TARGET_NODES+=("$1")
+      shift
+      ;;
   esac
 done
 
+if [ ${#TARGET_NODES[@]} -eq 0 ] && [ "$SCOPE_ALL" = false ]; then
+    if [ ${#EXCLUDE_NODES[@]} -gt 0 ]; then
+      TARGET_NODES=("all")
+    else
+      echo "Error: No target nodes specified. Please provide node names or use 'all'."
+      echo "Usage: $0 [node1 node2 ...] | all"
+      exit 1
+    fi
+fi
+
+if [[ "${#TARGET_NODES[@]}" -eq 1 && "${TARGET_NODES[0]}" == "all" ]]; then
+    if [[ ${#EXCLUDE_NODES[@]} -gt 0 ]]; then
+      log "Mode: KILL ALL workspace nodes except: ${EXCLUDE_NODES[*]}"
+    else
+      log "Mode: KILL ALL workspace nodes"
+    fi
+    TARGET_NODES=() # Clear target list to trigger full workspace kill logic
+    # Falls through to existing logic
+elif [[ ${#TARGET_NODES[@]} -gt 0 ]]; then
+    log "Mode: KILL SPECIFIC nodes: ${TARGET_NODES[*]}"
+fi
+
+usage() {
+  cat <<EOF
+Usage: $(basename "$0") [OPTIONS] [NODE_NAMES... | all]
+Options:
+  -A, --all-system  Kill all ROS 2 processes system-wide (not only this workspace)
+  -n, --dry-run     Dry run (list candidate PIDs and commands)
+  -y, --yes         Assume yes (no confirmation prompt)
+  -v, --verbose     Verbose output
+  -x, --exclude     Comma-separated node names/keywords to exclude
+  -h, --help        Show this help and exit
+  
+Arguments:
+  all               Kill ALL ROS 2 nodes associated with workspace
+  NODE_NAMES...     List of node executable names or keywords to kill
+  except NODES...   Exclude nodes (kill all except these)
+Examples:
+  $(basename "$0") all -x rm_arm_node,vision_node
+EOF
+}
+
 # Return 0 if PID is alive, else 1
 is_alive() { kill -0 "$1" 2>/dev/null; }
 
@@ -65,6 +141,30 @@ list_candidates() {
     cmd=${line#* } # everything after first space
     # Skip self
     [[ "$pid" -eq $$ || "$pid" -eq $PPID ]] && continue
+    
+    # --- Filter by specific node names first if provided ---
+    if [[ ${#TARGET_NODES[@]} -gt 0 ]]; then
+        local match_found=false
+        for target in "${TARGET_NODES[@]}"; do
+            if grep -q "$target" <<<"$cmd"; then
+                match_found=true
+                break
+            fi
+        done
+        $match_found || continue
+    fi
+
+    # --- Exclude specific node names if provided ---
+    if [[ ${#EXCLUDE_NODES[@]} -gt 0 ]]; then
+      local exclude_hit=false
+      for excl in "${EXCLUDE_NODES[@]}"; do
+        if grep -q "$excl" <<<"$cmd"; then
+          exclude_hit=true
+          break
+        fi
+      done
+      $exclude_hit && continue
+    fi
 
     # Scope filter
     local in_scope=false
@@ -72,7 +172,7 @@ list_candidates() {
       in_scope=true
     else
       # Focus on processes started from this workspace or via ros2 run/launch
-      if grep -qE "/install/.*/lib/" <<<"$cmd" ||
+      if grep -qE "/install/.*/lib/|/opt/ros/.*/lib/" <<<"$cmd" ||
          grep -qE "\bros2 (run|launch)\b" <<<"$cmd" ||
          grep -qE "python(3)? .*launch" <<<"$cmd"; then
         # If workspace path is known, prefer matches containing it

launch.conf

@@ -2,11 +2,17 @@
 # mode = run or launch
 # group multiple nodes in the same window by using the same title
 
-DRIVER|launch|ethercat_control|ethercat_node.launch.py|
+#DRIVER|launch|ethercat_control|ethercat_node.launch.py|
+#DRIVER|run|led_dev|led_dev_node|
 MOTOR|run|motor_dev|motor_dev_node|
 IMU|launch|imu_dev|imu_dev.launch.py|
-MOTION|launch|robot_control|robot_control.launch.py|
-ARM|launch|rm_arm_control|rm_arm_control.launch.py|
+# ARM|launch|rm_arm_control|rm_arm_control.launch.py|
+ARM|launch|dual_arm_action_server|dual_arm_action_server.launch.py|
 IMG|launch|img_dev|img_dev.launch.py|
-BRAIN|run|brain|brain_node|
-TOOL|run|ctrlgui|ctrlgui_node|
+VISION|launch|vision_detect|medical_sense.launch.py|
+GRIPPER|launch|gripper_dev|gripper_dev.launch.py|
+BRAIN|launch|brain|brain.launch.py|
+MOTION|launch|robot_control|robot_control.launch.py|
+VOICE|launch|robot_speaker|voice.launch.py|
+VOICE|run|robot_speaker|skill_bridge_node|
+TOOL|launch|ctrlgui|ctrlgui.launch.py|

launch_robot.sh

@@ -1,13 +1,51 @@
 #!/usr/bin/env bash
 
+#   ./launch_robot.sh [node_names...] # launch specific nodes
+#   ./launch_robot.sh all             # launch all nodes in launch.conf
+
 # Safer bash defaults for this script
 set -Eeuo pipefail
 
+# Argument check
+TARGET_NODES=()
+LAUNCH_ALL=false
+
+if [ $# -eq 0 ]; then
+    echo "Error: No target nodes specified."
+    echo "Usage: $0 [node1 node2 ...] | all"
+    exit 1
+fi
+
+if [[ "$1" == "all" ]]; then
+    LAUNCH_ALL=true
+else
+    TARGET_NODES=("$@")
+fi
+
 # Workspace directory defaults to the current working directory when the script is run
 WS_DIR="${WS_DIR:-$PWD}"
 SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" >/dev/null 2>&1 && pwd)"
 CONFIG_FILE="${CONFIG_FILE:-${SCRIPT_DIR}/launch.conf}"
 
+# 获取日期和时间戳函数
+date_day() { date +"%Y%m%d"; }
+readable_timestamp() { date +"%Y%m%d_%H%M%S"; }
+
+# 1. 自动创建当天的日志根目录并包含本次启动的时间子目录
+DAILY_LOG_ROOT="${WS_DIR}/logs/$(date_day)"
+LOG_DIR="${DAILY_LOG_ROOT}/robot_logs_$(readable_timestamp)"
+# LOG_ORIGIN_DIR="${LOG_DIR}/origin"
+mkdir -p "${LOG_DIR}"
+# mkdir -p "${LOG_ORIGIN_DIR}"
+
+# 2. 导出相关环境变量
+#export ROS_LOG_DIR="${LOG_ORIGIN_DIR}"
+# ROS 2 无法原生完全禁用文件日志，将其指向 /tmp 下的临时目录以避免持久化保存
+export ROS_LOG_DIR="/tmp/ros_log_discard_$(readable_timestamp)"
+export PYTHONUNBUFFERED=1
+export RCUTILS_LOGGING_BUFFERED_STREAM=1
+export RCUTILS_COLORIZED_OUTPUT=0
+
 # title|mode|package|target|args
 # mode: run => ros2 run <pkg> <executable>
 #       launch => ros2 launch <pkg> <launch_file.py>
@@ -36,7 +74,24 @@ launch_with_gnome_terminal() {
     # Build unique title list
     local unique_titles=()
     for entry in "${COMPONENTS[@]}"; do
-        IFS='|' read -r title _ _ _ _ <<<"$entry"
+        IFS='|' read -r title mode pkg target args <<<"$entry"
+        
+        # --- Filter Logic ---
+        if [ "$LAUNCH_ALL" = false ]; then
+            local match_found=false
+            for node_filter in "${TARGET_NODES[@]}"; do
+                # Match against package name or executable name
+                if [[ "$node_filter" == "$pkg" ]] || [[ "$node_filter" == "$target" ]]; then
+                    match_found=true
+                    break
+                fi
+            done
+            if [ "$match_found" = false ]; then
+                continue
+            fi
+        fi
+        # --------------------
+
         local found=0
         for t in "${unique_titles[@]:-}"; do
             if [[ "$t" == "$title" ]]; then found=1; break; fi
@@ -50,21 +105,51 @@ launch_with_gnome_terminal() {
         for entry in "${COMPONENTS[@]}"; do
             IFS='|' read -r etitle mode pkg target args <<<"$entry"
             [[ "$etitle" != "$title" ]] && continue
+
+            # --- Filter Logic (Repeated for execution block) ---
+            if [ "$LAUNCH_ALL" = false ]; then
+                local match_found=false
+                for node_filter in "${TARGET_NODES[@]}"; do
+                    # Match against package name or executable name
+                    if [[ "$node_filter" == "$pkg" ]] || [[ "$node_filter" == "$target" ]]; then
+                        match_found=true
+                        break
+                    fi
+                done
+                if [ "$match_found" = false ]; then
+                    continue
+                fi
+            fi
+            # --------------------
+
             args="${args:-}"
             local ros_cmd
-            if [[ "$mode" == "launch" ]]; then
+            if [[ "${pkg}" == "ethercat_control" ]]; then
+                ros_cmd="taskset -c 7 ./cpu7.sh"
+            elif [[ "$mode" == "launch" ]]; then
                 ros_cmd="ros2 launch ${pkg} ${target} ${args}"
             else
                 ros_cmd="ros2 run ${pkg} ${target} ${args}"
             fi
-            cmds_block+="${ros_cmd} "$'\n'
-            if [[${pkg} == "ethercat_control"]]; then
-                cmds_block="taskset -c 7 ./cpu7.sh"
+
+            if [[ "${pkg}" == "robot_control" ]]; then
+                ros_cmd="echo 'Waiting for EtherCAT slaves to be OP...'; while ! ethercat slaves 2>&1 | grep -q 'OP' || ethercat slaves 2>&1 | grep -v 'OP' | grep -q .; do sleep 2; done; ${ros_cmd}"
             fi
+
+            # 3. 为每个组件生成带明文时间戳的日志文件名并添加重定向
+            local log_file="${LOG_DIR}/${etitle}_${target}_$(readable_timestamp).log"
+            ros_cmd="{ ${ros_cmd}; } 2>&1 | tee \"${log_file}\""
+
+            cmds_block+="${ros_cmd} &"$'\n'
         done
+        
+        # If no commands collected for this title (because all were filtered out), skip launching terminal
+        if [[ -z "$cmds_block" ]]; then continue; fi
+
         # Compose the final script for this terminal
         local cmd
         cmd=$(compose_terminal_script "$title" "$cmds_block")
+
         # gnome-terminal --title="${title}" -- bash -lc "$cmd" &
         bash -c "$cmd" &
         sleep 3
@@ -73,7 +158,24 @@ launch_with_gnome_terminal() {
 launch_with_xterm() {
     local unique_titles=()
     for entry in "${COMPONENTS[@]}"; do
-        IFS='|' read -r title _ _ _ _ <<<"$entry"
+        IFS='|' read -r title mode pkg target args <<<"$entry"
+        
+        # --- Filter Logic ---
+        if [ "$LAUNCH_ALL" = false ]; then
+            local match_found=false
+            for node_filter in "${TARGET_NODES[@]}"; do
+                # Match against package name or executable name
+                if [[ "$node_filter" == "$pkg" ]] || [[ "$node_filter" == "$target" ]]; then
+                    match_found=true
+                    break
+                fi
+            done
+            if [ "$match_found" = false ]; then
+                continue
+            fi
+        fi
+        # --------------------
+
         local found=0
         for t in "${unique_titles[@]:-}"; do
             if [[ "$t" == "$title" ]]; then found=1; break; fi
@@ -86,15 +188,47 @@ launch_with_xterm() {
         for entry in "${COMPONENTS[@]}"; do
             IFS='|' read -r etitle mode pkg target args <<<"$entry"
             [[ "$etitle" != "$title" ]] && continue
+
+            # --- Filter Logic (Repeated for execution block) ---
+            if [ "$LAUNCH_ALL" = false ]; then
+                local match_found=false
+                for node_filter in "${TARGET_NODES[@]}"; do
+                    # Match against package name or executable name
+                    if [[ "$node_filter" == "$pkg" ]] || [[ "$node_filter" == "$target" ]]; then
+                        match_found=true
+                        break
+                    fi
+                done
+                if [ "$match_found" = false ]; then
+                    continue
+                fi
+            fi
+            # --------------------
+
             args="${args:-}"
             local ros_cmd
-            if [[ "$mode" == "launch" ]]; then
+            if [[ "${pkg}" == "ethercat_control" ]]; then
+                ros_cmd="taskset -c 7 ./cpu7.sh"
+            elif [[ "$mode" == "launch" ]]; then
                 ros_cmd="ros2 launch ${pkg} ${target} ${args}"
             else
                 ros_cmd="ros2 run ${pkg} ${target} ${args}"
             fi
+
+            if [[ "${pkg}" == "robot_control" ]]; then
+                ros_cmd="echo 'Waiting for EtherCAT slaves to be OP...'; while ! ethercat slaves 2>&1 | grep -q 'OP' || ethercat slaves 2>&1 | grep -v 'OP' | grep -q .; do sleep 2; done; ${ros_cmd}"
+            fi
+
+            # 3. 为每个组件生成带明文时间戳的日志文件名并添加重定向
+            local log_file="${LOG_DIR}/${etitle}_${target}_$(readable_timestamp).log"
+            ros_cmd="{ ${ros_cmd}; } 2>&1 | tee \"${log_file}\""
+
             cmds_block+="${ros_cmd} &"$'\n'
         done
+        
+        # If no commands collected for this title (because all were filtered out), skip launching terminal
+        if [[ -z "$cmds_block" ]]; then continue; fi
+
         local cmd
         cmd=$(compose_terminal_script "$title" "$cmds_block")
         xterm -T "${title}" -hold -e bash -lc "$cmd" &
@@ -109,8 +243,8 @@ main() {
         echo "Using xterm"
         launch_with_xterm
     else
-        echo "No terminal emulator found (gnome-terminal or xterm). Please install one and retry." >&2
-        exit 1
+        echo "No terminal emulator found (gnome-terminal or xterm). Proceeding with background execution." >&2
+        launch_with_gnome_terminal
     fi
 }