322 lines
8.3 KiB
Bash
Executable File
322 lines
8.3 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/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
|
|
# subnet and make the same topic appear to have multiple publishers.
|
|
export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST
|
|
|
|
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
|
|
}
|
|
|
|
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
|
|
# 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
|
|
}
|
|
|
|
restart_ros_daemon_localhost() {
|
|
ros2 daemon stop >/dev/null 2>&1 || true
|
|
ros2 daemon start >/dev/null 2>&1 || true
|
|
}
|
|
|
|
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_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
|
|
|
|
source_workspace
|
|
restart_ros_daemon_localhost
|
|
start_can
|
|
|
|
echo "Starting linker_hand service..."
|
|
echo "Log: ${LOG_FILE}"
|
|
|
|
: > "${LOG_FILE}"
|
|
setsid bash -lc "export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST; 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}")"
|
|
start_bag_recorder
|
|
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_bag_recorder
|
|
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_bag_recorder
|
|
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_bag_recorder
|
|
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_bag_recorder
|
|
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
|
|
|
|
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'
|
|
else
|
|
echo "${can_if}: not found"
|
|
fi
|
|
done
|
|
}
|
|
|
|
case "${1:-}" in
|
|
start)
|
|
start_driver
|
|
;;
|
|
stop)
|
|
stop_driver
|
|
;;
|
|
status)
|
|
status_driver
|
|
;;
|
|
*)
|
|
usage
|
|
exit 1
|
|
;;
|
|
esac
|