From 2bd4138a86c11ab134dbef0c6d89bfca2093e93e Mon Sep 17 00:00:00 2001 From: justagist Date: Thu, 15 Oct 2026 14:16:03 +0000 Subject: [PATCH 1/2] retry motor limit queries until every motor replies or a timeout --- .../piper_cpp/interface/piper_interface_v2.h | 10 ++- .../src/interface/piper_interface_v2.cpp | 20 +++++ piper_cpp/test/piper_interface_v2_test.cpp | 87 +++++++++++++++++++ 3 files changed, 116 insertions(+), 1 deletion(-) diff --git a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h b/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h index 2346e75..2eac2e7 100644 --- a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h +++ b/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h @@ -87,6 +87,14 @@ class PiperInterfaceV2 /// want to re-discover its limits. void piperInit(); + /// Block until every motor has answered both limit queries, re-sending the queries every + /// `retry_interval` so a lost frame does not leave the aggregates invalid for the session. + /// Requires the read thread to be running. Returns false if `timeout` passes first. + bool waitForMotorLimits( + std::chrono::milliseconds timeout = std::chrono::milliseconds{2000}, + std::chrono::milliseconds retry_interval = std::chrono::milliseconds{200} + ); + // ---- State getters -------------------------------------------------------------------- // // All getters below return a `StateSnapshot` that bundles the latest received message @@ -170,7 +178,7 @@ class PiperInterfaceV2 /// Per-motor angle/speed limits for all six motors. The arm fills this in after the search /// queries that `connectPort()` fires automatically; ready within a second of opening. - /// `is_valid` only once every motor has replied. + /// `is_valid` only once every motor has replied; `waitForMotorLimits()` retries until then. StateSnapshot getArmAllMotorAngleLimitMaxSpd() const { return StateSnapshot(arm_all_motor_angle_limit_max_spd_); diff --git a/piper_cpp/src/interface/piper_interface_v2.cpp b/piper_cpp/src/interface/piper_interface_v2.cpp index 2afe869..5a44430 100644 --- a/piper_cpp/src/interface/piper_interface_v2.cpp +++ b/piper_cpp/src/interface/piper_interface_v2.cpp @@ -932,6 +932,26 @@ bool PiperInterfaceV2::searchAllMotorLimits() return true; } +bool PiperInterfaceV2::waitForMotorLimits(std::chrono::milliseconds timeout, std::chrono::milliseconds retry_interval) +{ + using clock = std::chrono::steady_clock; + const auto deadline = clock::now() + timeout; + auto last_sent = clock::now(); + searchAllMotorLimits(); + while (!(arm_all_motor_angle_limit_max_spd_.isValid() && arm_all_motor_max_acc_limit_.isValid())) + { + if (clock::now() >= deadline) + return false; + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + if (clock::now() - last_sent >= retry_interval) + { + searchAllMotorLimits(); + last_sent = clock::now(); + } + } + return true; +} + bool PiperInterfaceV2::sendFirmwareQuery() { if (!connected_.load(std::memory_order_relaxed)) diff --git a/piper_cpp/test/piper_interface_v2_test.cpp b/piper_cpp/test/piper_interface_v2_test.cpp index e86bdea..da7e6c7 100644 --- a/piper_cpp/test/piper_interface_v2_test.cpp +++ b/piper_cpp/test/piper_interface_v2_test.cpp @@ -6,6 +6,7 @@ #include #include +#include #include #include #include @@ -14,6 +15,7 @@ #include #include #include +#include #include using namespace std::chrono_literals; @@ -287,3 +289,88 @@ TEST(PiperInterfaceV2, FirmwareVersionNeedsTheWholeTagAndRestartsPerQuery) ASSERT_TRUE(waitFor([&] { return firmwareBytes(8); })); EXPECT_EQ(bus.piper->getFirmwareVersion(), "S-V2.0-1"); } + +namespace +{ + +// Plays the arm's side of the limit queries on the peer end: every 0x472 gets its 0x473 or +// 0x47C reply, except those listed in `drop_once`, which go unanswered the first time. +struct LimitResponder +{ + int peer; + std::vector> drop_once; // {motor, search_content} + std::atomic stop{false}; + std::atomic rounds{0}; // counted by motor 1's angle query + std::thread thread; + + LimitResponder(int peer_fd, std::vector> drop) + : peer(peer_fd), drop_once(std::move(drop)), thread([this] { run(); }) + { + } + ~LimitResponder() + { + stop = true; + thread.join(); + } + + void run() + { + while (!stop) + { + pollfd pfd{peer, POLLIN, 0}; + if (poll(&pfd, 1, 20) <= 0) + continue; + can_frame q{}; + if (::recv(peer, &q, sizeof(q), 0) != static_cast(sizeof(q)) || q.can_id != 0x472) + continue; + const uint8_t motor = q.data[0], content = q.data[1]; + if (motor == 1 && content == 1) + ++rounds; + auto it = std::find(drop_once.begin(), drop_once.end(), std::make_pair(motor, content)); + if (it != drop_once.end()) + { + drop_once.erase(it); + continue; + } + const auto reply = + content == 1 ? frame(0x473, {motor, 0, motor, 0, 0, 0, 0}) : frame(0x47C, {motor, 0, motor}); + ::send(peer, &reply, sizeof(reply), 0); + } + } +}; + +} // namespace + +TEST(PiperInterfaceV2, WaitForMotorLimitsRetriesLostReplies) +{ + auto bus = makeBus(); + ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true)); + LimitResponder arm( + bus.peer, + { + {3, 1}, + {5, 2} + } + ); // motor 3's angle reply and motor 5's acc reply get lost once + + EXPECT_TRUE(bus.piper->waitForMotorLimits(2s, 50ms)); + EXPECT_GE(arm.rounds.load(), 2) << "the missing replies can only arrive after a re-query"; + const auto angles = bus.piper->getArmAllMotorAngleLimitMaxSpd(); + const auto accs = bus.piper->getArmAllMotorMaxAccLimit(); + ASSERT_TRUE(angles.is_valid); + ASSERT_TRUE(accs.is_valid); + EXPECT_EQ(angles.value.motor[2].max_angle_limit, 3); + EXPECT_EQ(accs.value.motor[4].max_joint_acc, 5); +} + +TEST(PiperInterfaceV2, WaitForMotorLimitsGivesUpAtTheTimeout) +{ + auto bus = makeBus(); + ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true)); + // Nobody answers. The few dozen queries sent before the timeout fit in the socket buffer, + // so the sends never block. + const auto t0 = steady_clock::now(); + EXPECT_FALSE(bus.piper->waitForMotorLimits(300ms, 50ms)); + EXPECT_LT(std::chrono::duration_cast(steady_clock::now() - t0).count(), 1000); + EXPECT_FALSE(bus.piper->getArmAllMotorAngleLimitMaxSpd().is_valid); +} From 278dd7bef914ecf4e79f6741b2ce6f534a980a28 Mon Sep 17 00:00:00 2001 From: justagist Date: Thu, 15 Oct 2026 15:57:03 +0000 Subject: [PATCH 2/2] make waitForMotorLimits demand fresh replies and fail fast when disconnected --- .../piper_cpp/interface/piper_interface_v2.h | 7 ++++--- piper_cpp/src/interface/piper_interface_v2.cpp | 6 +++++- piper_cpp/test/piper_interface_v2_test.cpp | 16 ++++++++++------ 3 files changed, 19 insertions(+), 10 deletions(-) diff --git a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h b/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h index 2eac2e7..53c86e1 100644 --- a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h +++ b/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h @@ -87,9 +87,10 @@ class PiperInterfaceV2 /// want to re-discover its limits. void piperInit(); - /// Block until every motor has answered both limit queries, re-sending the queries every - /// `retry_interval` so a lost frame does not leave the aggregates invalid for the session. - /// Requires the read thread to be running. Returns false if `timeout` passes first. + /// Discard the cached limits, then block until every motor has answered both limit queries, + /// re-sending them every `retry_interval` so a lost frame does not leave the aggregates + /// invalid. Requires the read thread to be running. Returns false if the first send fails + /// or `timeout` passes first. bool waitForMotorLimits( std::chrono::milliseconds timeout = std::chrono::milliseconds{2000}, std::chrono::milliseconds retry_interval = std::chrono::milliseconds{200} diff --git a/piper_cpp/src/interface/piper_interface_v2.cpp b/piper_cpp/src/interface/piper_interface_v2.cpp index 5a44430..cf99c08 100644 --- a/piper_cpp/src/interface/piper_interface_v2.cpp +++ b/piper_cpp/src/interface/piper_interface_v2.cpp @@ -936,8 +936,12 @@ bool PiperInterfaceV2::waitForMotorLimits(std::chrono::milliseconds timeout, std { using clock = std::chrono::steady_clock; const auto deadline = clock::now() + timeout; + // Success must mean replies to these queries, not whatever an earlier session cached. + arm_all_motor_angle_limit_max_spd_.reset(); + arm_all_motor_max_acc_limit_.reset(); auto last_sent = clock::now(); - searchAllMotorLimits(); + if (!searchAllMotorLimits()) + return false; // not connected or the bus is down: waiting cannot help while (!(arm_all_motor_angle_limit_max_spd_.isValid() && arm_all_motor_max_acc_limit_.isValid())) { if (clock::now() >= deadline) diff --git a/piper_cpp/test/piper_interface_v2_test.cpp b/piper_cpp/test/piper_interface_v2_test.cpp index da7e6c7..6016718 100644 --- a/piper_cpp/test/piper_interface_v2_test.cpp +++ b/piper_cpp/test/piper_interface_v2_test.cpp @@ -345,22 +345,24 @@ TEST(PiperInterfaceV2, WaitForMotorLimitsRetriesLostReplies) { auto bus = makeBus(); ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true)); + // The very last reply of the round goes missing once, so the angle aggregate is complete a + // whole retry round before the acc aggregate: waiting for only one of them would return + // with the other still invalid. LimitResponder arm( bus.peer, { - {3, 1}, - {5, 2} + {6, 2} } - ); // motor 3's angle reply and motor 5's acc reply get lost once + ); EXPECT_TRUE(bus.piper->waitForMotorLimits(2s, 50ms)); - EXPECT_GE(arm.rounds.load(), 2) << "the missing replies can only arrive after a re-query"; + EXPECT_GE(arm.rounds.load(), 2) << "the missing reply can only arrive after a re-query"; const auto angles = bus.piper->getArmAllMotorAngleLimitMaxSpd(); const auto accs = bus.piper->getArmAllMotorMaxAccLimit(); ASSERT_TRUE(angles.is_valid); ASSERT_TRUE(accs.is_valid); EXPECT_EQ(angles.value.motor[2].max_angle_limit, 3); - EXPECT_EQ(accs.value.motor[4].max_joint_acc, 5); + EXPECT_EQ(accs.value.motor[5].max_joint_acc, 6); } TEST(PiperInterfaceV2, WaitForMotorLimitsGivesUpAtTheTimeout) @@ -371,6 +373,8 @@ TEST(PiperInterfaceV2, WaitForMotorLimitsGivesUpAtTheTimeout) // so the sends never block. const auto t0 = steady_clock::now(); EXPECT_FALSE(bus.piper->waitForMotorLimits(300ms, 50ms)); - EXPECT_LT(std::chrono::duration_cast(steady_clock::now() - t0).count(), 1000); + const auto waited = std::chrono::duration_cast(steady_clock::now() - t0).count(); + EXPECT_GE(waited, 300) << "gave up before the timeout"; + EXPECT_LT(waited, 1000); EXPECT_FALSE(bus.piper->getArmAllMotorAngleLimitMaxSpd().is_valid); }