139 lines
3.2 KiB
Bash
Executable File
139 lines
3.2 KiB
Bash
Executable File
#!/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
|