SmartBench/cmvr-es
8
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix aubo state queries and remote motion tests

Browse Source
This commit is contained in:
xtkuang 2026-09-17 16:28:37 +08:00
parent d15d381b1d
commit aa2c82cba1
3 changed files with 1078 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
cmvr-es/devices/agv/seer_robokit/src/seer_robokit_status.cpp
View File

@ -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;

112
cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp
View File

@ -179,6 +179,28 @@ std::vector<std::string> listAuboWorldFrames(
return {frame_names.begin(), frame_names.end()};
}
CartesianPose auboPoseFromVector(const std::vector<double>& 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<std::string>& 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<std::string>& frame_names) const
ArmErrorCode::RobotNotReady,
"[AuboArm] world-frame interface is unavailable");
}
frame_names = listAuboWorldFrames(robot_interface->getSyncMove());
std::set<std::string> 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<std::string>& frame_names) const
"[AuboArm] world-frame interface is unavailable");
}
const auto sync_move = robot_interface->getSyncMove();
frame_names = {"tool0", "flange", "tcp"};
std::set<std::string> 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)) {
frame_names.push_back(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

959
tools/grpc_remote_motion_test.sh Executable file
View File

@ -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] <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 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=<id>
AGV_PATH_SOURCE=<id> AGV_PATH_TARGET=<id>
Optional named frames for MoveL:
ARM_BASE_FRAME=<name> ARM_TCP_FRAME=<name>
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