From 77c6def8ca77cfc6fce452ca3f4f7d6450ac9d03 Mon Sep 17 00:00:00 2001 From: xtkuang <87661715@qq.com> Date: Thu, 17 Sep 2026 17:57:15 +0800 Subject: [PATCH] verify active arm motion before stop all tests --- cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp | 4 + .../service/grpc/action/src/action_queue.cpp | 35 +++++ cmvr-es/service/grpc/src/grpc_arm_service.cpp | 16 +++ tools/grpc_remote_motion_test.sh | 121 ++++++++++++++++-- 4 files changed, 166 insertions(+), 10 deletions(-) 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 ff436ca6..092371ef 100644 --- a/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp +++ b/cmvr-es/devices/arm/aubo_arm/src/aubo_arm.cpp @@ -738,6 +738,8 @@ Result AuboArm::moveJ(const JointPositionCommand& target, const MotionOptions& o options.velocity > 0.0 ? options.velocity : 0.5, options.blend_radius, 0); + CMVR_LOG(DEBUG) << "[AuboArm] moveJ submitted, id=" << id_ + << ", sdk ret=" << ret; const auto outcome = aubo_internal::resolveMotionCommand( ret, arcs::common_interface::AUBO_OK, @@ -977,6 +979,8 @@ Result AuboArm::moveL(const CartesianPose& target, options.velocity > 0.0 ? options.velocity : 0.25, options.blend_radius, 0); + CMVR_LOG(DEBUG) << "[AuboArm] moveL submitted, id=" << id_ + << ", sdk ret=" << ret; const auto outcome = aubo_internal::resolveMotionCommand( ret, arcs::common_interface::AUBO_OK, diff --git a/cmvr-es/service/grpc/action/src/action_queue.cpp b/cmvr-es/service/grpc/action/src/action_queue.cpp index b43e5904..7ae38e0d 100644 --- a/cmvr-es/service/grpc/action/src/action_queue.cpp +++ b/cmvr-es/service/grpc/action/src/action_queue.cpp @@ -37,6 +37,19 @@ bool finiteNonNegative(const double value) return std::isfinite(value) && value >= 0.0; } +const char* actionCommandName(const api::ActionStep::CommandCase command) +{ + switch (command) { + case api::ActionStep::kArmMoveJ: + return "MoveJ"; + case api::ActionStep::kArmMoveL: + return "MoveL"; + case api::ActionStep::COMMAND_NOT_SET: + return "NotSet"; + } + return "Unknown"; +} + device::JointPositionCommand toJointPosition( const api::JointPositionCommand& source) { @@ -400,6 +413,7 @@ void ActionQueue::execute( impl_->active_arm.reset(); impl_->state = State::Running; } + CMVR_LOG(DEBUG) << "[ActionQueue] accepted, steps=" << prepared.size(); const auto total_timeout = request.total_timeout_ms() == 0 ? kDefaultTotalTimeout @@ -420,6 +434,9 @@ void ActionQueue::execute( impl_->state = impl_->shutting_down.load() ? State::ShuttingDown : State::Idle; + CMVR_LOG(DEBUG) + << "[ActionQueue] canceled before next step, completed=" + << completed_steps; return; } if (impl_->pending.empty()) { @@ -428,6 +445,8 @@ void ActionQueue::execute( completed_steps, {}); impl_->active_arm.reset(); impl_->state = State::Idle; + CMVR_LOG(DEBUG) << "[ActionQueue] completed, steps=" + << completed_steps; return; } if (std::chrono::steady_clock::now() >= total_deadline) { @@ -444,6 +463,12 @@ void ActionQueue::execute( impl_->active_arm = step.arm; } + CMVR_LOG(DEBUG) << "[ActionQueue] step dispatch, index=" + << step.source_index + << ", command=" + << actionCommandName(step.source.command_case()) + << ", device=" << step.arm->id(); + auto step_deadline = total_deadline; if (step.source.timeout_ms() != 0) { step_deadline = std::min( @@ -471,6 +496,13 @@ void ActionQueue::execute( "ActionQueue command threw an unknown exception"); } + CMVR_LOG(DEBUG) << "[ActionQueue] step finished, index=" + << step.source_index + << ", command=" + << actionCommandName(step.source.command_case()) + << ", success=" << result.ok() + << (result.ok() ? "" : ", error=" + result.message); + { std::lock_guard lock(impl_->mutex); impl_->active_arm.reset(); @@ -484,6 +516,9 @@ void ActionQueue::execute( impl_->state = impl_->shutting_down.load() ? State::ShuttingDown : State::Idle; + CMVR_LOG(DEBUG) << "[ActionQueue] canceled, active_index=" + << step.source_index + << ", completed=" << completed_steps; return; } if (std::chrono::steady_clock::now() >= step_deadline) { diff --git a/cmvr-es/service/grpc/src/grpc_arm_service.cpp b/cmvr-es/service/grpc/src/grpc_arm_service.cpp index f65aa39c..1cd79f73 100644 --- a/cmvr-es/service/grpc/src/grpc_arm_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_arm_service.cpp @@ -240,11 +240,19 @@ grpc::Status gRPCArmServiceImpl::moveJ(grpc::ServerContext* context, options.cancellation_requested = [context]() { return context && context->IsCancelled(); }; + CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): dispatch, id=" + << device_id + << ", positions=" << request->target().position_size() + << ", velocity=" << options.velocity + << ", acceleration=" << options.acceleration; const auto result = arm->moveJ( toJointPositionCommand(request->target()), options); if (result.ok()) { CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): success, id=" << device_id << ", positions=" << request->target().position_size(); + } else { + CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveJ): finished, id=" + << device_id << ", error=" << result.message; } return setResponseResult(response, result); } catch (const std::exception& e) { @@ -275,6 +283,11 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext* context, options.cancellation_requested = [context]() { return context && context->IsCancelled(); }; + CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): dispatch, id=" + << device_id + << ", velocity=" << options.velocity + << ", acceleration=" << options.acceleration + << ", frame=" << request->frame(); const auto result = has_named_frame ? arm->moveL(toCartesianPose(request->target()), options, @@ -298,6 +311,9 @@ grpc::Status gRPCArmServiceImpl::moveL(grpc::ServerContext* context, << (request->has_tcp_frame() ? request->tcp_frame() : ""); + } else { + CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): finished, id=" + << device_id << ", error=" << result.message; } return setResponseResult(response, result); } catch (const std::exception& e) { diff --git a/tools/grpc_remote_motion_test.sh b/tools/grpc_remote_motion_test.sh index c6777b49..a8ec0c87 100755 --- a/tools/grpc_remote_motion_test.sh +++ b/tools/grpc_remote_motion_test.sh @@ -7,6 +7,8 @@ # * 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 @@ -33,6 +35,10 @@ 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}" @@ -56,6 +62,7 @@ STOP_ALL_USED=0 BG_PID="" BG_OUTPUT="" BACKGROUND_RESPONSE="" +QUIET_RESPONSE="" usage() { cat <<'EOF' @@ -71,8 +78,8 @@ Commands: 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. + 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: @@ -93,6 +100,12 @@ Optional AGV station/path tests: Optional named frames for MoveL: ARM_BASE_FRAME= ARM_TCP_FRAME= +Optional arm motion-detection tuning for StopAll tests: + ARM_MOTION_DETECT_TIMEOUT_S= + ARM_MOTION_POLL_INTERVAL_S= + ARM_MOTION_POSITION_THRESHOLD_RAD= + ARM_MOTION_VELOCITY_THRESHOLD_RAD_S= + Examples: # Read-only connectivity and state check tools/grpc_remote_motion_test.sh status @@ -193,6 +206,10 @@ 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 @@ -256,6 +273,78 @@ rpc_expect_success() { 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" @@ -678,31 +767,38 @@ show_background_result() { } arm_stopall_test() { - local q0 q1 request + 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；${STOP_DELAY_S}s 后调用 StopAll。调用后所有设备停止，必须重启 cmvr_es。" + "仅 J6 低速移动 ${ARM_STOP_J6_DELTA_RAD} rad；检测到 J6 确实正在运动后立即调用 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":{}}' + 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 - sleep "$STOP_DELAY_S" + 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 + 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") @@ -711,16 +807,21 @@ actionqueue_stopall_test() { ') confirm_motion "ActionQueue + StopAll" \ - "队列仅低速移动 J6；${STOP_DELAY_S}s 后调用 StopAll，验证活动步骤取消及后续步骤清空。随后必须重启 cmvr_es。" + "队列仅低速移动 J6；检测到 J6 确实正在运动后立即调用 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":{}}' + 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 - sleep "$STOP_DELAY_S" + 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"' \