disable some logs

2025-11-05 15:13:31 +08:00
parent 4233d3e79d
commit ada14a30e3
14 changed files with 456 additions and 87 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -20,3 +20,5 @@ csp_log.csv
 camera_pose_data.json
 eye_to_hand_matrix.json
 hand_pose_data.json
+
+.vscode/
--- a/.vscode/settings.json
+++ b/.vscode/settings.json
@@ -1,84 +1,145 @@
 {
-    "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"
-    }
+  "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"
 }
--- a/cpu7.sh
+++ b/cpu7.sh
@@ -0,0 +1 @@
+ros2 launch ethercat_control ethercat_node.launch.py
--- a/cv.py
+++ b/cv.py
@@ -0,0 +1,15 @@
+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()
--- a/1
+++ b/1
--- a/2
+++ b/2
--- a/1
+++ b/1
--- a/2
+++ b/2
--- a/2
+++ b/2
--- a/2
+++ b/2
--- a/2
+++ b/2
--- a/kill_robot.sh
+++ b/kill_robot.sh
@@ -0,0 +1,159 @@
+#!/usr/bin/env bash
+
+#   ./kill_robot.sh                 # kill 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
+
+set -Eeuo pipefail
+
+SCRIPT_DIR="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")" >/dev/null 2>&1 && pwd)"
+WS_DIR="${WS_DIR:-${SCRIPT_DIR}}"
+SCOPE_ALL=false
+DRY_RUN=false
+ASSUME_YES=false
+VERBOSE=false
+
+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 ;;
+  esac
+done
+
+# Return 0 if PID is alive, else 1
+is_alive() { kill -0 "$1" 2>/dev/null; }
+
+# Check if a PID looks like a ROS process by inspecting its environment
+is_ros_env() {
+  local pid=$1
+  [[ -r "/proc/${pid}/environ" ]] || return 1
+  tr '\0' '\n' <"/proc/${pid}/environ" | grep -Eq '^(ROS_DISTRO=|RMW_IMPLEMENTATION=)'
+}
+
+# List candidate PIDs and their commands (tab-separated)
+list_candidates() {
+  local ps_out
+  # Use full command line; exclude our own shell/grep processes later
+  ps_out=$(ps -eo pid=,cmd=)
+  while IFS= read -r line; do
+    local pid cmd
+    pid=$(awk '{print $1}' <<<"$line")
+    cmd=${line#* } # everything after first space
+    # Skip self
+    [[ "$pid" -eq $$ || "$pid" -eq $PPID ]] && continue
+
+    # Scope filter
+    local in_scope=false
+    if $SCOPE_ALL; then
+      in_scope=true
+    else
+      # Focus on processes started from this workspace or via ros2 run/launch
+      if grep -qE "/install/.*/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
+        if grep -q "${WS_DIR}" <<<"$cmd" || $SCOPE_ALL; then in_scope=true; else in_scope=true; fi
+      fi
+    fi
+
+    $in_scope || continue
+
+    # Must look like a ROS process (has ROS env vars)
+    if is_ros_env "$pid"; then
+      printf "%s\t%s\n" "$pid" "$cmd"
+    else
+      vlog "skip pid=$pid (no ROS env) cmd=$cmd"
+    fi
+  done <<<"$ps_out"
+}
+
+confirm() {
+  $ASSUME_YES && return 0
+  read -r -p "Kill the above processes? [y/N] " ans || true
+  [[ "${ans,,}" == "y" || "${ans,,}" == "yes" ]]
+}
+
+kill_with_escalation() {
+  local pids=("$@")
+  local stages=(INT TERM KILL)
+  local stage
+  for stage in "${stages[@]}"; do
+    local alive=()
+    for pid in "${pids[@]}"; do
+      if is_alive "$pid"; then alive+=("$pid"); fi
+    done
+    ((${#alive[@]}==0)) && return 0
+
+    log "Sending SIG${stage} to ${#alive[@]} process(es): ${alive[*]}"
+    kill -s "$stage" -- "${alive[@]}" 2>/dev/null || true
+
+    # Wait up to 3s for this stage
+    for _ in {1..30}; do
+      sleep 0.1
+      local still=()
+      for pid in "${alive[@]}"; do is_alive "$pid" && still+=("$pid"); done
+      ((${#still[@]}==0)) && break
+      alive=("${still[@]}")
+    done
+  done
+}
+
+main() {
+  log "Workspace: ${WS_DIR} | Scope: $($SCOPE_ALL && echo all || echo workspace)"
+  local candidates
+  candidates=$(list_candidates || true)
+  if [[ -z "$candidates" ]]; then
+    log "No ROS 2 processes found to kill."
+    exit 0
+  fi
+
+  echo "Candidates (PID\tCMD):"
+  echo "$candidates" | sed 's/^/  /'
+
+  if $DRY_RUN; then
+    log "Dry run only. Exiting."
+    exit 0
+  fi
+
+  if ! confirm; then
+    log "Cancelled by user."
+    exit 1
+  fi
+
+  # Extract PIDs and kill
+  mapfile -t pids < <(awk -F '\t' '{print $1}' <<<"$candidates" | sort -u)
+  kill_with_escalation "${pids[@]}"
+
+  # Optionally stop ros2 daemon so it doesn't hold stale graph
+  if command -v ros2 >/dev/null 2>&1; then
+    ros2 daemon stop >/dev/null 2>&1 || true
+  fi
+
+  log "Done."
+}
+
+main "$@"
--- a/launch.conf
+++ b/launch.conf
@@ -0,0 +1,12 @@
+# title|mode|package|target|args
+# mode = run or launch
+# group multiple nodes in the same window by using the same title
+
+DRIVER|launch|ethercat_control|ethercat_node.launch.py|
+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|
+IMG|launch|img_dev|img_dev.launch.py|
+BRAIN|run|brain|brain_node|
+TOOL|run|ctrlgui|ctrlgui_node|
--- a/launch_robot.sh
+++ b/launch_robot.sh
@@ -0,0 +1,117 @@
+#!/usr/bin/env bash
+
+# Safer bash defaults for this script
+set -Eeuo pipefail
+
+# 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}"
+
+# title|mode|package|target|args
+# mode: run => ros2 run <pkg> <executable>
+#       launch => ros2 launch <pkg> <launch_file.py>
+if [[ -f "${CONFIG_FILE}" ]]; then
+    mapfile -t COMPONENTS < <(grep -vE '^\s*#' "${CONFIG_FILE}" | sed '/^\s*$/d')
+else
+    echo "no configuration file found at ${CONFIG_FILE}, exiting." >&2
+    exit 1
+fi
+
+compose_terminal_script() {
+    local title="$1" cmds_block="$2"
+    cat <<EOF
+echo Launching Robot ${title}
+cd "${WS_DIR}"
+set +u; source install/setup.bash || { echo "Workspace not built (missing install/setup.bash). Run: colcon build" >&2; }; set -u;
+# Freeze terminal title so it won't be changed by shell rc/profile
+printf '\033]0;%s\007' "${title}"
+${cmds_block}
+echo "All processes launched for: Launching Robot ${title}"
+exec bash --noprofile --norc
+EOF
+}
+
+launch_with_gnome_terminal() {
+    # Build unique title list
+    local unique_titles=()
+    for entry in "${COMPONENTS[@]}"; do
+        IFS='|' read -r title _ _ _ _ <<<"$entry"
+        local found=0
+        for t in "${unique_titles[@]:-}"; do
+            if [[ "$t" == "$title" ]]; then found=1; break; fi
+        done
+        if [[ $found -eq 0 ]]; then unique_titles+=("$title"); fi
+    done
+
+    # For each title, gather commands and open a terminal
+    for title in "${unique_titles[@]}"; do
+        local cmds_block=""
+        for entry in "${COMPONENTS[@]}"; do
+            IFS='|' read -r etitle mode pkg target args <<<"$entry"
+            [[ "$etitle" != "$title" ]] && continue
+            args="${args:-}"
+            local ros_cmd
+            if [[ "$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"
+            fi
+        done
+        # 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
+    done
+}
+launch_with_xterm() {
+    local unique_titles=()
+    for entry in "${COMPONENTS[@]}"; do
+        IFS='|' read -r title _ _ _ _ <<<"$entry"
+        local found=0
+        for t in "${unique_titles[@]:-}"; do
+            if [[ "$t" == "$title" ]]; then found=1; break; fi
+        done
+        if [[ $found -eq 0 ]]; then unique_titles+=("$title"); fi
+    done
+
+    for title in "${unique_titles[@]}"; do
+        local cmds_block=""
+        for entry in "${COMPONENTS[@]}"; do
+            IFS='|' read -r etitle mode pkg target args <<<"$entry"
+            [[ "$etitle" != "$title" ]] && continue
+            args="${args:-}"
+            local ros_cmd
+            if [[ "$mode" == "launch" ]]; then
+                ros_cmd="ros2 launch ${pkg} ${target} ${args}"
+            else
+                ros_cmd="ros2 run ${pkg} ${target} ${args}"
+            fi
+            cmds_block+="${ros_cmd} &"$'\n'
+        done
+        local cmd
+        cmd=$(compose_terminal_script "$title" "$cmds_block")
+        xterm -T "${title}" -hold -e bash -lc "$cmd" &
+    done
+}
+main() {
+    echo "Starting robot components..."
+    if command -v gnome-terminal >/dev/null 2>&1; then
+        echo "Using gnome-terminal"
+        launch_with_gnome_terminal
+    elif command -v xterm >/dev/null 2>&1; then
+        echo "Using xterm"
+        launch_with_xterm
+    else
+        echo "No terminal emulator found (gnome-terminal or xterm). Please install one and retry." >&2
+        exit 1
+    fi
+}
+
+main "$@"
				`@@ -0,0 +1 @@`
				`ros2 launch ethercat_control ethercat_node.launch.py`