277 lines
8.0 KiB
Bash
277 lines
8.0 KiB
Bash
|
#!/usr/bin/env bash
|
||
|
|
# Shared environment bootstrap for the open-source go2-nav2-amcl workspace.
|
||
|
|
# Source this from scripts that need ROS 2 + Unitree DDS + this workspace.
|
||
|
|
|
||
|
|
_go2_os_fail() {
|
||
|
|
printf '[go2_env] ERROR: %s\n' "$*" >&2
|
||
|
|
return 1 2>/dev/null || exit 1
|
||
|
|
}
|
||
|
|
|
||
|
|
_go2_os_script_dir="$(cd "$(dirname "${BASH_SOURCE[0]}")" && pwd)"
|
||
|
|
export GO2_REPO_ROOT="$(cd "${_go2_os_script_dir}/.." && pwd)"
|
||
|
|
|
||
|
|
if [[ -n "${1:-}" ]]; then
|
||
|
|
GO2_OS_ROLE="$1"
|
||
|
|
fi
|
||
|
|
_go2_os_restore_nounset=0
|
||
|
|
case "$-" in
|
||
|
|
*u*)
|
||
|
|
_go2_os_restore_nounset=1
|
||
|
|
set +u
|
||
|
|
;;
|
||
|
|
esac
|
||
|
|
|
||
|
|
_go2_runtime_env="${GO2_OS_RUNTIME_ENV:-${GO2_REPO_ROOT}/scripts/go2_runtime.env}"
|
||
|
|
if [[ -f "${_go2_runtime_env}" ]]; then
|
||
|
|
# shellcheck disable=SC1090
|
||
|
|
source "${_go2_runtime_env}"
|
||
|
|
fi
|
||
|
|
|
||
|
|
_go2_os_filter_old_go2_path_var() {
|
||
|
|
local var_name="$1"
|
||
|
|
local raw_value="${!var_name:-}"
|
||
|
|
local filtered=""
|
||
|
|
local part
|
||
|
|
[[ -n "${raw_value}" ]] || return 0
|
||
|
|
IFS=':' read -r -a _go2_os_path_parts <<< "${raw_value}"
|
||
|
|
for part in "${_go2_os_path_parts[@]}"; do
|
||
|
|
[[ -n "${part}" ]] || continue
|
||
|
|
if [[ "${part}" == *"/GO2/"* || "${part}" == *"/GO2" ]]; then
|
||
|
|
continue
|
||
|
|
fi
|
||
|
|
if [[ -z "${filtered}" ]]; then
|
||
|
|
filtered="${part}"
|
||
|
|
else
|
||
|
|
filtered="${filtered}:${part}"
|
||
|
|
fi
|
||
|
|
done
|
||
|
|
printf -v "${var_name}" '%s' "${filtered}"
|
||
|
|
export "${var_name}"
|
||
|
|
}
|
||
|
|
|
||
|
|
_go2_os_cyclonedds_uri() {
|
||
|
|
local iface="$1"
|
||
|
|
local peers="${2:-}"
|
||
|
|
local discovery_xml=""
|
||
|
|
local peer_xml=""
|
||
|
|
local peer
|
||
|
|
|
||
|
|
peers="${peers//,/ }"
|
||
|
|
for peer in ${peers}; do
|
||
|
|
[[ -n "${peer}" ]] || continue
|
||
|
|
peer_xml="${peer_xml}<Peer Address=\"${peer}\"/>"
|
||
|
|
done
|
||
|
|
|
||
|
|
if [[ -n "${peer_xml}" ]]; then
|
||
|
|
discovery_xml="<Discovery><Peers>${peer_xml}</Peers></Discovery>"
|
||
|
|
fi
|
||
|
|
|
||
|
|
printf '<CycloneDDS><Domain><General><Interfaces><NetworkInterface name="%s" priority="default" multicast="default" /></Interfaces></General>%s</Domain></CycloneDDS>' "${iface}" "${discovery_xml}"
|
||
|
|
}
|
||
|
|
|
||
|
|
_go2_os_iface_exists() {
|
||
|
|
local iface="$1"
|
||
|
|
[[ -n "${iface}" && -d "/sys/class/net/${iface}" ]]
|
||
|
|
}
|
||
|
|
|
||
|
|
_go2_os_iface_operstate() {
|
||
|
|
local iface="$1"
|
||
|
|
cat "/sys/class/net/${iface}/operstate" 2>/dev/null || true
|
||
|
|
}
|
||
|
|
|
||
|
|
_go2_os_iface_carrier() {
|
||
|
|
local iface="$1"
|
||
|
|
cat "/sys/class/net/${iface}/carrier" 2>/dev/null || true
|
||
|
|
}
|
||
|
|
|
||
|
|
_go2_os_iface_ipv4() {
|
||
|
|
local iface="$1"
|
||
|
|
ip -4 -o addr show dev "${iface}" 2>/dev/null | awk '{print $4}' | cut -d/ -f1 | head -n 1
|
||
|
|
}
|
||
|
|
|
||
|
|
_go2_os_iface_ready() {
|
||
|
|
local iface="$1"
|
||
|
|
local require_ipv4="${2:-1}"
|
||
|
|
local state=""
|
||
|
|
local carrier=""
|
||
|
|
local ipv4=""
|
||
|
|
_go2_os_iface_exists "${iface}" || return 1
|
||
|
|
state="$(_go2_os_iface_operstate "${iface}")"
|
||
|
|
[[ "${state}" == "up" ]] || return 1
|
||
|
|
carrier="$(_go2_os_iface_carrier "${iface}")"
|
||
|
|
if [[ -n "${carrier}" && "${carrier}" != "1" ]]; then
|
||
|
|
return 1
|
||
|
|
fi
|
||
|
|
if [[ "${require_ipv4}" == "1" ]]; then
|
||
|
|
ipv4="$(_go2_os_iface_ipv4 "${iface}")"
|
||
|
|
[[ -n "${ipv4}" ]] || return 1
|
||
|
|
fi
|
||
|
|
return 0
|
||
|
|
}
|
||
|
|
|
||
|
|
_go2_os_iface_status_text() {
|
||
|
|
local iface="$1"
|
||
|
|
if ! _go2_os_iface_exists "${iface}"; then
|
||
|
|
printf 'missing'
|
||
|
|
return
|
||
|
|
fi
|
||
|
|
local state carrier ipv4
|
||
|
|
state="$(_go2_os_iface_operstate "${iface}")"
|
||
|
|
carrier="$(_go2_os_iface_carrier "${iface}")"
|
||
|
|
ipv4="$(_go2_os_iface_ipv4 "${iface}")"
|
||
|
|
printf 'state=%s carrier=%s ipv4=%s' "${state:-unknown}" "${carrier:-n/a}" "${ipv4:-none}"
|
||
|
|
}
|
||
|
|
|
||
|
|
_go2_os_role_transport_policy() {
|
||
|
|
if [[ "${GO2_OS_FORCE_WIRED:-0}" == "1" ]]; then
|
||
|
|
printf 'wired_required'
|
||
|
|
return
|
||
|
|
fi
|
||
|
|
case "${GO2_OS_ROLE}" in
|
||
|
|
mapping|map|slam|save_map|teleop|build)
|
||
|
|
printf 'wired_required'
|
||
|
|
;;
|
||
|
|
dashboard|nav|navigation|patrol|inspection)
|
||
|
|
printf 'wired_required'
|
||
|
|
;;
|
||
|
|
*)
|
||
|
|
printf 'wired_required'
|
||
|
|
;;
|
||
|
|
esac
|
||
|
|
}
|
||
|
|
|
||
|
|
_go2_os_select_iface() {
|
||
|
|
local policy="$1"
|
||
|
|
local selected_iface=""
|
||
|
|
local selected_reason=""
|
||
|
|
local fallback_active=0
|
||
|
|
|
||
|
|
case "${policy}" in
|
||
|
|
wired_required)
|
||
|
|
if _go2_os_iface_ready "${GO2_WIRED_IFACE}" 1; then
|
||
|
|
selected_iface="${GO2_WIRED_IFACE}"
|
||
|
|
selected_reason="wired_required:${GO2_WIRED_IFACE}"
|
||
|
|
else
|
||
|
|
GO2_NETWORK_ERROR="wired_required:${GO2_WIRED_IFACE}:$(_go2_os_iface_status_text "${GO2_WIRED_IFACE}")"
|
||
|
|
export GO2_NETWORK_ERROR
|
||
|
|
return 1
|
||
|
|
fi
|
||
|
|
;;
|
||
|
|
wireless_preferred)
|
||
|
|
if _go2_os_iface_ready "${GO2_WIRELESS_IFACE}" 1; then
|
||
|
|
selected_iface="${GO2_WIRELESS_IFACE}"
|
||
|
|
selected_reason="wireless_selected:${GO2_WIRELESS_IFACE}"
|
||
|
|
elif _go2_os_iface_ready "${GO2_WIRED_IFACE}" 1; then
|
||
|
|
selected_iface="${GO2_WIRED_IFACE}"
|
||
|
|
selected_reason="wireless_unavailable_using_wired_fallback:${GO2_WIRELESS_IFACE}:${GO2_WIRED_IFACE}"
|
||
|
|
fallback_active=1
|
||
|
|
else
|
||
|
|
GO2_NETWORK_ERROR="robot_ros_network_unavailable:${GO2_WIRELESS_IFACE}:${GO2_WIRED_IFACE}"
|
||
|
|
export GO2_NETWORK_ERROR
|
||
|
|
return 1
|
||
|
|
fi
|
||
|
|
;;
|
||
|
|
*)
|
||
|
|
GO2_NETWORK_ERROR="unknown_network_policy:${policy}"
|
||
|
|
export GO2_NETWORK_ERROR
|
||
|
|
return 1
|
||
|
|
;;
|
||
|
|
esac
|
||
|
|
|
||
|
|
GO2_OS_SELECTED_IFACE="${selected_iface}"
|
||
|
|
GO2_OS_SELECTED_IFACE_REASON="${selected_reason}"
|
||
|
|
GO2_OS_FALLBACK_ACTIVE="${fallback_active}"
|
||
|
|
export GO2_OS_SELECTED_IFACE GO2_OS_SELECTED_IFACE_REASON GO2_OS_FALLBACK_ACTIVE
|
||
|
|
return 0
|
||
|
|
}
|
||
|
|
|
||
|
|
for _go2_os_path_var in AMENT_PREFIX_PATH CMAKE_PREFIX_PATH COLCON_PREFIX_PATH PYTHONPATH LD_LIBRARY_PATH PATH; do
|
||
|
|
_go2_os_filter_old_go2_path_var "${_go2_os_path_var}"
|
||
|
|
done
|
||
|
|
|
||
|
|
: "${GO2_OS_ROLE:=generic}"
|
||
|
|
: "${GO2_WIRED_IFACE:=ens160}"
|
||
|
|
: "${GO2_WIRELESS_IFACE:=ens192}"
|
||
|
|
: "${GO2_OS_MAPPING_IFACE:=${GO2_WIRED_IFACE}}"
|
||
|
|
: "${GO2_OS_NAV_IFACE:=${GO2_WIRED_IFACE}}"
|
||
|
|
: "${GO2_OS_DASHBOARD_IFACE:=${GO2_WIRED_IFACE}}"
|
||
|
|
: "${GO2_OS_MAPPING_PEERS:=}"
|
||
|
|
: "${GO2_OS_NAV_PEERS:=}"
|
||
|
|
: "${GO2_OS_DASHBOARD_PEERS:=${GO2_OS_NAV_PEERS}}"
|
||
|
|
: "${GO2_UNITREE_SETUP:=${HOME}/unitree_ros2/cyclonedds_ws/install/setup.bash}"
|
||
|
|
: "${GO2_OS_MAP_DIR:=${HOME}/go2_maps}"
|
||
|
|
: "${GO2_OS_MAP_NAME:=competition_map}"
|
||
|
|
|
||
|
|
GO2_TRANSPORT_POLICY="$(_go2_os_role_transport_policy)"
|
||
|
|
export GO2_TRANSPORT_POLICY
|
||
|
|
unset GO2_NETWORK_ERROR
|
||
|
|
unset GO2_OS_SELECTED_IFACE
|
||
|
|
unset GO2_OS_SELECTED_IFACE_REASON
|
||
|
|
unset GO2_OS_FALLBACK_ACTIVE
|
||
|
|
|
||
|
|
if [[ -z "${GO2_ROS_IFACE:-}" ]]; then
|
||
|
|
_go2_os_select_iface "${GO2_TRANSPORT_POLICY}" || _go2_os_fail "${GO2_NETWORK_ERROR}"
|
||
|
|
GO2_ROS_IFACE="${GO2_OS_SELECTED_IFACE}"
|
||
|
|
fi
|
||
|
|
|
||
|
|
if [[ -z "${GO2_DDS_PEERS:-}" ]]; then
|
||
|
|
case "${GO2_OS_ROLE}" in
|
||
|
|
mapping|map|slam|save_map|teleop|build)
|
||
|
|
GO2_DDS_PEERS="${GO2_OS_MAPPING_PEERS}"
|
||
|
|
;;
|
||
|
|
dashboard)
|
||
|
|
GO2_DDS_PEERS="${GO2_OS_DASHBOARD_PEERS}"
|
||
|
|
;;
|
||
|
|
nav|navigation|patrol|inspection)
|
||
|
|
GO2_DDS_PEERS="${GO2_OS_NAV_PEERS}"
|
||
|
|
;;
|
||
|
|
*)
|
||
|
|
GO2_DDS_PEERS="${GO2_OS_MAPPING_PEERS}"
|
||
|
|
;;
|
||
|
|
esac
|
||
|
|
fi
|
||
|
|
|
||
|
|
[[ -f /opt/ros/humble/setup.bash ]] || _go2_os_fail "missing /opt/ros/humble/setup.bash"
|
||
|
|
[[ -f "${GO2_UNITREE_SETUP}" ]] || _go2_os_fail "missing Unitree setup: ${GO2_UNITREE_SETUP}"
|
||
|
|
[[ -d "/sys/class/net/${GO2_ROS_IFACE}" ]] || _go2_os_fail "network interface not found: ${GO2_ROS_IFACE}"
|
||
|
|
|
||
|
|
_go2_os_iface_state="$(_go2_os_iface_operstate "${GO2_ROS_IFACE}")"
|
||
|
|
if [[ "${_go2_os_iface_state}" != "up" && "${GO2_ALLOW_IFACE_DOWN:-0}" != "1" ]]; then
|
||
|
|
_go2_os_fail "network interface ${GO2_ROS_IFACE} is ${_go2_os_iface_state:-unknown}; connect the Go2 network first"
|
||
|
|
fi
|
||
|
|
|
||
|
|
# shellcheck disable=SC1091
|
||
|
|
source /opt/ros/humble/setup.bash
|
||
|
|
# shellcheck disable=SC1090
|
||
|
|
source "${GO2_UNITREE_SETUP}"
|
||
|
|
|
||
|
|
export RMW_IMPLEMENTATION=rmw_cyclonedds_cpp
|
||
|
|
export CYCLONEDDS_URI="$(_go2_os_cyclonedds_uri "${GO2_ROS_IFACE}" "${GO2_DDS_PEERS}")"
|
||
|
|
export GO2_OS_ROLE
|
||
|
|
export GO2_WIRED_IFACE
|
||
|
|
export GO2_WIRELESS_IFACE
|
||
|
|
export GO2_OS_MAPPING_IFACE
|
||
|
|
export GO2_OS_NAV_IFACE
|
||
|
|
export GO2_OS_DASHBOARD_IFACE
|
||
|
|
export GO2_OS_MAPPING_PEERS
|
||
|
|
export GO2_OS_NAV_PEERS
|
||
|
|
export GO2_OS_DASHBOARD_PEERS
|
||
|
|
export GO2_DDS_PEERS
|
||
|
|
export GO2_ROS_IFACE
|
||
|
|
export GO2_OS_MAP_DIR
|
||
|
|
export GO2_OS_MAP_NAME
|
||
|
|
|
||
|
|
if [[ "${GO2_OS_SKIP_WORKSPACE_SETUP:-0}" != "1" && -f "${GO2_REPO_ROOT}/install/setup.bash" ]]; then
|
||
|
|
# shellcheck disable=SC1091
|
||
|
|
source "${GO2_REPO_ROOT}/install/setup.bash"
|
||
|
|
fi
|
||
|
|
|
||
|
|
if [[ "${_go2_os_restore_nounset}" == "1" ]]; then
|
||
|
|
set -u
|
||
|
|
fi
|
||
|
|
|
||
|
|
unset _go2_os_script_dir _go2_runtime_env _go2_os_iface_state _go2_os_restore_nounset
|
||
|
|
unset _go2_os_path_var _go2_os_path_parts
|
||
|
|
unset -f _go2_os_filter_old_go2_path_var
|
||
|
|
unset -f _go2_os_cyclonedds_uri
|