verify active arm motion before stop all tests
This commit is contained in:
parent
c3c58f4563
commit
77c6def8ca
@ -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,
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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) {
|
||||
|
||||
@ -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=<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"' \
|
||||
|
||||
Loading…
Reference in New Issue
Block a user