From 58aa9e296467a202b8bba15bbf5e4c3faa21d8d6 Mon Sep 17 00:00:00 2001 From: weipengzhe <3389096668@qq.com> Date: Fri, 18 Sep 2026 18:39:55 +0800 Subject: [PATCH] =?UTF-8?q?=E5=8A=A0=E5=85=A5=E7=81=B5=E5=B7=A7=E6=89=8B?= =?UTF-8?q?=E7=9A=84rosbag=E5=BD=95=E5=88=B6?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- scripts/linker_hand_control.sh | 131 +++++++++++++++++++++++++++++++++ 1 file changed, 131 insertions(+) diff --git a/scripts/linker_hand_control.sh b/scripts/linker_hand_control.sh index 0e55d2f..0361de3 100755 --- a/scripts/linker_hand_control.sh +++ b/scripts/linker_hand_control.sh @@ -6,9 +6,20 @@ WS_DIR="$(cd "${SCRIPT_DIR}/.." && pwd)" STATE_DIR="/tmp/linker_hand_control" PID_FILE="${STATE_DIR}/linker_hand.pid" PGID_FILE="${STATE_DIR}/linker_hand.pgid" +BAG_PID_FILE="${STATE_DIR}/rosbag.pid" +BAG_PGID_FILE="${STATE_DIR}/rosbag.pgid" LOG_FILE="${WS_DIR}/log/linker_hand_control.log" +BAG_LOG_FILE="${WS_DIR}/log/linker_hand_rosbag.log" CAN_INTERFACES=(can0 can1) CAN_BITRATE=1000000 +ROSBAG_TOPICS=( + /l20_left_hand_control_cmd + /l20_left_hand_state + /l20_left_hand_matrix_touch_mass + /l20_right_hand_control_cmd + /l20_right_hand_state + /l20_right_hand_matrix_touch_mass +) # Keep this driver isolated from ROS 2 nodes on other machines in the same LAN. # Without this, DDS discovery can pick up stale linker_hand_sdk nodes from the @@ -27,6 +38,14 @@ is_running() { kill -0 "${pid}" 2>/dev/null } +is_bag_running() { + [[ -f "${BAG_PID_FILE}" ]] || return 1 + local pid + pid="$(cat "${BAG_PID_FILE}")" + [[ -n "${pid}" ]] || return 1 + kill -0 "${pid}" 2>/dev/null +} + source_workspace() { set +u if [[ -n "${ROS_DISTRO:-}" && -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]]; then @@ -78,11 +97,110 @@ stop_can() { done } +start_bag_recorder() { + mkdir -p "${STATE_DIR}" "$(dirname "${BAG_LOG_FILE}")" + + if is_bag_running; then + echo "rosbag recorder is already running, pid=$(cat "${BAG_PID_FILE}")" + return 0 + fi + + echo "Starting rosbag recorder..." + echo "Rosbag log: ${BAG_LOG_FILE}" + + : > "${BAG_LOG_FILE}" + setsid env \ + WS_DIR="${WS_DIR}" \ + ROS_AUTOMATIC_DISCOVERY_RANGE="${ROS_AUTOMATIC_DISCOVERY_RANGE}" \ + bash -lc ' + set -euo pipefail + set +u + source "${WS_DIR}/install/setup.bash" + set -u + while true; do + start_time=$(date +%Y%m%d_%H%M%S) + output=/tmp/L20_Hand_${start_time}.mcap + echo "Starting rosbag: ${output}" + set +e + timeout 3600 ros2 bag record \ + --storage mcap \ + --output "${output}" \ + --polling-interval 2 \ + --include-unpublished-topics \ + --disable-keyboard-controls \ + --log-level warn \ + --topics \ + /l20_left_hand_control_cmd \ + /l20_left_hand_state \ + /l20_left_hand_matrix_touch_mass \ + /l20_right_hand_control_cmd \ + /l20_right_hand_state \ + /l20_right_hand_matrix_touch_mass + rc=$? + set -e + if [[ ${rc} -ne 124 ]]; then + exit ${rc} + fi + done + ' >> "${BAG_LOG_FILE}" 2>&1 & + + local pid=$! + local pgid + pgid="$(ps -o pgid= -p "${pid}" | tr -d ' ')" + echo "${pid}" > "${BAG_PID_FILE}" + echo "${pgid:-${pid}}" > "${BAG_PGID_FILE}" + + sleep 1 + if is_bag_running; then + echo "rosbag recorder started, pid=${pid}, pgid=$(cat "${BAG_PGID_FILE}")" + else + echo "rosbag recorder failed to start. Check log: ${BAG_LOG_FILE}" + rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}" + return 1 + fi +} + +stop_bag_recorder() { + if ! [[ -f "${BAG_PID_FILE}" ]]; then + return 0 + fi + + local pid pgid + pid="$(cat "${BAG_PID_FILE}")" + pgid="${pid}" + if [[ -f "${BAG_PGID_FILE}" ]]; then + pgid="$(cat "${BAG_PGID_FILE}")" + fi + + if ! kill -0 "${pid}" 2>/dev/null; then + rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}" + return 0 + fi + + echo "Stopping rosbag recorder, pid=${pid}, pgid=${pgid}..." + kill -TERM -- "-${pgid}" 2>/dev/null || kill -TERM "${pid}" 2>/dev/null || true + + for _ in {1..30}; do + if ! kill -0 "${pid}" 2>/dev/null; then + rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}" + echo "rosbag recorder stopped" + return 0 + fi + sleep 0.2 + done + + echo "rosbag recorder did not exit after SIGTERM, forcing stop..." + kill -KILL -- "-${pgid}" 2>/dev/null || kill -KILL "${pid}" 2>/dev/null || true + rm -f "${BAG_PID_FILE}" "${BAG_PGID_FILE}" + echo "rosbag recorder stopped" +} + start_driver() { mkdir -p "${STATE_DIR}" "$(dirname "${LOG_FILE}")" if is_running; then echo "linker_hand service is already running, pid=$(cat "${PID_FILE}")" + start_bag_recorder return 0 fi @@ -106,6 +224,7 @@ start_driver() { sleep 1 if is_running; then echo "linker_hand service started, pid=${pid}, pgid=$(cat "${PGID_FILE}")" + start_bag_recorder else echo "linker_hand service failed to start. Check log: ${LOG_FILE}" rm -f "${PID_FILE}" "${PGID_FILE}" @@ -117,6 +236,7 @@ start_driver() { stop_driver() { if ! [[ -f "${PID_FILE}" ]]; then echo "linker_hand service is not running" + stop_bag_recorder stop_can return 0 fi @@ -131,6 +251,7 @@ stop_driver() { if ! kill -0 "${pid}" 2>/dev/null; then echo "linker_hand process is not alive, cleaning state" rm -f "${PID_FILE}" "${PGID_FILE}" + stop_bag_recorder stop_can return 0 fi @@ -142,6 +263,7 @@ stop_driver() { if ! kill -0 "${pid}" 2>/dev/null; then rm -f "${PID_FILE}" "${PGID_FILE}" echo "linker_hand service stopped" + stop_bag_recorder stop_can return 0 fi @@ -152,6 +274,7 @@ stop_driver() { kill -KILL -- "-${pgid}" 2>/dev/null || kill -KILL "${pid}" 2>/dev/null || true rm -f "${PID_FILE}" "${PGID_FILE}" echo "linker_hand service stopped" + stop_bag_recorder stop_can } @@ -164,6 +287,14 @@ status_driver() { echo "ROS_AUTOMATIC_DISCOVERY_RANGE=${ROS_AUTOMATIC_DISCOVERY_RANGE}" + if is_bag_running; then + echo "rosbag recorder is running, pid=$(cat "${BAG_PID_FILE}"), pgid=$(cat "${BAG_PGID_FILE}" 2>/dev/null || cat "${BAG_PID_FILE}")" + else + echo "rosbag recorder is not running" + fi + + echo "rosbag topics: ${ROSBAG_TOPICS[*]}" + for can_if in "${CAN_INTERFACES[@]}"; do if ip link show "${can_if}" >/dev/null 2>&1; then ip -details link show "${can_if}" | sed -n '1,2p'