verify active arm motion before stop all tests

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,

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) {

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()
                                     : "<default>");
+        } else {
+            CMVR_LOG(DEBUG) << "[gRPCArmServiceImpl] (moveL): finished, id="
+                            << device_id << ", error=" << result.message;
         }
         return setResponseResult(response, result);
     } catch (const std::exception& e) {

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.
+  arm-stopall            Observe a slow J6 MoveJ, then call StopAll.
-  actionqueue-stopall    Call StopAll during an ActionQueue.
+  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=<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
@@ -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"' \