L20灵巧手启动文件

2026-09-18 13:14:27 +08:00
parent fb682422d3
commit 37c426f994
1 changed files with 177 additions and 0 deletions
@@ -0,0 +1,177 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+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"
+LOG_FILE="${WS_DIR}/log/linker_hand_control.log"
+CAN_INTERFACES=(can0 can1)
+CAN_BITRATE=1000000
+
+usage() {
+  echo "Usage: $0 {start|stop|status}"
+}
+
+is_running() {
+  [[ -f "${PID_FILE}" ]] || return 1
+  local pid
+  pid="$(cat "${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
+    # shellcheck disable=SC1090
+    source "/opt/ros/${ROS_DISTRO}/setup.bash"
+  elif [[ -f "/opt/ros/jazzy/setup.bash" ]]; then
+    # shellcheck disable=SC1091
+    source "/opt/ros/jazzy/setup.bash"
+  fi
+  set -u
+
+  if [[ ! -f "${WS_DIR}/install/setup.bash" ]]; then
+    echo "Workspace is not built: ${WS_DIR}/install/setup.bash not found"
+    echo "Run: cd ${WS_DIR} && colcon build --packages-select linker_hand_ros2_sdk"
+    exit 1
+  fi
+
+  set +u
+  # shellcheck disable=SC1091
+  source "${WS_DIR}/install/setup.bash"
+  set -u
+}
+
+start_can() {
+  for can_if in "${CAN_INTERFACES[@]}"; do
+    if ! ip link show "${can_if}" >/dev/null 2>&1; then
+      echo "CAN interface not found: ${can_if}"
+      exit 1
+    fi
+
+    echo "Starting ${can_if} at ${CAN_BITRATE} bps..."
+    sudo ip link set "${can_if}" down 2>/dev/null || true
+    sudo ip link set "${can_if}" type can bitrate "${CAN_BITRATE}"
+    sudo ip link set "${can_if}" up
+  done
+}
+
+stop_can() {
+  for can_if in "${CAN_INTERFACES[@]}"; do
+    if ip link show "${can_if}" >/dev/null 2>&1; then
+      echo "Stopping ${can_if}..."
+      sudo ip link set "${can_if}" down 2>/dev/null || true
+    fi
+  done
+}
+
+start_driver() {
+  mkdir -p "${STATE_DIR}" "$(dirname "${LOG_FILE}")"
+
+  if is_running; then
+    echo "linker_hand service is already running, pid=$(cat "${PID_FILE}")"
+    return 0
+  fi
+
+  source_workspace
+  start_can
+
+  echo "Starting linker_hand service..."
+  echo "Log: ${LOG_FILE}"
+
+  : > "${LOG_FILE}"
+  setsid bash -lc "set +u; source '${WS_DIR}/install/setup.bash'; set -u; exec ros2 launch linker_hand_ros2_sdk linker_hand_double.launch.py" \
+    >> "${LOG_FILE}" 2>&1 &
+
+  local pid=$!
+  local pgid
+  pgid="$(ps -o pgid= -p "${pid}" | tr -d ' ')"
+  echo "${pid}" > "${PID_FILE}"
+  echo "${pgid:-${pid}}" > "${PGID_FILE}"
+
+  sleep 1
+  if is_running; then
+    echo "linker_hand service started, pid=${pid}, pgid=$(cat "${PGID_FILE}")"
+  else
+    echo "linker_hand service failed to start. Check log: ${LOG_FILE}"
+    rm -f "${PID_FILE}" "${PGID_FILE}"
+    stop_can
+    exit 1
+  fi
+}
+
+stop_driver() {
+  if ! [[ -f "${PID_FILE}" ]]; then
+    echo "linker_hand service is not running"
+    stop_can
+    return 0
+  fi
+
+  local pid pgid
+  pid="$(cat "${PID_FILE}")"
+  pgid="${pid}"
+  if [[ -f "${PGID_FILE}" ]]; then
+    pgid="$(cat "${PGID_FILE}")"
+  fi
+
+  if ! kill -0 "${pid}" 2>/dev/null; then
+    echo "linker_hand process is not alive, cleaning state"
+    rm -f "${PID_FILE}" "${PGID_FILE}"
+    stop_can
+    return 0
+  fi
+
+  echo "Stopping linker_hand service, 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 "${PID_FILE}" "${PGID_FILE}"
+      echo "linker_hand service stopped"
+      stop_can
+      return 0
+    fi
+    sleep 0.2
+  done
+
+  echo "linker_hand service did not exit after SIGTERM, forcing stop..."
+  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_can
+}
+
+status_driver() {
+  if is_running; then
+    echo "linker_hand service is running, pid=$(cat "${PID_FILE}"), pgid=$(cat "${PGID_FILE}" 2>/dev/null || cat "${PID_FILE}")"
+  else
+    echo "linker_hand service is not running"
+  fi
+
+  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'
+    else
+      echo "${can_if}: not found"
+    fi
+  done
+}
+
+case "${1:-}" in
+  start)
+    start_driver
+    ;;
+  stop)
+    stop_driver
+    ;;
+  status)
+    status_driver
+    ;;
+  *)
+    usage
+    exit 1
+    ;;
+esac