From aa2c82cba11fbcac4fdcf411e66c6cb3179493c5 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Thu, 17 Sep 2026 16:28:37 +0800 Subject: [PATCH] fix aubo state queries and remote motion tests --- .../seer_robokit/src/seer_robokit_status.cpp | 12 +- cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp | 118 ++- tools/grpc_remote_motion_test.sh | 959 ++++++++++++++++++ 3 files changed, 1078 insertions(+), 11 deletions(-) create mode 100755 tools/grpc_remote_motion_test.sh diff --git a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp index 475d4648..2a6ff9d2 100644 --- a/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp +++ b/cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp @@ -20,6 +20,13 @@ using namespace seer_robokit::detail; namespace { +// The controller reports small non-zero velocity noise while physically +// stopped (observed around 1e-4 m/s and 4e-4 rad/s). Keep the deadband well +// below the minimum smoke-test command speed while avoiding a permanent +// moving=true state at rest. +constexpr double kStoppedLinearVelocityThreshold = 1e-3; +constexpr double kStoppedAngularVelocityThreshold = 1e-3; + bool hasFaultArray(const Json::Value& value, const char* key) { const auto* found = jsonFind(value, key); @@ -638,7 +645,10 @@ void SeerRobokitAgv::updateCachedRuntimeState_(const Json::Value& payload) state.emergency_stopped = jsonGet(payload, "emergency", state.emergency_stopped).asBool(); } - state.moving = std::hypot(state.velocity.vx, state.velocity.vy) > 1e-4 || std::abs(state.velocity.wz) > 1e-4; + state.moving = + std::hypot(state.velocity.vx, state.velocity.vy) > + kStoppedLinearVelocityThreshold || + std::abs(state.velocity.wz) > kStoppedAngularVelocityThreshold; const bool has_fatals = jsonHas(payload, "fatals"); const bool has_errors = jsonHas(payload, "errors"); const bool has_fault_fields = has_fatals || has_errors; diff --git a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp index 29540094..679861c9 100644 --- a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp @@ -179,6 +179,28 @@ std::vector listAuboWorldFrames( return {frame_names.begin(), frame_names.end()}; } +CartesianPose auboPoseFromVector(const std::vector& values, + const std::string& context) +{ + if (values.size() < 6) { + throw std::runtime_error( + context + " returned an invalid pose, expected 6 values, actual=" + + std::to_string(values.size())); + } + return { + values[0], values[1], values[2], + values[3], values[4], values[5], + }; +} + +void insertFrameName(std::set& frame_names, + const std::string& frame_name) +{ + if (!frame_name.empty()) { + frame_names.insert(frame_name); + } +} + bool isAuboTcpFrame(const arcs::common_interface::SyncMovePtr& sync_move, const std::string& frame_name) { @@ -332,7 +354,33 @@ JointGroupState AuboArm::getJointState() const CartesianPose AuboArm::getTcpPose(FrameType frame) const { (void)frame; +#if defined(CMVR_HAS_AUBO_SDK) + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + CMVR_LOG(ERROR) << "[AuboArm] getTcpPose failed: arm is not connected"; + return {}; + } + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + CMVR_LOG(ERROR) << "[AuboArm] getTcpPose failed: robot name list is empty"; + return {}; + } + const auto robot_interface = + sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface || !robot_interface->getRobotState()) { + CMVR_LOG(ERROR) << "[AuboArm] getTcpPose failed: robot state interface is unavailable"; + return {}; + } + return auboPoseFromVector( + robot_interface->getRobotState()->getTcpPose(), + "RobotState.getTcpPose"); + } catch (const std::exception& e) { + CMVR_LOG(ERROR) << "[AuboArm] getTcpPose failed: " << e.what(); + return {}; + } +#else return {}; +#endif } RobotMode AuboArm::getRobotMode() const @@ -387,7 +435,24 @@ Result AuboArm::listBaseFrame(std::vector& frame_names) const ArmErrorCode::RobotNotReady, "[AuboArm] world-frame interface is unavailable"); } - frame_names = listAuboWorldFrames(robot_interface->getSyncMove()); + std::set available_frames{"world", "base"}; + insertFrameName(available_frames, vendor_cfg_.base_frame()); + try { + const auto discovered_frames = + listAuboWorldFrames(robot_interface->getSyncMove()); + available_frames.insert( + discovered_frames.begin(), discovered_frames.end()); + } catch (const std::exception& e) { + // Older Aubo controller releases do not expose + // SyncMove.frameGetChildren even though SDK 0.27.1 declares it. + // Keep the built-in/configured frames usable instead of failing + // the whole gRPC request with JSON-RPC -32601. + CMVR_LOG(WARNING) + << "[AuboArm] ListBaseFrame dynamic discovery unavailable; " + "using built-in/configured frames: " + << e.what(); + } + frame_names.assign(available_frames.begin(), available_frames.end()); return Result::success(); } catch (const std::exception& e) { return Result::failure( @@ -423,13 +488,22 @@ Result AuboArm::listTCPFrame(std::vector& frame_names) const "[AuboArm] world-frame interface is unavailable"); } const auto sync_move = robot_interface->getSyncMove(); - frame_names = {"tool0", "flange", "tcp"}; - for (const auto& frame_name : listAuboWorldFrames(sync_move)) { - if (frame_name != "flange" && frame_name != "tcp" && - isAuboTcpFrame(sync_move, frame_name)) { - frame_names.push_back(frame_name); + std::set available_frames{"tool0", "flange", "tcp"}; + insertFrameName(available_frames, vendor_cfg_.tool_frame()); + try { + for (const auto& frame_name : listAuboWorldFrames(sync_move)) { + if (frame_name != "flange" && frame_name != "tcp" && + isAuboTcpFrame(sync_move, frame_name)) { + available_frames.insert(frame_name); + } } + } catch (const std::exception& e) { + CMVR_LOG(WARNING) + << "[AuboArm] ListTCPFrame dynamic discovery unavailable; " + "using built-in/configured frames: " + << e.what(); } + frame_names.assign(available_frames.begin(), available_frames.end()); return Result::success(); } catch (const std::exception& e) { return Result::failure( @@ -1233,15 +1307,39 @@ CartesianPose AuboArm::fk(const std::string& base_link, const std::string& ee_li { (void)base_link; (void)ee_link; - CMVR_LOG(ERROR) << "[AuboArm] fk(base,ee) is not implemented"; - return {}; + return fk(true); } CartesianPose AuboArm::fk(bool is_tcp) { - (void)is_tcp; - CMVR_LOG(ERROR) << "[AuboArm] fk is not implemented"; +#if defined(CMVR_HAS_AUBO_SDK) + if (!connected_.load() || !sdk_ || !sdk_->rpc_client) { + throw std::runtime_error("arm is not connected"); + } + try { + const auto robot_names = sdk_->rpc_client->getRobotNames(); + if (robot_names.empty()) { + throw std::runtime_error("robot name list is empty"); + } + const auto robot_interface = + sdk_->rpc_client->getRobotInterface(robot_names.front()); + if (!robot_interface || !robot_interface->getRobotState()) { + throw std::runtime_error( + "robot state interface is unavailable"); + } + const auto robot_state = robot_interface->getRobotState(); + return auboPoseFromVector( + is_tcp ? robot_state->getTcpPose() : robot_state->getToolPose(), + is_tcp ? "RobotState.getTcpPose" : "RobotState.getToolPose"); + } catch (const std::exception& e) { + const std::string message = + std::string{"[AuboArm] fk failed: "} + e.what(); + CMVR_LOG(ERROR) << message; + throw std::runtime_error(message); + } +#else return {}; +#endif } Result AuboArm::unsupported_(const std::string& name) const diff --git a/tools/grpc_remote_motion_test.sh b/tools/grpc_remote_motion_test.sh new file mode 100755 index 00000000..c6777b49 --- /dev/null +++ b/tools/grpc_remote_motion_test.sh @@ -0,0 +1,959 @@ +#!/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. +# * 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:-}" + +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="" + +usage() { + cat <<'EOF' +Usage: + tools/grpc_remote_motion_test.sh [options] + +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 Call StopAll during a slow, small J6 MoveJ. + actionqueue-stopall Call StopAll during an ActionQueue. + 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= + AGV_PATH_SOURCE= AGV_PATH_TARGET= + +Optional named frames for MoveL: + ARM_BASE_FRAME= ARM_TCP_FRAME= + +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 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 +} + +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 + require_arm_ready_and_stopped + q0=$(jq -c '.state.position' <<<"$ARM_JOINT_RESPONSE") + 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；${STOP_DELAY_S}s 后调用 StopAll。调用后所有设备停止，必须重启 cmvr_es。" + if ((!EXECUTE)); then + print_plan "background slow MoveJ" "cmvr.api.ArmService/moveJ" "$request" + print_plan "StopAll after ${STOP_DELAY_S}s" "cmvr.api.SystemService/StopAll" '{"header":{}}' + return 0 + fi + + MOTION_ACTIVE=1 + start_background_rpc "cmvr.api.ArmService/moveJ" "$request" 60 + sleep "$STOP_DELAY_S" + stop_all_now "arm-stopall test" + show_background_result +} + +actionqueue_stopall_test() { + local q0 q1 j1 j2 request + require_arm_ready_and_stopped + q0=$(jq -c '.state.position' <<<"$ARM_JOINT_RESPONSE") + 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；${STOP_DELAY_S}s 后调用 StopAll，验证活动步骤取消及后续步骤清空。随后必须重启 cmvr_es。" + if ((!EXECUTE)); then + print_plan "background ActionQueue" "cmvr.api.SystemService/ExecuteActionQueue" "$request" + print_plan "StopAll after ${STOP_DELAY_S}s" "cmvr.api.SystemService/StopAll" '{"header":{}}' + return 0 + fi + + MOTION_ACTIVE=1 + start_background_rpc "cmvr.api.SystemService/ExecuteActionQueue" "$request" 90 + sleep "$STOP_DELAY_S" + 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