加入灵巧手的rosbag录制

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'