1061 lines
37 KiB
Bash
Executable File
1061 lines
37 KiB
Bash
Executable File
#!/usr/bin/env bash
|
||
|
||
# CMVR ES remote motion smoke tests.
|
||
#
|
||
# Safety properties:
|
||
# * Motion is disabled unless --execute is supplied.
|
||
# * The current device state is read immediately before every motion group.
|
||
# * Arm tests change J6 only or Cartesian Z only.
|
||
# * AGV tests use low speeds and short distances.
|
||
# * Arm StopAll tests require observed J6 displacement and velocity before
|
||
# issuing StopAll; submitting a command alone is not considered a pass.
|
||
# * Interactive confirmation is required before each motion group unless
|
||
# --yes is supplied explicitly.
|
||
# * StopAll tests are separate because the current server implementation
|
||
# calls DeviceManager::stop() and leaves all devices stopped.
|
||
|
||
set -Eeuo pipefail
|
||
|
||
ENDPOINT="${ENDPOINT:-192.168.0.28:50052}"
|
||
ARM_ID="${ARM_ID:-aubo_arm}"
|
||
AGV_ID="${AGV_ID:-src1100}"
|
||
GRPCURL_BIN="${GRPCURL_BIN:-grpcurl}"
|
||
|
||
# Conservative defaults. Arm angles are radians; Cartesian/AGV distances are
|
||
# metres; angular rates are radians per second.
|
||
ARM_J6_DELTA_RAD="${ARM_J6_DELTA_RAD:-0.01}"
|
||
ARM_STOP_J6_DELTA_RAD="${ARM_STOP_J6_DELTA_RAD:-0.03}"
|
||
ARM_JOINT_VELOCITY="${ARM_JOINT_VELOCITY:-0.05}"
|
||
ARM_STOP_JOINT_VELOCITY="${ARM_STOP_JOINT_VELOCITY:-0.01}"
|
||
ARM_JOINT_ACCELERATION="${ARM_JOINT_ACCELERATION:-0.10}"
|
||
ARM_STATIONARY_VELOCITY_RAD_S="${ARM_STATIONARY_VELOCITY_RAD_S:-0.005}"
|
||
ARM_Z_DELTA_M="${ARM_Z_DELTA_M:-0.005}"
|
||
ARM_LINEAR_VELOCITY="${ARM_LINEAR_VELOCITY:-0.01}"
|
||
ARM_LINEAR_ACCELERATION="${ARM_LINEAR_ACCELERATION:-0.02}"
|
||
ARM_SPEED_DURATION_S="${ARM_SPEED_DURATION_S:-0.25}"
|
||
ARM_BASE_FRAME="${ARM_BASE_FRAME:-}"
|
||
ARM_TCP_FRAME="${ARM_TCP_FRAME:-}"
|
||
ARM_MOTION_DETECT_TIMEOUT_S="${ARM_MOTION_DETECT_TIMEOUT_S:-3.0}"
|
||
ARM_MOTION_POLL_INTERVAL_S="${ARM_MOTION_POLL_INTERVAL_S:-0.05}"
|
||
ARM_MOTION_POSITION_THRESHOLD_RAD="${ARM_MOTION_POSITION_THRESHOLD_RAD:-0.0005}"
|
||
ARM_MOTION_VELOCITY_THRESHOLD_RAD_S="${ARM_MOTION_VELOCITY_THRESHOLD_RAD_S:-0.001}"
|
||
|
||
AGV_POSE_DELTA_M="${AGV_POSE_DELTA_M:-0.05}"
|
||
AGV_NAV_SPEED="${AGV_NAV_SPEED:-0.03}"
|
||
AGV_NAV_ACCELERATION="${AGV_NAV_ACCELERATION:-0.05}"
|
||
AGV_BODY_SPEED="${AGV_BODY_SPEED:-0.02}"
|
||
AGV_VELOCITY_DURATION_S="${AGV_VELOCITY_DURATION_S:-0.75}"
|
||
AGV_TRANSLATE_DISTANCE_M="${AGV_TRANSLATE_DISTANCE_M:-0.03}"
|
||
AGV_TEST_STATION="${AGV_TEST_STATION:-}"
|
||
AGV_PATH_SOURCE="${AGV_PATH_SOURCE:-}"
|
||
AGV_PATH_TARGET="${AGV_PATH_TARGET:-}"
|
||
|
||
STOP_DELAY_S="${STOP_DELAY_S:-0.50}"
|
||
|
||
EXECUTE=0
|
||
ASSUME_YES=0
|
||
USE_TLS=0
|
||
COMMAND="status"
|
||
LAST_RESPONSE=""
|
||
MOTION_ACTIVE=0
|
||
STOP_ALL_USED=0
|
||
BG_PID=""
|
||
BG_OUTPUT=""
|
||
BACKGROUND_RESPONSE=""
|
||
QUIET_RESPONSE=""
|
||
|
||
usage() {
|
||
cat <<'EOF'
|
||
Usage:
|
||
tools/grpc_remote_motion_test.sh [options] <command>
|
||
|
||
Commands:
|
||
status Read system, arm, frame and AGV state (default).
|
||
arm-motions Test moveJ/moveL/speedJ/speedL/servoJ/stopMotion.
|
||
actionqueue Test an ActionQueue containing MoveJ and, when a
|
||
trustworthy TCP pose is available, MoveL.
|
||
agv-motions Test pose navigation, velocity, translate,
|
||
pause/resume/cancel and optional station/path APIs.
|
||
all-safe Run status, arm-motions, actionqueue, agv-motions.
|
||
StopAll tests are deliberately excluded.
|
||
arm-stopall Observe a slow J6 MoveJ, then call StopAll.
|
||
actionqueue-stopall Observe the ActionQueue moving J6, then call StopAll.
|
||
agv-stopall Call StopAll during low-speed AGV velocity motion.
|
||
|
||
Options:
|
||
--endpoint HOST:PORT gRPC endpoint (default: 192.168.0.28:50052).
|
||
--arm-id ID Arm device id (default: aubo_arm).
|
||
--agv-id ID AGV device id (default: src1100).
|
||
--execute Actually send motion requests. Without this option,
|
||
state is read and planned requests are printed only.
|
||
--yes Skip interactive MOVE confirmation. Requires
|
||
--execute; use only with a physically supervised rig.
|
||
--tls Use TLS. The default is grpcurl -plaintext.
|
||
-h, --help Show this help.
|
||
|
||
Optional AGV station/path tests:
|
||
AGV_TEST_STATION=<id>
|
||
AGV_PATH_SOURCE=<id> AGV_PATH_TARGET=<id>
|
||
|
||
Optional named frames for MoveL:
|
||
ARM_BASE_FRAME=<name> ARM_TCP_FRAME=<name>
|
||
|
||
Optional arm motion-detection tuning for StopAll tests:
|
||
ARM_MOTION_DETECT_TIMEOUT_S=<seconds>
|
||
ARM_MOTION_POLL_INTERVAL_S=<seconds>
|
||
ARM_MOTION_POSITION_THRESHOLD_RAD=<radians>
|
||
ARM_MOTION_VELOCITY_THRESHOLD_RAD_S=<radians-per-second>
|
||
|
||
Examples:
|
||
# Read-only connectivity and state check
|
||
tools/grpc_remote_motion_test.sh status
|
||
|
||
# Preview all non-StopAll requests, without moving
|
||
tools/grpc_remote_motion_test.sh all-safe
|
||
|
||
# Execute the ordinary motion tests, with confirmations
|
||
tools/grpc_remote_motion_test.sh --execute arm-motions
|
||
|
||
# Each StopAll test must be run separately. Restart cmvr_es afterwards.
|
||
tools/grpc_remote_motion_test.sh --execute arm-stopall
|
||
EOF
|
||
}
|
||
|
||
log() {
|
||
printf '\n[%s] %s\n' "$(date '+%H:%M:%S')" "$*"
|
||
}
|
||
|
||
warn() {
|
||
printf '\nWARNING: %s\n' "$*" >&2
|
||
}
|
||
|
||
die() {
|
||
printf '\nERROR: %s\n' "$*" >&2
|
||
exit 1
|
||
}
|
||
|
||
while (($#)); do
|
||
case "$1" in
|
||
--endpoint)
|
||
(($# >= 2)) || die "--endpoint requires HOST:PORT"
|
||
ENDPOINT="$2"
|
||
shift 2
|
||
;;
|
||
--arm-id)
|
||
(($# >= 2)) || die "--arm-id requires a device id"
|
||
ARM_ID="$2"
|
||
shift 2
|
||
;;
|
||
--agv-id)
|
||
(($# >= 2)) || die "--agv-id requires a device id"
|
||
AGV_ID="$2"
|
||
shift 2
|
||
;;
|
||
--execute)
|
||
EXECUTE=1
|
||
shift
|
||
;;
|
||
--yes)
|
||
ASSUME_YES=1
|
||
shift
|
||
;;
|
||
--tls)
|
||
USE_TLS=1
|
||
shift
|
||
;;
|
||
-h|--help)
|
||
usage
|
||
exit 0
|
||
;;
|
||
status|arm-motions|actionqueue|agv-motions|all-safe|arm-stopall|actionqueue-stopall|agv-stopall)
|
||
COMMAND="$1"
|
||
shift
|
||
;;
|
||
*)
|
||
die "unknown argument: $1 (use --help)"
|
||
;;
|
||
esac
|
||
done
|
||
|
||
if ((ASSUME_YES && !EXECUTE)); then
|
||
die "--yes is meaningful only together with --execute"
|
||
fi
|
||
|
||
command -v "$GRPCURL_BIN" >/dev/null 2>&1 || die "grpcurl not found: $GRPCURL_BIN"
|
||
command -v jq >/dev/null 2>&1 || die "jq is required"
|
||
command -v awk >/dev/null 2>&1 || die "awk is required"
|
||
|
||
require_positive_bounded() {
|
||
local name="$1"
|
||
local value="$2"
|
||
local maximum="$3"
|
||
awk -v v="$value" -v max="$maximum" '
|
||
BEGIN { exit !(v ~ /^[0-9]+([.][0-9]+)?$/ && v > 0 && v <= max) }
|
||
' || die "$name must be numeric, > 0 and <= $maximum (actual: $value)"
|
||
}
|
||
|
||
# Keep environment overrides inside a smoke-test envelope. A typo such as
|
||
# 5 instead of 0.005 must not turn a diagnostic script into a large move.
|
||
require_positive_bounded ARM_J6_DELTA_RAD "$ARM_J6_DELTA_RAD" 0.03
|
||
require_positive_bounded ARM_STOP_J6_DELTA_RAD "$ARM_STOP_J6_DELTA_RAD" 0.05
|
||
require_positive_bounded ARM_JOINT_VELOCITY "$ARM_JOINT_VELOCITY" 0.10
|
||
require_positive_bounded ARM_STOP_JOINT_VELOCITY "$ARM_STOP_JOINT_VELOCITY" 0.05
|
||
require_positive_bounded ARM_JOINT_ACCELERATION "$ARM_JOINT_ACCELERATION" 0.30
|
||
require_positive_bounded ARM_STATIONARY_VELOCITY_RAD_S "$ARM_STATIONARY_VELOCITY_RAD_S" 0.02
|
||
require_positive_bounded ARM_Z_DELTA_M "$ARM_Z_DELTA_M" 0.01
|
||
require_positive_bounded ARM_LINEAR_VELOCITY "$ARM_LINEAR_VELOCITY" 0.03
|
||
require_positive_bounded ARM_LINEAR_ACCELERATION "$ARM_LINEAR_ACCELERATION" 0.10
|
||
require_positive_bounded ARM_SPEED_DURATION_S "$ARM_SPEED_DURATION_S" 0.50
|
||
require_positive_bounded ARM_MOTION_DETECT_TIMEOUT_S "$ARM_MOTION_DETECT_TIMEOUT_S" 10.00
|
||
require_positive_bounded ARM_MOTION_POLL_INTERVAL_S "$ARM_MOTION_POLL_INTERVAL_S" 0.50
|
||
require_positive_bounded ARM_MOTION_POSITION_THRESHOLD_RAD "$ARM_MOTION_POSITION_THRESHOLD_RAD" 0.01
|
||
require_positive_bounded ARM_MOTION_VELOCITY_THRESHOLD_RAD_S "$ARM_MOTION_VELOCITY_THRESHOLD_RAD_S" 0.02
|
||
require_positive_bounded AGV_POSE_DELTA_M "$AGV_POSE_DELTA_M" 0.05
|
||
require_positive_bounded AGV_NAV_SPEED "$AGV_NAV_SPEED" 0.05
|
||
require_positive_bounded AGV_NAV_ACCELERATION "$AGV_NAV_ACCELERATION" 0.10
|
||
require_positive_bounded AGV_BODY_SPEED "$AGV_BODY_SPEED" 0.03
|
||
require_positive_bounded AGV_VELOCITY_DURATION_S "$AGV_VELOCITY_DURATION_S" 1.00
|
||
require_positive_bounded AGV_TRANSLATE_DISTANCE_M "$AGV_TRANSLATE_DISTANCE_M" 0.05
|
||
require_positive_bounded STOP_DELAY_S "$STOP_DELAY_S" 2.00
|
||
|
||
GRPC_ARGS=()
|
||
if ((!USE_TLS)); then
|
||
GRPC_ARGS+=(-plaintext)
|
||
fi
|
||
|
||
pretty_json_or_raw() {
|
||
local input="$1"
|
||
if jq . >/dev/null 2>&1 <<<"$input"; then
|
||
jq . <<<"$input"
|
||
else
|
||
printf '%s\n' "$input"
|
||
fi
|
||
}
|
||
|
||
rpc() {
|
||
local method="$1"
|
||
local payload="$2"
|
||
local max_time="${3:-10}"
|
||
local output
|
||
local rc
|
||
|
||
log "RPC $method"
|
||
printf 'request: '
|
||
jq -c . <<<"$payload"
|
||
|
||
set +e
|
||
output=$("$GRPCURL_BIN" "${GRPC_ARGS[@]}" -max-time "$max_time" \
|
||
-d "$payload" "$ENDPOINT" "$method" 2>&1)
|
||
rc=$?
|
||
set -e
|
||
|
||
LAST_RESPONSE="$output"
|
||
printf 'response:\n'
|
||
pretty_json_or_raw "$output"
|
||
return "$rc"
|
||
}
|
||
|
||
response_is_success() {
|
||
jq -e '
|
||
if has("header") then (.header.success == true)
|
||
else (.success == true)
|
||
end
|
||
' >/dev/null 2>&1 <<<"$LAST_RESPONSE"
|
||
}
|
||
|
||
rpc_expect_success() {
|
||
local method="$1"
|
||
local payload="$2"
|
||
local max_time="${3:-10}"
|
||
if ! rpc "$method" "$payload" "$max_time"; then
|
||
return 1
|
||
fi
|
||
response_is_success
|
||
}
|
||
|
||
rpc_quiet() {
|
||
local method="$1"
|
||
local payload="$2"
|
||
local max_time="${3:-2}"
|
||
local rc
|
||
|
||
set +e
|
||
QUIET_RESPONSE=$("$GRPCURL_BIN" "${GRPC_ARGS[@]}" -max-time "$max_time" \
|
||
-d "$payload" "$ENDPOINT" "$method" 2>&1)
|
||
rc=$?
|
||
set -e
|
||
return "$rc"
|
||
}
|
||
|
||
read_arm_joint_state_quiet() {
|
||
local payload
|
||
payload=$(jq -cn --arg id "$ARM_ID" '{header:{deviceId:$id}}')
|
||
rpc_quiet "cmvr.api.ArmService/getJointState" "$payload" 1 || return 1
|
||
jq -e '
|
||
(.header.success == true) and
|
||
(.state.position | type == "array") and
|
||
(.state.position | length == 6) and
|
||
(.state.velocity | type == "array") and
|
||
(.state.velocity | length == 6)
|
||
' >/dev/null 2>&1 <<<"$QUIET_RESPONSE"
|
||
}
|
||
|
||
wait_for_arm_j6_motion() {
|
||
local initial_position="$1"
|
||
local current_position current_velocity
|
||
local deadline_ms timeout_ms
|
||
|
||
timeout_ms=$(awk -v timeout="$ARM_MOTION_DETECT_TIMEOUT_S" \
|
||
'BEGIN { printf "%.0f", timeout * 1000 }')
|
||
deadline_ms=$(($(date +%s%3N) + timeout_ms))
|
||
|
||
while (( $(date +%s%3N) < deadline_ms )); do
|
||
if [[ -n "$BG_PID" ]] && ! kill -0 "$BG_PID" 2>/dev/null; then
|
||
warn "background motion RPC ended before J6 motion was observed"
|
||
return 2
|
||
fi
|
||
|
||
if read_arm_joint_state_quiet; then
|
||
current_position=$(jq -er '.state.position[5]' <<<"$QUIET_RESPONSE") || true
|
||
current_velocity=$(jq -er '.state.velocity[5]' <<<"$QUIET_RESPONSE") || true
|
||
if [[ -n "$current_position" && -n "$current_velocity" ]] && \
|
||
awk \
|
||
-v start="$initial_position" \
|
||
-v position="$current_position" \
|
||
-v velocity="$current_velocity" \
|
||
-v position_threshold="$ARM_MOTION_POSITION_THRESHOLD_RAD" \
|
||
-v velocity_threshold="$ARM_MOTION_VELOCITY_THRESHOLD_RAD_S" '
|
||
BEGIN {
|
||
displacement = position - start
|
||
if (displacement < 0) displacement = -displacement
|
||
speed = velocity
|
||
if (speed < 0) speed = -speed
|
||
exit !(displacement >= position_threshold &&
|
||
speed >= velocity_threshold)
|
||
}
|
||
'; then
|
||
log "confirmed J6 is moving: displacement=$(awk -v a="$initial_position" -v b="$current_position" 'BEGIN { d=b-a; if (d<0) d=-d; printf "%.6f", d }') rad, velocity=${current_velocity} rad/s"
|
||
return 0
|
||
fi
|
||
fi
|
||
sleep "$ARM_MOTION_POLL_INTERVAL_S"
|
||
done
|
||
|
||
warn "J6 motion was not observed within ${ARM_MOTION_DETECT_TIMEOUT_S}s"
|
||
return 1
|
||
}
|
||
|
||
print_plan() {
|
||
local label="$1"
|
||
local method="$2"
|
||
local payload="$3"
|
||
log "DRY RUN: $label"
|
||
printf 'method: %s\nrequest:\n' "$method"
|
||
jq . <<<"$payload"
|
||
}
|
||
|
||
confirm_motion() {
|
||
local label="$1"
|
||
local detail="$2"
|
||
local answer
|
||
|
||
if ((!EXECUTE)); then
|
||
return 0
|
||
fi
|
||
if ((ASSUME_YES)); then
|
||
warn "--yes active; executing: $label"
|
||
return 0
|
||
fi
|
||
|
||
printf '\n============================================================\n' >&2
|
||
printf '即将执行: %s\n%s\n' "$label" "$detail" >&2
|
||
printf '请确认：现场无人、机械臂/底盘周围无障碍物，急停按钮可立即触达。\n' >&2
|
||
printf '输入 MOVE 继续，其他输入取消: ' >&2
|
||
read -r answer
|
||
[[ "$answer" == "MOVE" ]] || die "operator canceled: $label"
|
||
}
|
||
|
||
run_control() {
|
||
local label="$1"
|
||
local method="$2"
|
||
local payload="$3"
|
||
local max_time="${4:-30}"
|
||
|
||
if ((!EXECUTE)); then
|
||
print_plan "$label" "$method" "$payload"
|
||
return 0
|
||
fi
|
||
|
||
MOTION_ACTIVE=1
|
||
if ! rpc_expect_success "$method" "$payload" "$max_time"; then
|
||
warn "$label failed. Emergency cleanup will invoke StopAll."
|
||
return 1
|
||
fi
|
||
MOTION_ACTIVE=0
|
||
}
|
||
|
||
run_optional_control() {
|
||
local label="$1"
|
||
local method="$2"
|
||
local payload="$3"
|
||
local max_time="${4:-10}"
|
||
|
||
if ((!EXECUTE)); then
|
||
print_plan "$label" "$method" "$payload"
|
||
return 0
|
||
fi
|
||
|
||
MOTION_ACTIVE=1
|
||
if rpc_expect_success "$method" "$payload" "$max_time"; then
|
||
log "$label: supported and successful"
|
||
MOTION_ACTIVE=0
|
||
return 0
|
||
fi
|
||
MOTION_ACTIVE=0
|
||
warn "$label returned unsupported/error; continuing capability test"
|
||
return 1
|
||
}
|
||
|
||
run_stop_motion() {
|
||
local label="$1"
|
||
local payload="$2"
|
||
if ((!EXECUTE)); then
|
||
print_plan "$label" "cmvr.api.ArmService/stopMotion" "$payload"
|
||
return 0
|
||
fi
|
||
rpc_expect_success "cmvr.api.ArmService/stopMotion" "$payload" 10
|
||
}
|
||
|
||
stop_all_now() {
|
||
local reason="$1"
|
||
local payload='{"header":{}}'
|
||
|
||
warn "invoking SystemService.StopAll: $reason"
|
||
if rpc_expect_success "cmvr.api.SystemService/StopAll" "$payload" 30; then
|
||
STOP_ALL_USED=1
|
||
MOTION_ACTIVE=0
|
||
warn "StopAll succeeded. The server called DeviceManager::stop(); restart cmvr_es before any further test."
|
||
return 0
|
||
fi
|
||
warn "StopAll failed; use the physical emergency stop immediately if anything is still moving"
|
||
return 1
|
||
}
|
||
|
||
cleanup() {
|
||
local rc=$?
|
||
trap - EXIT INT TERM
|
||
|
||
if ((MOTION_ACTIVE && !STOP_ALL_USED)); then
|
||
warn "script exited while motion may still be active"
|
||
stop_all_now "automatic cleanup after interruption/failure" || true
|
||
fi
|
||
if [[ -n "$BG_PID" ]]; then
|
||
wait "$BG_PID" 2>/dev/null || true
|
||
fi
|
||
if [[ -n "$BG_OUTPUT" && -f "$BG_OUTPUT" ]]; then
|
||
rm -f -- "$BG_OUTPUT"
|
||
fi
|
||
exit "$rc"
|
||
}
|
||
trap cleanup EXIT INT TERM
|
||
|
||
read_system_status() {
|
||
rpc_expect_success "cmvr.api.SystemService/GetSystemStatus" '{}' 10 || \
|
||
die "cannot read system status from $ENDPOINT"
|
||
}
|
||
|
||
read_arm_joint_state() {
|
||
local payload
|
||
payload=$(jq -cn --arg id "$ARM_ID" '{header:{deviceId:$id}}')
|
||
rpc_expect_success "cmvr.api.ArmService/getJointState" "$payload" 10 || \
|
||
die "cannot read joint state for $ARM_ID"
|
||
jq -e '
|
||
(.state.position | type == "array") and
|
||
(.state.position | length == 6) and
|
||
all(.state.position[]; type == "number")
|
||
' >/dev/null <<<"$LAST_RESPONSE" || \
|
||
die "invalid joint state: expected six numeric joint positions"
|
||
ARM_JOINT_RESPONSE="$LAST_RESPONSE"
|
||
}
|
||
|
||
require_arm_ready_and_stopped() {
|
||
read_arm_joint_state
|
||
jq -e --argjson limit "$ARM_STATIONARY_VELOCITY_RAD_S" '
|
||
(.state.velocity | type == "array") and
|
||
(.state.velocity | length == 6) and
|
||
all(.state.velocity[];
|
||
type == "number" and . >= -$limit and . <= $limit)
|
||
' >/dev/null <<<"$ARM_JOINT_RESPONSE" || \
|
||
die "arm is not stationary: all six joint velocities must be within +/-${ARM_STATIONARY_VELOCITY_RAD_S} rad/s"
|
||
}
|
||
|
||
read_arm_pose() {
|
||
local payload
|
||
payload=$(jq -cn --arg id "$ARM_ID" '{header:{deviceId:$id}}')
|
||
if ! rpc_expect_success "cmvr.api.ArmService/getPose" "$payload" 10; then
|
||
warn "cannot read TCP pose for $ARM_ID"
|
||
return 1
|
||
fi
|
||
|
||
# Proto3 omits zero-valued JSON fields. Treat an all-zero pose as invalid;
|
||
# using it as an absolute MoveL target is unsafe even if the server marks
|
||
# the response successful.
|
||
if ! jq -e '
|
||
[.pose.x // 0, .pose.y // 0, .pose.z // 0,
|
||
.pose.rx // 0, .pose.ry // 0, .pose.rz // 0] as $p |
|
||
all($p[]; type == "number") and any($p[]; . != 0)
|
||
' >/dev/null <<<"$LAST_RESPONSE"; then
|
||
warn "getPose returned an empty/all-zero pose; refusing absolute MoveL for safety"
|
||
return 1
|
||
fi
|
||
ARM_POSE_RESPONSE="$LAST_RESPONSE"
|
||
}
|
||
|
||
list_arm_frames() {
|
||
local payload
|
||
payload=$(jq -cn --arg id "$ARM_ID" '{header:{deviceId:$id}}')
|
||
if ! rpc_expect_success "cmvr.api.ArmService/ListBaseFrame" "$payload" 10; then
|
||
warn "ListBaseFrame failed"
|
||
fi
|
||
if ! rpc_expect_success "cmvr.api.ArmService/ListTCPFrame" "$payload" 10; then
|
||
warn "ListTCPFrame failed"
|
||
fi
|
||
}
|
||
|
||
read_agv_state() {
|
||
local payload
|
||
payload=$(jq -cn --arg id "$AGV_ID" '{header:{deviceId:$id}}')
|
||
rpc_expect_success "cmvr.api.AgvService/getRuntimeState" "$payload" 10 || \
|
||
die "cannot read AGV state for $AGV_ID"
|
||
AGV_STATE_RESPONSE="$LAST_RESPONSE"
|
||
}
|
||
|
||
require_agv_ready_and_stopped() {
|
||
read_agv_state
|
||
jq -e '
|
||
(.state.connected == true) and
|
||
(.state.localized == true) and
|
||
((.state.moving // false) == false) and
|
||
((.state.fault // false) == false) and
|
||
(((.state.emergency_stopped == true) or
|
||
(.state.emergencyStopped == true)) | not)
|
||
' >/dev/null <<<"$AGV_STATE_RESPONSE" || \
|
||
die "AGV is not safe to move: require connected, localized, stopped, no fault and no emergency stop"
|
||
}
|
||
|
||
wait_for_agv_stopped() {
|
||
local attempts="${1:-20}"
|
||
local i
|
||
for ((i = 1; i <= attempts; ++i)); do
|
||
read_agv_state
|
||
if jq -e '
|
||
((.state.moving // false) == false) and
|
||
((.state.fault // false) == false) and
|
||
(((.state.emergency_stopped == true) or
|
||
(.state.emergencyStopped == true)) | not)
|
||
' >/dev/null <<<"$AGV_STATE_RESPONSE"; then
|
||
return 0
|
||
fi
|
||
sleep 0.25
|
||
done
|
||
return 1
|
||
}
|
||
|
||
status_test() {
|
||
log "endpoint=$ENDPOINT arm=$ARM_ID agv=$AGV_ID execute=$EXECUTE"
|
||
read_system_status
|
||
read_arm_joint_state
|
||
list_arm_frames
|
||
if ! read_arm_pose; then
|
||
warn "Cartesian MoveL tests will be skipped until getPose returns a real TCP pose"
|
||
fi
|
||
read_agv_state
|
||
}
|
||
|
||
build_movej_request() {
|
||
local positions="$1"
|
||
local velocity="$2"
|
||
local acceleration="$3"
|
||
jq -cn \
|
||
--arg id "$ARM_ID" \
|
||
--argjson q "$positions" \
|
||
--argjson v "$velocity" \
|
||
--argjson a "$acceleration" \
|
||
'{header:{deviceId:$id},target:{position:$q},options:{velocity:$v,acceleration:$a,asynchronous:false}}'
|
||
}
|
||
|
||
build_movel_request() {
|
||
local pose="$1"
|
||
jq -cn \
|
||
--arg id "$ARM_ID" \
|
||
--argjson p "$pose" \
|
||
--argjson v "$ARM_LINEAR_VELOCITY" \
|
||
--argjson a "$ARM_LINEAR_ACCELERATION" \
|
||
--arg base "$ARM_BASE_FRAME" \
|
||
--arg tcp "$ARM_TCP_FRAME" '
|
||
{header:{deviceId:$id},target:$p,
|
||
options:{velocity:$v,acceleration:$a,asynchronous:false},
|
||
frame:"ARM_FRAME_BASE"}
|
||
+ (if $base == "" then {} else {baseFrame:$base} end)
|
||
+ (if $tcp == "" then {} else {tcpFrame:$tcp} end)
|
||
'
|
||
}
|
||
|
||
arm_movej_pair() {
|
||
local q0 q1 forward back
|
||
require_arm_ready_and_stopped
|
||
q0=$(jq -c '.state.position' <<<"$ARM_JOINT_RESPONSE")
|
||
q1=$(jq -c --argjson d "$ARM_J6_DELTA_RAD" '.[5] += $d' <<<"$q0")
|
||
forward=$(build_movej_request "$q1" "$ARM_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")
|
||
back=$(build_movej_request "$q0" "$ARM_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")
|
||
|
||
confirm_motion "Arm moveJ" \
|
||
"仅 J6 从 $(jq -r '.[5]' <<<"$q0") rad 增加 ${ARM_J6_DELTA_RAD} rad，然后回到读取到的起点。"
|
||
run_control "moveJ J6 +delta" "cmvr.api.ArmService/moveJ" "$forward" 30
|
||
read_arm_joint_state
|
||
run_control "moveJ return to start" "cmvr.api.ArmService/moveJ" "$back" 30
|
||
read_arm_joint_state
|
||
}
|
||
|
||
arm_movel_pair() {
|
||
local p0 p1 forward back
|
||
require_arm_ready_and_stopped
|
||
if ! read_arm_pose; then
|
||
warn "SKIP moveL: no trustworthy current TCP pose"
|
||
return 0
|
||
fi
|
||
p0=$(jq -c '{x:(.pose.x // 0),y:(.pose.y // 0),z:(.pose.z // 0),rx:(.pose.rx // 0),ry:(.pose.ry // 0),rz:(.pose.rz // 0)}' <<<"$ARM_POSE_RESPONSE")
|
||
p1=$(jq -c --argjson d "$ARM_Z_DELTA_M" '.z += $d' <<<"$p0")
|
||
forward=$(build_movel_request "$p1")
|
||
back=$(build_movel_request "$p0")
|
||
|
||
confirm_motion "Arm moveL" \
|
||
"保持 X/Y/姿态不变，仅 Z 增加 ${ARM_Z_DELTA_M} m，然后回到读取到的起点。"
|
||
run_control "moveL Z +delta" "cmvr.api.ArmService/moveL" "$forward" 30
|
||
read_arm_pose || die "cannot verify TCP pose after moveL"
|
||
run_control "moveL return to start" "cmvr.api.ArmService/moveL" "$back" 30
|
||
read_arm_pose || die "cannot verify TCP pose after moveL return"
|
||
}
|
||
|
||
arm_speed_and_servo_capability_tests() {
|
||
local q0 qservo speedj speedl servoj stop_req return_req
|
||
require_arm_ready_and_stopped
|
||
q0=$(jq -c '.state.position' <<<"$ARM_JOINT_RESPONSE")
|
||
qservo=$(jq -c '.[5] += 0.002' <<<"$q0")
|
||
stop_req=$(jq -cn --arg id "$ARM_ID" '{deviceId:$id}')
|
||
return_req=$(build_movej_request "$q0" "$ARM_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")
|
||
speedj=$(jq -cn \
|
||
--arg id "$ARM_ID" \
|
||
--argjson v "$ARM_JOINT_VELOCITY" \
|
||
--argjson a "$ARM_JOINT_ACCELERATION" \
|
||
--argjson t "$ARM_SPEED_DURATION_S" '
|
||
{header:{deviceId:$id},velocity:{velocity:[0,0,0,0,0,$v]},acceleration:$a,duration:$t}
|
||
')
|
||
speedl=$(jq -cn \
|
||
--arg id "$ARM_ID" \
|
||
--argjson v "$ARM_LINEAR_VELOCITY" \
|
||
--argjson a "$ARM_LINEAR_ACCELERATION" \
|
||
--argjson t "$ARM_SPEED_DURATION_S" '
|
||
{header:{deviceId:$id},velocity:{vx:0,vy:0,vz:$v,wx:0,wy:0,wz:0},acceleration:$a,duration:$t,frame:"ARM_FRAME_BASE"}
|
||
')
|
||
servoj=$(jq -cn --arg id "$ARM_ID" --argjson q "$qservo" \
|
||
'{header:{deviceId:$id},target:{position:$q}}')
|
||
|
||
confirm_motion "Arm speedJ/speedL/servoJ capability" \
|
||
"仅 J6 或 Z 以极低速度短时运动；每条命令后立即 stopMotion，并用 moveJ/moveL 恢复。Aubo 当前预期返回 unsupported。"
|
||
|
||
run_optional_control "speedJ (J6 only)" "cmvr.api.ArmService/speedJ" "$speedj" 10 || true
|
||
run_stop_motion "stopMotion after speedJ" "$stop_req" || \
|
||
die "stopMotion failed after speedJ"
|
||
if ((EXECUTE)); then
|
||
run_control "restore joints after speedJ" "cmvr.api.ArmService/moveJ" "$return_req" 30
|
||
else
|
||
print_plan "restore joints after speedJ" "cmvr.api.ArmService/moveJ" "$return_req"
|
||
fi
|
||
|
||
if read_arm_pose; then
|
||
local pose_before pose_return
|
||
pose_before=$(jq -c '{x:(.pose.x // 0),y:(.pose.y // 0),z:(.pose.z // 0),rx:(.pose.rx // 0),ry:(.pose.ry // 0),rz:(.pose.rz // 0)}' <<<"$ARM_POSE_RESPONSE")
|
||
pose_return=$(build_movel_request "$pose_before")
|
||
run_optional_control "speedL (Z only)" "cmvr.api.ArmService/speedL" "$speedl" 10 || true
|
||
run_stop_motion "stopMotion after speedL" "$stop_req" || \
|
||
die "stopMotion failed after speedL"
|
||
if ((EXECUTE)); then
|
||
run_control "restore TCP pose after speedL" "cmvr.api.ArmService/moveL" "$pose_return" 30
|
||
else
|
||
print_plan "restore TCP pose after speedL" "cmvr.api.ArmService/moveL" "$pose_return"
|
||
fi
|
||
else
|
||
warn "SKIP speedL: a real TCP pose is required to guarantee a safe return"
|
||
fi
|
||
|
||
run_optional_control "servoJ (J6 +0.002 rad)" "cmvr.api.ArmService/servoJ" "$servoj" 10 || true
|
||
run_stop_motion "stopMotion after servoJ" "$stop_req" || \
|
||
die "stopMotion failed after servoJ"
|
||
if ((EXECUTE)); then
|
||
run_control "restore joints after servoJ" "cmvr.api.ArmService/moveJ" "$return_req" 30
|
||
else
|
||
print_plan "restore joints after servoJ" "cmvr.api.ArmService/moveJ" "$return_req"
|
||
fi
|
||
}
|
||
|
||
arm_motion_tests() {
|
||
arm_movej_pair
|
||
arm_movel_pair
|
||
arm_speed_and_servo_capability_tests
|
||
}
|
||
|
||
actionqueue_test() {
|
||
local q0 q1 movej1 movej2 p0 p1 movel1 movel2 request details
|
||
require_arm_ready_and_stopped
|
||
q0=$(jq -c '.state.position' <<<"$ARM_JOINT_RESPONSE")
|
||
q1=$(jq -c --argjson d "$ARM_J6_DELTA_RAD" '.[5] += $d' <<<"$q0")
|
||
movej1=$(build_movej_request "$q1" "$ARM_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")
|
||
movej2=$(build_movej_request "$q0" "$ARM_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")
|
||
|
||
request=$(jq -cn --argjson j1 "$movej1" --argjson j2 "$movej2" '
|
||
{steps:[{timeoutMs:20000,armMoveJ:$j1},{timeoutMs:20000,armMoveJ:$j2}],totalTimeoutMs:60000}
|
||
')
|
||
details="队列前两步仅让 J6 增加 ${ARM_J6_DELTA_RAD} rad 并返回。"
|
||
|
||
if read_arm_pose; then
|
||
p0=$(jq -c '{x:(.pose.x // 0),y:(.pose.y // 0),z:(.pose.z // 0),rx:(.pose.rx // 0),ry:(.pose.ry // 0),rz:(.pose.rz // 0)}' <<<"$ARM_POSE_RESPONSE")
|
||
p1=$(jq -c --argjson d "$ARM_Z_DELTA_M" '.z += $d' <<<"$p0")
|
||
movel1=$(build_movel_request "$p1")
|
||
movel2=$(build_movel_request "$p0")
|
||
request=$(jq -c --argjson l1 "$movel1" --argjson l2 "$movel2" '
|
||
.steps += [{timeoutMs:20000,armMoveL:$l1},{timeoutMs:20000,armMoveL:$l2}] |
|
||
.totalTimeoutMs = 90000
|
||
' <<<"$request")
|
||
details+=" 后两步仅让 Z 增加 ${ARM_Z_DELTA_M} m 并返回。"
|
||
else
|
||
warn "ActionQueue will test MoveJ only; MoveL is skipped because getPose is not trustworthy"
|
||
fi
|
||
|
||
confirm_motion "SystemService.ExecuteActionQueue" "$details"
|
||
run_control "ActionQueue" "cmvr.api.SystemService/ExecuteActionQueue" "$request" 100
|
||
if ((EXECUTE)); then
|
||
jq -e '.result == "ACTION_RESULT_CODE_COMPLETED" and (.header.success == true)' \
|
||
>/dev/null <<<"$LAST_RESPONSE" || die "ActionQueue did not complete"
|
||
fi
|
||
read_arm_joint_state
|
||
}
|
||
|
||
start_background_rpc() {
|
||
local method="$1"
|
||
local payload="$2"
|
||
local max_time="${3:-120}"
|
||
BG_OUTPUT=$(mktemp -t cmvr-grpc-test.XXXXXX)
|
||
"$GRPCURL_BIN" "${GRPC_ARGS[@]}" -max-time "$max_time" \
|
||
-d "$payload" "$ENDPOINT" "$method" >"$BG_OUTPUT" 2>&1 &
|
||
BG_PID=$!
|
||
}
|
||
|
||
show_background_result() {
|
||
local rc=0
|
||
if [[ -n "$BG_PID" ]]; then
|
||
wait "$BG_PID" || rc=$?
|
||
fi
|
||
log "background RPC result (exit=$rc)"
|
||
if [[ -n "$BG_OUTPUT" && -f "$BG_OUTPUT" ]]; then
|
||
BACKGROUND_RESPONSE="$(<"$BG_OUTPUT")"
|
||
pretty_json_or_raw "$BACKGROUND_RESPONSE"
|
||
rm -f -- "$BG_OUTPUT"
|
||
fi
|
||
BG_PID=""
|
||
BG_OUTPUT=""
|
||
}
|
||
|
||
arm_stopall_test() {
|
||
local q0 q1 request initial_j6
|
||
require_arm_ready_and_stopped
|
||
q0=$(jq -c '.state.position' <<<"$ARM_JOINT_RESPONSE")
|
||
initial_j6=$(jq -er '.[5]' <<<"$q0")
|
||
q1=$(jq -c --argjson d "$ARM_STOP_J6_DELTA_RAD" '.[5] += $d' <<<"$q0")
|
||
request=$(build_movej_request "$q1" "$ARM_STOP_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")
|
||
|
||
confirm_motion "Arm MoveJ + StopAll" \
|
||
"仅 J6 低速移动 ${ARM_STOP_J6_DELTA_RAD} rad；检测到 J6 确实正在运动后立即调用 StopAll。调用后所有设备停止，必须重启 cmvr_es。"
|
||
if ((!EXECUTE)); then
|
||
print_plan "background slow MoveJ" "cmvr.api.ArmService/moveJ" "$request"
|
||
log "DRY RUN: poll J6 until both displacement and velocity prove active motion"
|
||
print_plan "StopAll while J6 is moving" "cmvr.api.SystemService/StopAll" '{"header":{}}'
|
||
return 0
|
||
fi
|
||
|
||
MOTION_ACTIVE=1
|
||
start_background_rpc "cmvr.api.ArmService/moveJ" "$request" 60
|
||
if ! wait_for_arm_j6_motion "$initial_j6"; then
|
||
stop_all_now "arm-stopall motion was not confirmed"
|
||
show_background_result
|
||
die "arm-stopall aborted because J6 was not confirmed moving"
|
||
fi
|
||
stop_all_now "arm-stopall test"
|
||
show_background_result
|
||
}
|
||
|
||
actionqueue_stopall_test() {
|
||
local q0 q1 j1 j2 request initial_j6
|
||
require_arm_ready_and_stopped
|
||
q0=$(jq -c '.state.position' <<<"$ARM_JOINT_RESPONSE")
|
||
initial_j6=$(jq -er '.[5]' <<<"$q0")
|
||
q1=$(jq -c --argjson d "$ARM_STOP_J6_DELTA_RAD" '.[5] += $d' <<<"$q0")
|
||
j1=$(build_movej_request "$q1" "$ARM_STOP_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")
|
||
j2=$(build_movej_request "$q0" "$ARM_STOP_JOINT_VELOCITY" "$ARM_JOINT_ACCELERATION")
|
||
request=$(jq -cn --argjson j1 "$j1" --argjson j2 "$j2" '
|
||
{steps:[{timeoutMs:30000,armMoveJ:$j1},{timeoutMs:30000,armMoveJ:$j2}],totalTimeoutMs:60000}
|
||
')
|
||
|
||
confirm_motion "ActionQueue + StopAll" \
|
||
"队列仅低速移动 J6；检测到 J6 确实正在运动后立即调用 StopAll，验证活动步骤取消及后续步骤清空。随后必须重启 cmvr_es。"
|
||
if ((!EXECUTE)); then
|
||
print_plan "background ActionQueue" "cmvr.api.SystemService/ExecuteActionQueue" "$request"
|
||
log "DRY RUN: poll J6 until both displacement and velocity prove active motion"
|
||
print_plan "StopAll while ActionQueue J6 step is moving" "cmvr.api.SystemService/StopAll" '{"header":{}}'
|
||
return 0
|
||
fi
|
||
|
||
MOTION_ACTIVE=1
|
||
start_background_rpc "cmvr.api.SystemService/ExecuteActionQueue" "$request" 90
|
||
if ! wait_for_arm_j6_motion "$initial_j6"; then
|
||
stop_all_now "actionqueue-stopall motion was not confirmed"
|
||
show_background_result
|
||
die "actionqueue-stopall aborted because J6 was not confirmed moving"
|
||
fi
|
||
stop_all_now "actionqueue-stopall test"
|
||
show_background_result
|
||
jq -e '.result == "ACTION_RESULT_CODE_CANCELED"' \
|
||
>/dev/null 2>&1 <<<"$BACKGROUND_RESPONSE" || \
|
||
die "ActionQueue did not report ACTION_RESULT_CODE_CANCELED after StopAll"
|
||
}
|
||
|
||
build_agv_options() {
|
||
local async="$1"
|
||
jq -cn \
|
||
--argjson speed "$AGV_NAV_SPEED" \
|
||
--argjson accel "$AGV_NAV_ACCELERATION" \
|
||
--argjson async "$async" '
|
||
{maxSpeed:$speed,maxAngularSpeed:0.1,maxAcceleration:$accel,
|
||
maxAngularAcceleration:0.1,reachDistance:0.01,reachAngle:0.03,
|
||
speedRatio:0.2,asynchronous:$async,waitTimeoutMs:30000,
|
||
pollIntervalMs:200,blockedTimeoutMs:5000}
|
||
'
|
||
}
|
||
|
||
agv_velocity_test() {
|
||
local start stop
|
||
require_agv_ready_and_stopped
|
||
start=$(jq -cn --arg id "$AGV_ID" --argjson v "$AGV_BODY_SPEED" \
|
||
'{header:{deviceId:$id},velocity:{vx:$v,vy:0,wz:0}}')
|
||
stop=$(jq -cn --arg id "$AGV_ID" '{deviceId:$id}')
|
||
|
||
confirm_motion "AGV setVelocity/stopVelocityControl" \
|
||
"底盘以 vx=${AGV_BODY_SPEED} m/s 前进 ${AGV_VELOCITY_DURATION_S}s（预计位移很小），随后停止。"
|
||
if ((!EXECUTE)); then
|
||
print_plan "setVelocity" "cmvr.api.AgvService/setVelocity" "$start"
|
||
print_plan "stopVelocityControl after ${AGV_VELOCITY_DURATION_S}s" "cmvr.api.AgvService/stopVelocityControl" "$stop"
|
||
return 0
|
||
fi
|
||
|
||
MOTION_ACTIVE=1
|
||
rpc_expect_success "cmvr.api.AgvService/setVelocity" "$start" 10 || \
|
||
die "setVelocity failed"
|
||
sleep "$AGV_VELOCITY_DURATION_S"
|
||
rpc_expect_success "cmvr.api.AgvService/stopVelocityControl" "$stop" 10 || \
|
||
die "stopVelocityControl failed"
|
||
wait_for_agv_stopped || die "AGV still reports moving after stopVelocityControl"
|
||
MOTION_ACTIVE=0
|
||
}
|
||
|
||
agv_translate_test() {
|
||
local request stop wait_s
|
||
require_agv_ready_and_stopped
|
||
request=$(jq -cn \
|
||
--arg id "$AGV_ID" \
|
||
--argjson d "$AGV_TRANSLATE_DISTANCE_M" \
|
||
--argjson v "$AGV_BODY_SPEED" '
|
||
{header:{deviceId:$id},translation:{distance:$d,vx:$v,vy:0,mode:"AGV_TRANSLATION_MODE_ODOMETRY"}}
|
||
')
|
||
stop=$(jq -cn --arg id "$AGV_ID" '{deviceId:$id}')
|
||
wait_s=$(awk -v d="$AGV_TRANSLATE_DISTANCE_M" -v v="$AGV_BODY_SPEED" 'BEGIN {printf "%.2f", d/v+2.0}')
|
||
|
||
confirm_motion "AGV translate" \
|
||
"底盘沿车体 X 正方向平移 ${AGV_TRANSLATE_DISTANCE_M} m，速度 ${AGV_BODY_SPEED} m/s。"
|
||
if ((!EXECUTE)); then
|
||
print_plan "translate" "cmvr.api.AgvService/translate" "$request"
|
||
return 0
|
||
fi
|
||
|
||
MOTION_ACTIVE=1
|
||
rpc_expect_success "cmvr.api.AgvService/translate" "$request" 10 || \
|
||
die "translate was not accepted"
|
||
sleep "$wait_s"
|
||
rpc_expect_success "cmvr.api.AgvService/stopVelocityControl" "$stop" 10 || true
|
||
wait_for_agv_stopped || die "AGV still reports moving after translate/stopVelocityControl"
|
||
MOTION_ACTIVE=0
|
||
}
|
||
|
||
agv_navigate_pose_test() {
|
||
local p0 p1 options go back
|
||
require_agv_ready_and_stopped
|
||
p0=$(jq -c '{x:(.state.pose.x // 0),y:(.state.pose.y // 0),theta:(.state.pose.theta // 0)}' <<<"$AGV_STATE_RESPONSE")
|
||
p1=$(jq -c --argjson d "$AGV_POSE_DELTA_M" '.x += $d' <<<"$p0")
|
||
options=$(build_agv_options false)
|
||
go=$(jq -cn --arg id "$AGV_ID" --argjson p "$p1" --argjson o "$options" \
|
||
'{header:{deviceId:$id},pose:$p,options:$o}')
|
||
back=$(jq -cn --arg id "$AGV_ID" --argjson p "$p0" --argjson o "$options" \
|
||
'{header:{deviceId:$id},pose:$p,options:$o}')
|
||
|
||
confirm_motion "AGV navigateToPose" \
|
||
"地图坐标 X 增加 ${AGV_POSE_DELTA_M} m，Y/航向不变，然后返回读取到的起点。请确认这两个点间无障碍物。"
|
||
run_control "navigateToPose +X" "cmvr.api.AgvService/navigateToPose" "$go" 40
|
||
MOTION_ACTIVE=1
|
||
require_agv_ready_and_stopped
|
||
MOTION_ACTIVE=0
|
||
run_control "navigateToPose return" "cmvr.api.AgvService/navigateToPose" "$back" 40
|
||
MOTION_ACTIVE=1
|
||
require_agv_ready_and_stopped
|
||
MOTION_ACTIVE=0
|
||
}
|
||
|
||
agv_pause_resume_cancel_test() {
|
||
local p0 p1 options request header
|
||
require_agv_ready_and_stopped
|
||
p0=$(jq -c '{x:(.state.pose.x // 0),y:(.state.pose.y // 0),theta:(.state.pose.theta // 0)}' <<<"$AGV_STATE_RESPONSE")
|
||
p1=$(jq -c --argjson d "$AGV_POSE_DELTA_M" '.x += (2*$d)' <<<"$p0")
|
||
options=$(build_agv_options true)
|
||
request=$(jq -cn --arg id "$AGV_ID" --argjson p "$p1" --argjson o "$options" \
|
||
'{header:{deviceId:$id},pose:$p,options:$o}')
|
||
header=$(jq -cn --arg id "$AGV_ID" '{deviceId:$id}')
|
||
|
||
confirm_motion "AGV pause/resume/cancel" \
|
||
"异步目标最多偏移 ${AGV_POSE_DELTA_M}*2 m；短时运行后暂停、恢复，再取消。"
|
||
if ((!EXECUTE)); then
|
||
print_plan "async navigateToPose" "cmvr.api.AgvService/navigateToPose" "$request"
|
||
print_plan "pauseNavigation" "cmvr.api.AgvService/pauseNavigation" "$header"
|
||
print_plan "resumeNavigation" "cmvr.api.AgvService/resumeNavigation" "$header"
|
||
print_plan "cancelNavigation" "cmvr.api.AgvService/cancelNavigation" "$header"
|
||
return 0
|
||
fi
|
||
|
||
MOTION_ACTIVE=1
|
||
rpc_expect_success "cmvr.api.AgvService/navigateToPose" "$request" 10 || die "async navigation failed"
|
||
sleep 0.5
|
||
rpc_expect_success "cmvr.api.AgvService/pauseNavigation" "$header" 10 || die "pauseNavigation failed"
|
||
read_agv_state
|
||
rpc_expect_success "cmvr.api.AgvService/resumeNavigation" "$header" 10 || die "resumeNavigation failed"
|
||
sleep 0.5
|
||
rpc_expect_success "cmvr.api.AgvService/cancelNavigation" "$header" 10 || die "cancelNavigation failed"
|
||
sleep 0.5
|
||
wait_for_agv_stopped || die "AGV still reports moving after cancelNavigation"
|
||
MOTION_ACTIVE=0
|
||
}
|
||
|
||
agv_optional_station_tests() {
|
||
local header stations options request
|
||
header=$(jq -cn --arg id "$AGV_ID" '{header:{deviceId:$id}}')
|
||
if ! rpc_expect_success "cmvr.api.AgvService/listStations" "$header" 15; then
|
||
warn "listStations failed; station/path tests skipped"
|
||
return 0
|
||
fi
|
||
stations="$LAST_RESPONSE"
|
||
options=$(build_agv_options false)
|
||
|
||
if [[ -n "$AGV_TEST_STATION" ]]; then
|
||
jq -e --arg id "$AGV_TEST_STATION" 'any(.stations[]?; .id == $id)' \
|
||
>/dev/null <<<"$stations" || die "AGV_TEST_STATION not found: $AGV_TEST_STATION"
|
||
require_agv_ready_and_stopped
|
||
request=$(jq -cn --arg id "$AGV_ID" --arg station "$AGV_TEST_STATION" --argjson o "$options" \
|
||
'{header:{deviceId:$id},stationId:$station,options:$o}')
|
||
confirm_motion "AGV navigateToStation" \
|
||
"目标站点=${AGV_TEST_STATION}。该路径可能超过小范围，必须由现场人员核对地图和路线。"
|
||
run_control "navigateToStation" "cmvr.api.AgvService/navigateToStation" "$request" 120
|
||
else
|
||
warn "SKIP navigateToStation: set AGV_TEST_STATION after checking listStations output"
|
||
fi
|
||
|
||
if [[ -n "$AGV_PATH_SOURCE" || -n "$AGV_PATH_TARGET" ]]; then
|
||
[[ -n "$AGV_PATH_SOURCE" && -n "$AGV_PATH_TARGET" ]] || \
|
||
die "set both AGV_PATH_SOURCE and AGV_PATH_TARGET"
|
||
jq -e --arg s "$AGV_PATH_SOURCE" --arg t "$AGV_PATH_TARGET" '
|
||
any(.stations[]?; .id == $s) and any(.stations[]?; .id == $t)
|
||
' >/dev/null <<<"$stations" || die "AGV path source/target is not present in listStations"
|
||
require_agv_ready_and_stopped
|
||
request=$(jq -cn \
|
||
--arg id "$AGV_ID" --arg s "$AGV_PATH_SOURCE" --arg t "$AGV_PATH_TARGET" --argjson o "$options" '
|
||
{header:{deviceId:$id},path:[{sourceStation:$s,targetStation:$t}],options:$o}
|
||
')
|
||
confirm_motion "AGV followPath" \
|
||
"路径=${AGV_PATH_SOURCE} -> ${AGV_PATH_TARGET}。该路径可能超过小范围，必须由现场人员核对。"
|
||
run_control "followPath" "cmvr.api.AgvService/followPath" "$request" 180
|
||
else
|
||
warn "SKIP followPath: set AGV_PATH_SOURCE and AGV_PATH_TARGET after checking listStations output"
|
||
fi
|
||
}
|
||
|
||
agv_motion_tests() {
|
||
agv_navigate_pose_test
|
||
agv_velocity_test
|
||
agv_translate_test
|
||
agv_pause_resume_cancel_test
|
||
agv_optional_station_tests
|
||
}
|
||
|
||
agv_stopall_test() {
|
||
local start
|
||
require_agv_ready_and_stopped
|
||
start=$(jq -cn --arg id "$AGV_ID" --argjson v "$AGV_BODY_SPEED" \
|
||
'{header:{deviceId:$id},velocity:{vx:$v,vy:0,wz:0}}')
|
||
|
||
confirm_motion "AGV setVelocity + StopAll" \
|
||
"底盘以 vx=${AGV_BODY_SPEED} m/s 低速前进；${STOP_DELAY_S}s 后调用 StopAll。随后必须重启 cmvr_es。"
|
||
if ((!EXECUTE)); then
|
||
print_plan "setVelocity" "cmvr.api.AgvService/setVelocity" "$start"
|
||
print_plan "StopAll after ${STOP_DELAY_S}s" "cmvr.api.SystemService/StopAll" '{"header":{}}'
|
||
return 0
|
||
fi
|
||
|
||
MOTION_ACTIVE=1
|
||
rpc_expect_success "cmvr.api.AgvService/setVelocity" "$start" 10 || die "setVelocity failed"
|
||
sleep "$STOP_DELAY_S"
|
||
stop_all_now "agv-stopall test"
|
||
}
|
||
|
||
case "$COMMAND" in
|
||
status)
|
||
status_test
|
||
;;
|
||
arm-motions)
|
||
arm_motion_tests
|
||
;;
|
||
actionqueue)
|
||
actionqueue_test
|
||
;;
|
||
agv-motions)
|
||
agv_motion_tests
|
||
;;
|
||
all-safe)
|
||
status_test
|
||
arm_motion_tests
|
||
actionqueue_test
|
||
agv_motion_tests
|
||
;;
|
||
arm-stopall)
|
||
arm_stopall_test
|
||
;;
|
||
actionqueue-stopall)
|
||
actionqueue_stopall_test
|
||
;;
|
||
agv-stopall)
|
||
agv_stopall_test
|
||
;;
|
||
esac
|
||
|
||
if ((STOP_ALL_USED)); then
|
||
warn "TEST COMPLETE: StopAll was used. Restart remote cmvr_es before the next test."
|
||
elif ((!EXECUTE)); then
|
||
log "dry run complete; no motion request was sent (add --execute to run)"
|
||
else
|
||
log "test complete"
|
||
fi
|