diff --git a/scripts/linker_hand_control.sh b/scripts/linker_hand_control.sh new file mode 100755 index 0000000..1a3f44a --- /dev/null +++ b/scripts/linker_hand_control.sh @@ -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