机械臂与灵巧手位控逻辑已完备

2026-09-17 10:50:14 +08:00
parent c649b25845
commit 9479a8ccb2
50 changed files with 4237 additions and 0 deletions
@@ -0,0 +1,138 @@
+#!/usr/bin/env bash
+set -euo pipefail
+
+SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
+WS_DIR="$(cd "${SCRIPT_DIR}/.." && pwd)"
+STATE_DIR="/tmp/lbot_arm_control"
+PID_FILE="${STATE_DIR}/lbot_driver.pid"
+PGID_FILE="${STATE_DIR}/lbot_driver.pgid"
+LOG_FILE="${WS_DIR}/log/lbot_arm_control.log"
+
+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 lbot_arm_interfaces lbot_driver"
+    exit 1
+  fi
+
+  # shellcheck disable=SC1091
+  set +u
+  source "${WS_DIR}/install/setup.bash"
+  set -u
+}
+
+start_driver() {
+  mkdir -p "${STATE_DIR}" "$(dirname "${LOG_FILE}")"
+
+  if is_running; then
+    echo "lbot_driver is already running, pid=$(cat "${PID_FILE}")"
+    return 0
+  fi
+
+  source_workspace
+
+  echo "Starting lbot_driver from officer_sdk..."
+  echo "Log: ${LOG_FILE}"
+
+  : > "${LOG_FILE}"
+  setsid bash -lc "set +u; source '${WS_DIR}/install/setup.bash'; set -u; exec ros2 launch lbot_driver lbot_start_driver.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 "lbot_driver started, pid=${pid}, pgid=$(cat "${PGID_FILE}")"
+  else
+    echo "lbot_driver failed to start. Check log: ${LOG_FILE}"
+    rm -f "${PID_FILE}" "${PGID_FILE}"
+    exit 1
+  fi
+}
+
+stop_driver() {
+  if ! [[ -f "${PID_FILE}" ]]; then
+    echo "lbot_driver is not running"
+    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 "lbot_driver process is not alive, cleaning state"
+    rm -f "${PID_FILE}" "${PGID_FILE}"
+    return 0
+  fi
+
+  echo "Stopping lbot_driver, 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 "lbot_driver stopped"
+      return 0
+    fi
+    sleep 0.2
+  done
+
+  echo "lbot_driver 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 "lbot_driver stopped"
+}
+
+status_driver() {
+  if is_running; then
+    echo "lbot_driver is running, pid=$(cat "${PID_FILE}"), pgid=$(cat "${PGID_FILE}" 2>/dev/null || cat "${PID_FILE}")"
+  else
+    echo "lbot_driver is not running"
+  fi
+}
+
+case "${1:-}" in
+  start)
+    start_driver
+    ;;
+  stop)
+    stop_driver
+    ;;
+  status)
+    status_driver
+    ;;
+  *)
+    usage
+    exit 1
+    ;;
+esac