136 lines
4.1 KiB
Bash
136 lines
4.1 KiB
Bash
#!/usr/bin/env bash
|
|
set -euo pipefail
|
|
|
|
SCRIPT_DIR="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
|
# shellcheck disable=SC1091
|
|
source "${SCRIPT_DIR}/load-env.sh"
|
|
|
|
if [[ -f "/opt/ros/${ROS_DISTRO}/setup.bash" ]]; then
|
|
# shellcheck disable=SC1091
|
|
set +u
|
|
source "/opt/ros/${ROS_DISTRO}/setup.bash"
|
|
set -u
|
|
fi
|
|
if [[ -f "${HOME}/xos/setup.bash" ]]; then
|
|
# shellcheck disable=SC1091
|
|
set +u
|
|
source "${HOME}/xos/setup.bash"
|
|
set -u
|
|
fi
|
|
|
|
service_active() {
|
|
systemctl is-active --quiet "$1"
|
|
}
|
|
|
|
start_service_if_allowed() {
|
|
local service="$1"
|
|
local auto_start="${2:-0}"
|
|
|
|
if service_active "${service}"; then
|
|
echo "[ros-camera-services] ${service} is active" >&2
|
|
return 0
|
|
fi
|
|
if [[ "${auto_start}" != "1" ]]; then
|
|
echo "[ros-camera-services] ${service} is inactive; automatic start is disabled" >&2
|
|
return 1
|
|
fi
|
|
echo "[ros-camera-services] starting ${service}" >&2
|
|
sudo systemctl start "${service}"
|
|
service_active "${service}"
|
|
}
|
|
|
|
topic_exists() {
|
|
local topic="$1"
|
|
ros2 topic list 2>/dev/null | grep -Fxq "${topic}"
|
|
}
|
|
|
|
wait_for_topics() {
|
|
local timeout_sec="${1:-20}"
|
|
local deadline=$((SECONDS + timeout_sec))
|
|
|
|
while (( SECONDS < deadline )); do
|
|
if topic_exists "${OMNI_ROS2_HEAD_RGB_TOPIC}" \
|
|
&& topic_exists "${OMNI_ROS2_WAIST_RGB_TOPIC}"; then
|
|
echo "[ros-camera-services] RGB topics are available" >&2
|
|
return 0
|
|
fi
|
|
sleep 1
|
|
done
|
|
return 1
|
|
}
|
|
|
|
camera_devices_are_free() {
|
|
local device
|
|
local busy=0
|
|
|
|
if ! command -v fuser >/dev/null 2>&1; then
|
|
echo "[ros-camera-services] fuser is required to verify camera ownership" >&2
|
|
return 1
|
|
fi
|
|
for device in "${OMNI_CAMERA_HEAD_DEVICE}" "${OMNI_CAMERA_WAIST_DEVICE}"; do
|
|
if [[ ! -e "${device}" ]]; then
|
|
echo "[ros-camera-services] camera device is missing: ${device}" >&2
|
|
busy=1
|
|
continue
|
|
fi
|
|
if fuser "${device}" >/dev/null 2>&1; then
|
|
echo "[ros-camera-services] camera device is busy: ${device}" >&2
|
|
fuser -v "${device}" 2>&1 || true
|
|
busy=1
|
|
fi
|
|
done
|
|
return "${busy}"
|
|
}
|
|
|
|
if ! command -v systemctl >/dev/null 2>&1; then
|
|
echo "[ros-camera-services] systemctl is required on the robot" >&2
|
|
exit 1
|
|
fi
|
|
if ! command -v ros2 >/dev/null 2>&1; then
|
|
echo "[ros-camera-services] ros2 is not in PATH; source the ROS2 and robot setup files first" >&2
|
|
exit 1
|
|
fi
|
|
|
|
if ! service_active "${OMNI_PROC_MANAGER_SERVICE}"; then
|
|
echo "[ros-camera-services] ${OMNI_PROC_MANAGER_SERVICE} is not active" >&2
|
|
if [[ "${OMNI_PROC_MANAGER_AUTO_START:-0}" == "1" ]]; then
|
|
start_service_if_allowed "${OMNI_PROC_MANAGER_SERVICE}" "${OMNI_PROC_MANAGER_AUTO_START}"
|
|
else
|
|
echo "[ros-camera-services] proc_manager auto-start disabled; using the explicit camera-service fallback if allowed" >&2
|
|
fi
|
|
fi
|
|
|
|
# proc_manager owns the ROS camera components when it is active. Never start
|
|
# separate camera services on top of it: that would create a second device
|
|
# owner and reproduce EBUSY. The individual services are only a fallback when
|
|
# proc_manager is inactive and explicitly allowed by the environment.
|
|
if wait_for_topics 20; then
|
|
exit 0
|
|
fi
|
|
|
|
if service_active "${OMNI_PROC_MANAGER_SERVICE}"; then
|
|
echo "[ros-camera-services] proc_manager is active but the expected RGB topics are missing" >&2
|
|
if ! camera_devices_are_free; then
|
|
echo "[ros-camera-services] refusing to start ${OMNI_CAMERA_HEAD_SERVICE}/${OMNI_CAMERA_WAIST_SERVICE}: a camera device is already owned" >&2
|
|
exit 1
|
|
fi
|
|
echo "[ros-camera-services] proc_manager does not own the head/waist devices; starting the configured camera services" >&2
|
|
fi
|
|
|
|
if [[ "${OMNI_CAMERA_START_SERVICES:-1}" != "1" ]]; then
|
|
echo "[ros-camera-services] individual camera service fallback is disabled" >&2
|
|
exit 1
|
|
fi
|
|
|
|
if ! camera_devices_are_free; then
|
|
echo "[ros-camera-services] refusing to start camera services while a device is busy" >&2
|
|
exit 1
|
|
fi
|
|
|
|
start_service_if_allowed "${OMNI_CAMERA_HEAD_SERVICE}" "${OMNI_CAMERA_START_SERVICES:-1}"
|
|
start_service_if_allowed "${OMNI_CAMERA_WAIST_SERVICE}" "${OMNI_CAMERA_START_SERVICES:-1}"
|
|
if ! wait_for_topics 20; then
|
|
echo "[ros-camera-services] camera services started but RGB topics did not appear" >&2
|
|
exit 1
|
|
fi
|