Skip to content

fix: arrival budget, diagnostics, and protocol-sync robustness #12

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
X-F-R merged 2 commits into from
Oct 13, 2026
Merged

fix: arrival budget, diagnostics, and protocol-sync robustness #12

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
233ad7c
fix(tests): tolerate a CRLF firmware checkout in test_protocol_sync
YOLO-DT Oct 13, 2026
95aadc6
fix: progress-based arrival budget + actionable diagnostics
YOLO-DT Oct 13, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
4 changes: 3 additions & 1 deletion include/litearm/cart.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -159,7 +159,9 @@ struct CartPlan {
/// CANCELED 单独成型是刻意的: 接管是正常用法, 混进"规划失败"会让调用方走故障恢复。
/// 反过来 CartesianPlanError 的三档都是"这条轨迹本身不成立, 臂一步没动"。
/// 未登记的 err 值也归到 CartesianPlanError (ok=0 就是"规划没成"), 但消息里带上原始码。
void raise_for_plan(const CartPlan& plan);
/// `arm` 可选: 传了就用于在 err=CART_ERR_LIMIT 时**本地推断**最紧的轴 (0x4E 载荷里
/// 没有轴号), 并把"下一步做什么"写进异常消息; 传 nullptr 时消息与从前逐字节相同。
void raise_for_plan(const CartPlan& plan, Arm* arm = nullptr);

/// 一条在途笛卡尔请求的票据 (SDK 自造, 固件不知道它的存在)。
struct CartToken {
Expand Down
3 changes: 3 additions & 0 deletions include/litearm/testing.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -237,6 +237,9 @@ class FakeTransport : public Transport {
// 覆盖 GET_TCP 回读的位置/朝向 (模拟固件的 rpy 规范化 —— 含万向锁强制 yaw=0)
std::optional<std::vector<double>> pos_override;
std::optional<std::vector<double>> rpy_override;
/// 非空时**主动状态帧**里的 dq 用它 —— 原实现恒传空 vector, 于是桩里臂
/// "永远不在动", "还在动就续期"这类判据在离线用例里根本走不到。
std::optional<std::vector<double>> dq_override;
/// 非空时 `0x43` 原样回这段载荷 (而不是打包 6 个 f32) —— 用于造"帧长不足"那条分支。
/// ⚠ 用**桩的确定注入点**, 不要在用例里手工 push_frame: 那样造出来的帧会与读线程抢
/// 时序 (它可能先被读进队列, 再由 `drain_for` 清掉), 用例变成偶发红。
Expand Down
73 changes: 69 additions & 4 deletions src/arm.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -935,6 +935,13 @@ std::string Arm::zero_g_error_text() {
// 到位等待 / 关节运动
// ===========================================================================

/// arrive() 的**硬上限倍率**: 无进展预算之外的兜底。
///
/// 为什么必须有: 无进展判据靠"还在动就续期"。若臂被外力顶住、或停在原地抖,
/// 不加兜底就会**永不返回**。取 budget 的倍数而不是绝对秒数 —— 想要更长的总窗,
/// 该做的是调大 budget, 而不是指望这条兜底变长。
static constexpr double kArriveHardCapFactor = 4.0;

RobotState Arm::arrive(const std::vector<double>& target, double timeout) {
Ack& a = require();
const std::vector<double> tgt = target;
Expand All @@ -950,9 +957,35 @@ RobotState Arm::arrive(const std::vector<double>& target, double timeout) {
}
return true;
};
// 下面两个只服务报错文案。max_q_err 尺寸不符时返回 -1 = "取不到", 由调用方兜。
auto max_q_err = [&](const RobotState& st) {
if (int(st.joints.size()) != int(tgt.size())) return -1.0;
const auto qs = st.q();
double m = 0.0;
for (size_t i = 0; i < tgt.size(); ++i) {
const double d = std::fabs(qs[i] - tgt[i]);
if (d > m) m = d;
}
return m;
};
auto max_abs_dq = [](const RobotState& st) {
double m = 0.0;
for (double d : st.dq()) {
const double ad = std::fabs(d);
if (ad > m) m = ad;
}
return m;
};
int n_ok = 0; // 连续到位拍数 (防到位瞬间误判/抖动)
uint64_t seq0 = a.status_seq_now();
const double end = clock().now_s() + budget;
const double t0 = clock().now_s();
// ★ [S2] 预算是"**无进展**预算", 不是墙钟预算: 只要臂还在按有意义的速率动
// (max|dq| >= dq_tol —— 与到位判据**同一把尺子**), 就把 deadline 往后推。
// 旧实现在大行程上会把**正常推进中**的动作判成 "未到位, 超时 15s": 实测从
// J4=-3.06 回零 (要走 175°) 就是这样 —— 报"超时"时 J4 的 dq 还是 0.21 rad/s,
// 臂在正常走。把成功报成失败, 比慢一点更糟。
double end = t0 + budget;
const double hard_end = t0 + kArriveHardCapFactor * budget;
Ack::MuLock lk(a.mu, a.dbg_mu_owner);
a.dbg_check("arrive");
while (true) {
Expand All @@ -966,12 +999,29 @@ RobotState Arm::arrive(const std::vector<double>& target, double timeout) {
}
n_ok = done(*a.state) ? n_ok + 1 : 0;
if (n_ok >= arrive_frames) return *a.state;
if (max_abs_dq(*a.state) >= dq_tol) end = clock().now_s() + budget; // [S2] 续期
}
const double left = end - clock().now_s();
const double now = clock().now_s();
if (now >= hard_end) break;
const double left = end - now;
if (left <= 0.0) break;
a.cv.wait_for(lk.ul(), std::chrono::duration<double>(left));
}
throw MotionTimeoutError("未到位, 超时 " + fnum(budget) + "s");
// ★ [S1] 报错前先看最后一帧: "**仍在运动**而没等到" 与 "**停住了**却没到位" 是
// 两件事, 旧实现一律报 "未到位, 超时 Ns", 现场于是把大行程的正常动作当失败。
const double elapsed = clock().now_s() - t0;
const double last_dq = a.state ? max_abs_dq(*a.state) : 0.0;
if (a.state && last_dq >= dq_tol) {
std::string tail;
const double e = max_q_err(*a.state);
if (e >= 0.0) tail = ", 距目标还差 " + fnum(e) + " rad";
throw MotionTimeoutError("未到位: 等到 " + fnum(elapsed) + "s 时臂**仍在运动** (max|dq|=" +
fnum(last_dq) + " rad/s" + tail +
") —— 不是失败, 是没等够; 调大 move_timeout, 或给 "
"home()/arrive() 传更长的 timeout");
}
throw MotionTimeoutError("未到位, 超时 " + fnum(budget) + "s (已静止: max|dq|=" +
fnum(last_dq) + " rad/s)");
}

// ===========================================================================
Expand Down Expand Up @@ -1442,7 +1492,7 @@ std::optional<CartPlan> Arm::poll_cart() {
const auto tok = cart_->claim_resolved();
if (!tok || !tok->reply) return std::nullopt;
CartPlan plan = CartPlan::from_reply(*tok->reply);
raise_for_plan(plan);
raise_for_plan(plan, this);
return plan;
}

Expand Down Expand Up @@ -1537,6 +1587,21 @@ void print_state(const Msg<std::optional<RobotState>>& m) {
}
std::cout << " hz=" << fnum(m.hz) << " age=" << fnum(now_s() - m.timestamp, 2)
<< "s\n";
// 固件 G7 断轴位图 (1.5.0 起上报; 旧布局恒 0)。**必须显式打**: 0x06 那句报错
// ("锁存, 须先 RESET -- EMERGENCY 或 joint_fault 非 0") 叫用户去查 joint_fault,
// 而本行原先只打 flags 与每关节 err -- 那是**另一个**字段, 现场看不出是哪个轴被断。
char jf[8];
std::snprintf(jf, sizeof(jf), "%04X", unsigned(st.joint_fault));
std::cout << "joint_fault=0x" << jf;
const std::vector<int> bad = st.fault_axes();
if (!bad.empty()) {
std::cout << " 断轴=";
for (size_t i = 0; i < bad.size(); ++i) {
if (i) std::cout << ",";
std::cout << "J" << (bad[i] + 1);
}
}
std::cout << "\n";
for (size_t i = 0; i < st.joints.size(); ++i) {
const auto& j = st.joints[i];
char buf[256];
Expand Down
55 changes: 52 additions & 3 deletions src/cart.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,7 +39,54 @@ CartPlan CartPlan::from_reply(const std::vector<uint8_t>& payload) {
return p;
}

void raise_for_plan(const CartPlan& plan) {
namespace {

/// err==CART_ERR_LIMIT 时补一句"是哪个轴、差多少、下一步做什么"。
///
/// 为什么只能本地推: 固件 0x4E 的载荷是 8B (ok/err/n_wp/plan_us), **不带轴号** ——
/// "路点或段中插值越关节限位"这句话本身不指向任何一根轴, 现场只能靠猜。
///
/// best-effort 三条底线:
/// ① 只读**本地缓存** (`get_state(false)` = 最近一帧被动状态帧, 不发帧) —— 错误路径上
/// 绝不再加链路动作;
/// ② 任何一步取不到就返回空串, 消息退化成原样;
/// ③ 整个函数**不抛** —— 错误路径上再冒一个异常, 才是真把原始错误盖掉。
std::string tight_axis_hint(Arm* arm) {
if (arm == nullptr) return {};
try {
const auto st = arm->get_state(false).value;
if (!st) return {};
const auto jps = arm->params().all_joint_params();
if (jps.empty() || jps.size() != st->joints.size()) return {};

size_t tight = 0;
double best = -1.0;
for (size_t i = 0; i < jps.size(); ++i) {
const double lo = st->joints[i].q - jps[i].q_min;
const double hi = jps[i].q_max - st->joints[i].q;
const double m = (lo < hi) ? lo : hi;
if (best < 0.0 || m < best) {
best = m;
tight = i;
}
}
char buf[512];
std::snprintf(buf, sizeof(buf),
" [轴号不在 0x4E 载荷里, 本地按最近一帧状态推断: 最紧的是 J%zu, "
"距软限位仅 %.1f mrad (q=%+.4f, 限位 %+.4f..%+.4f)。规划器按软限位"
"零容差硬判 => 先 movej 到关节空间中部的\"工作位形\"再发笛卡尔 "
"(例 [0,-0.5,0,-1.0,0,0,0])]",
tight + 1, best * 1000.0, st->joints[tight].q,
jps[tight].q_min, jps[tight].q_max);
return buf;
} catch (...) {
return {};
}
}

} // namespace

void raise_for_plan(const CartPlan& plan, Arm* arm) {
if (plan.ok) return;
const int err = plan.err;
if (err == CART_ERR_IK) {
Expand All @@ -62,8 +109,10 @@ void raise_for_plan(const CartPlan& plan) {
case CART_ERR_LIMIT: detail = "路点或段中插值越关节限位"; break;
default: detail = "未登记的 err=" + std::to_string(err); break;
}
std::string hint;
if (err == CART_ERR_LIMIT) hint = tight_axis_hint(arm);
throw CartesianPlanError("固件笛卡尔规划失败: " + detail + " (err=" +
std::to_string(err) + ")");
std::to_string(err) + ")" + hint);
}

// ===========================================================================
Expand Down Expand Up @@ -549,7 +598,7 @@ struct CartImpl {
}
const auto reply = arm->cart_->wait(tok, arm->move_timeout);
CartPlan plan = CartPlan::from_reply(reply);
raise_for_plan(plan);
raise_for_plan(plan, arm);
if (wait) {
const SettleInfo info = wait_settled(arm, label, seq0);
plan.started_busy = info.started_busy;
Expand Down
4 changes: 2 additions & 2 deletions src/testing.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -302,7 +302,7 @@ void FakeTransport::write_frame(uint8_t cmd, const uint8_t* payload, size_t len)
auto_status = true;
push(make_status(q, {}, 0, status_flags(enabled), 0, 0, n));
} else if (cmd == proto::CMD_GET_STATUS) {
push(make_status(q, {}, 1, status_flags(enabled), 0, 0, n));
push(make_status(q, dq_override.value_or(std::vector<double>{}), 1, status_flags(enabled), 0, 0, n));
} else if (cmd == proto::CMD_SET_FF_VEC) { // 存下来供 0x2B 读回
if (!p.empty()) ff_vec[int(p[0])] = proto::unpack_f32s(p.data(), 1, n);
push(proto::pack_frame(proto::RSP_ACK, std::vector<uint8_t>{cmd}));
Expand Down Expand Up @@ -766,7 +766,7 @@ std::optional<proto::Frame> FakeTransport::read_frame(double timeout_in) {
const double now = now_s();
if (now - auto_status_last_ < auto_status_period) return std::nullopt;
auto_status_last_ = now;
fr = proto::unpack_frame(make_status(q, {}, 1, status_flags(enabled), 0, 0, n));
fr = proto::unpack_frame(make_status(q, dq_override.value_or(std::vector<double>{}), 1, status_flags(enabled), 0, 0, n));
}
return stamp_status(fr);
}
Expand Down
32 changes: 32 additions & 0 deletions tests/test_cart.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -547,3 +547,35 @@ TEST(cart_error_code_constants_match_the_firmware_enum) {
CHECK_EQ(CART_ERR_BADARG, 6);
CHECK_EQ(CartPlan::ERR_REPLY_LOST, -1);
}

TEST(cart_err_limit_names_the_tightest_axis_and_points_at_the_fix) {
// 固件 0x4E 载荷只有 8B, **不带轴号** => "是哪个轴越限"只能 SDK 侧拿
// "最近一帧状态里的 q + 软限位"本地推。本用例钉住: 这条报错必须点出**最紧的轴**
// 并给出下一步 —— 否则现场只看到"插值越关节限位"而不知所指。
lt::Offline off;
off.t().cart_err_override = CART_ERR_LIMIT;
// 先把状态缓存灌上 (实现只读本地缓存, 不在错误路径上发帧)
off->get_state(true);
// 期望值由**夹具自己的状态**独立推出, 不抄实现
const auto st = off->get_state(false).value;
CHECK(st.has_value());
const auto jps = off->params().all_joint_params();
CHECK_EQ(jps.size(), st->joints.size());
size_t tightest = 0;
double best = 1e30;
for (size_t i = 0; i < jps.size(); ++i) {
const double lo = st->joints[i].q - jps[i].q_min;
const double hi = jps[i].q_max - st->joints[i].q;
const double m = (lo < hi) ? lo : hi;
if (m < best) { best = m; tightest = i; }
}
try {
off->move_l(kP2, 0.5);
FAIL("CART_ERR_LIMIT 应当抛 CartesianPlanError");
} catch (const CartesianPlanError& e) {
const std::string m = e.what();
CHECK(m.find("越关节限位") != std::string::npos); // 原有信息仍在
CHECK(m.find("J" + std::to_string(tightest + 1)) != std::string::npos);
CHECK(m.find("工作位形") != std::string::npos);
}
}
44 changes: 44 additions & 0 deletions tests/test_commands.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -429,3 +429,47 @@ TEST(cmd_submodules_are_lazily_created_and_stable) {
CHECK(&off->log() == &off->log());
CHECK(&off->diag() == &off->diag());
}

TEST(cmd_arrive_keeps_waiting_while_the_arm_is_still_moving) {
// [S2] move_timeout 是"**无进展**预算", 不是墙钟预算: 臂还在按有意义的速率动
// (max|dq| >= dq_tol, 与到位判据同一把尺子) 就不该判超时。旧实现在这里报
// "未到位, 超时 0.2s" —— 把**正常推进中**的动作报成失败。真机实测: 从 J4=-3.06
// 回零要走 175°, 报超时时 J4 的 dq 还有 0.21 rad/s (臂在正常走)。
lt::Offline off;
off->move_timeout = 0.2;
// 桩会随命令更新 q (home 让它归零 => 立刻就"到位"), 所以用旋钮把到位判据
// **关掉**: q_tol=0 时 |q-tgt| >= 0 恒真 => done() 永不满足 => 桩"到不了"。
off->q_tol = 0.0;
off->dq_tol = 0.05;
off.t().dq_override = std::vector<double>(7, 0.5); // 7 轴都在动 (> dq_tol)
const uint64_t seq0 = off->status_seq();
try {
off->home(-1.0);
FAIL("应当抛 MotionTimeoutError");
} catch (const MotionTimeoutError& e) {
const std::string m = e.what();
CHECK(m.find("仍在运动") != std::string::npos); // [S1] 说清是哪一种超时
CHECK(m.find("没等够") != std::string::npos); // 并给出下一步
}
// 续期确实发生: 等待远超一个 budget (0.2s @~1kHz ≈ 200 帧), 但被硬上限 4× 兜住
const uint64_t used = off->status_seq() - seq0;
CHECK(used > 300);
CHECK(used < 4000);
}

TEST(cmd_arrive_still_reports_a_real_stall_as_a_timeout) {
// 另一半: **停住了却没到位**必须仍是"未到位, 超时" —— 别把判据放松成
// "永远报还在动", 那会把真卡死也瞒过去。
lt::Offline off;
off->move_timeout = 0.2;
off->q_tol = 0.0; // 到位判据永不满足; dq 不覆写 => 桩报空 dq => 判"已静止"
try {
off->home(-1.0);
FAIL("应当抛 MotionTimeoutError");
} catch (const MotionTimeoutError& e) {
const std::string m = e.what();
CHECK(m.find("未到位, 超时") != std::string::npos);
CHECK(m.find("已静止") != std::string::npos);
CHECK(m.find("仍在运动") == std::string::npos);
}
}
20 changes: 20 additions & 0 deletions tests/test_protocol_sync.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,12 +73,19 @@ std::vector<std::string> split_lines(const std::string& s) {
std::string cur;
for (char c : s) {
if (c == '\n') {
// 吃掉落行符前的 CR: 固件树若是 CRLF 检出 (Windows 上 git archive, 或
// 无 autocrlf 的克隆), 行尾会带 CR —— 而 ECMAScript 的 `.` 不匹配 CR,
// 下面 fw_defines 的 `(.*)$` 吸不掉它 -> 整行匹配失败 -> CMD_/RSP_ 一条
// 都解析不出 -> 下游判据全报成「漂移」(实测: CRLF 树 6/12 FAIL, 提示误导)。
if (!cur.empty() && cur.back() == '\r') cur.pop_back();
out.push_back(cur);
cur.clear();
} else {
cur.push_back(c);
}
}
// 末尾那行也可能带 CR (文件不以换行结尾时走不到循环内的分支)。
if (!cur.empty() && cur.back() == '\r') cur.pop_back();
out.push_back(cur);
return out;
}
Expand Down Expand Up @@ -588,3 +595,16 @@ TEST(sync_firmware_version_not_below_sdk_minimum) {
}
if (!bad.empty()) FAIL("固件自报版本不满足 SDK 最低要求:" + bad);
}

TEST(protocol_sync_split_lines_tolerates_crlf) {
// 固件树若是 CRLF 检出 (Windows 上 git archive / 无 autocrlf 的克隆), 行尾会带 CR;
// 而 ECMAScript 的 `.` **不匹配 CR**, 于是 fw_defines 的 `(.*)$` 吸不掉它 —— 整行
// 匹配失败 => CMD_/RSP_ 一条都解析不出 => 下游判据全报成「漂移」。实测(修之前):
// 同一棵 CRLF 固件树 6/12 FAIL, 而且打出 48 条"SDK 声明了固件没有的命令" —— 全是
// 解析器空转的假象。本用例钉住 split_lines 必须把 CR 吃掉。
const std::vector<std::string> ls = split_lines("a\r\nb\r\nc");
CHECK_EQ(ls.size(), size_t(3));
CHECK_EQ(ls[0], std::string("a"));
CHECK_EQ(ls[1], std::string("b"));
CHECK_EQ(ls[2], std::string("c"));
}
Loading