From ada14a30e3939da606859a330006ef4575a1da25 Mon Sep 17 00:00:00 2001 From: NuoDaJia02 Date: Wed, 5 Nov 2025 15:13:31 +0800 Subject: [PATCH] disable some logs --- .gitignore | 2 + .vscode/settings.json | 225 ++++++++++++++++++++++++-------------- cpu7.sh | 1 + cv.py | 15 +++ hivecore_robot_arm | 1 + hivecore_robot_brain | 2 +- hivecore_robot_ctrlgui | 1 + hivecore_robot_drivers | 2 +- hivecore_robot_interfaces | 2 +- hivecore_robot_motion | 2 +- hivecore_robot_vision | 2 +- kill_robot.sh | 159 +++++++++++++++++++++++++++ launch.conf | 12 ++ launch_robot.sh | 117 ++++++++++++++++++++ 14 files changed, 456 insertions(+), 87 deletions(-) create mode 100755 cpu7.sh create mode 100644 cv.py create mode 160000 hivecore_robot_arm create mode 160000 hivecore_robot_ctrlgui create mode 100755 kill_robot.sh create mode 100755 launch.conf create mode 100755 launch_robot.sh diff --git a/.gitignore b/.gitignore index 7e06825..420b442 100644 --- 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/ \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json index d541702..44afb56 100644 --- 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" } \ No newline at end of file diff --git a/cpu7.sh b/cpu7.sh new file mode 100755 index 0000000..380adb6 --- /dev/null +++ b/cpu7.sh @@ -0,0 +1 @@ +ros2 launch ethercat_control ethercat_node.launch.py diff --git a/cv.py b/cv.py new file mode 100644 index 0000000..765181b --- /dev/null +++ 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() diff --git a/hivecore_robot_arm b/hivecore_robot_arm new file mode 160000 index 0000000..21d9952 --- /dev/null +++ b/hivecore_robot_arm @@ -0,0 +1 @@ +Subproject commit 21d995292261692aea3ef8b7ef1a5c5272525ddf diff --git a/hivecore_robot_brain b/hivecore_robot_brain index 1ba451a..893bca0 160000 --- a/hivecore_robot_brain +++ b/hivecore_robot_brain @@ -1 +1 @@ -Subproject commit 1ba451a3cccd2f11113a54406941e6de8edfb722 +Subproject commit 893bca0d2e96e13252b6a3ef46ad3a21a3aaf7b7 diff --git a/hivecore_robot_ctrlgui b/hivecore_robot_ctrlgui new file mode 160000 index 0000000..6815114 --- /dev/null +++ b/hivecore_robot_ctrlgui @@ -0,0 +1 @@ +Subproject commit 68151146e87ac15912044d3383398a8b8ace1863 diff --git a/hivecore_robot_drivers b/hivecore_robot_drivers index bfe060e..080af2f 160000 --- a/hivecore_robot_drivers +++ b/hivecore_robot_drivers @@ -1 +1 @@ -Subproject commit bfe060e01a2115bc03cb03b11925d13c174fec83 +Subproject commit 080af2f7fa79218d4289f88eaa547f17e8a6622b diff --git a/hivecore_robot_interfaces b/hivecore_robot_interfaces index e6ff6a8..62f17e0 160000 --- a/hivecore_robot_interfaces +++ b/hivecore_robot_interfaces @@ -1 +1 @@ -Subproject commit e6ff6a8900f88873676b66c2c080137bd04a96cf +Subproject commit 62f17e052a86bff7e7ae188f2e67a7b0406ee5ce diff --git a/hivecore_robot_motion b/hivecore_robot_motion index 2f69f94..55dd17e 160000 --- a/hivecore_robot_motion +++ b/hivecore_robot_motion @@ -1 +1 @@ -Subproject commit 2f69f94bbda2a53f98572b79db52404555b3f63a +Subproject commit 55dd17e055e24ae747ffd48dccb7384de791faac diff --git a/hivecore_robot_vision b/hivecore_robot_vision index c0638c2..2740a75 160000 --- a/hivecore_robot_vision +++ b/hivecore_robot_vision @@ -1 +1 @@ -Subproject commit c0638c262eed64e444cd38a1cab93639c95d153c +Subproject commit 2740a751f521e08509483cfa6e9b62450b88d882 diff --git a/kill_robot.sh b/kill_robot.sh new file mode 100755 index 0000000..b2994f7 --- /dev/null +++ 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 <&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 "$@" diff --git a/launch.conf b/launch.conf new file mode 100755 index 0000000..b3d1f76 --- /dev/null +++ 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| diff --git a/launch_robot.sh b/launch_robot.sh new file mode 100755 index 0000000..0e93e94 --- /dev/null +++ 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 +# launch => ros2 launch +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 <&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 "$@"