From 26ad2f183ff2ba9d9d598dd019aaf72075efb722 Mon Sep 17 00:00:00 2001 From: justagist Date: Thu, 15 Oct 2026 09:15:49 +0000 Subject: [PATCH 1/7] make CAN receive interruptible and drop the read loop sleep --- piper_cpp/CMakeLists.txt | 6 + piper_cpp/README.md | 13 +- .../piper_cpp/interface/piper_interface_v2.h | 14 ++- .../include/piper_cpp/std_can_interface.h | 68 +++++++++- .../src/interface/piper_interface_v2.cpp | 19 ++- piper_cpp/test/CMakeLists.txt | 12 ++ piper_cpp/test/piper_interface_v2_test.cpp | 106 ++++++++++++++++ piper_cpp/test/std_can_interface_test.cpp | 117 ++++++++++++++++++ 8 files changed, 342 insertions(+), 13 deletions(-) create mode 100644 piper_cpp/test/CMakeLists.txt create mode 100644 piper_cpp/test/piper_interface_v2_test.cpp create mode 100644 piper_cpp/test/std_can_interface_test.cpp diff --git a/piper_cpp/CMakeLists.txt b/piper_cpp/CMakeLists.txt index 13d3973..636f763 100644 --- a/piper_cpp/CMakeLists.txt +++ b/piper_cpp/CMakeLists.txt @@ -52,6 +52,12 @@ add_piper_executable(piper_set_role piper_set_role.cpp) add_piper_executable(piper_read_firmware piper_read_firmware.cpp) add_piper_executable(piper_reset piper_reset.cpp) +option(BUILD_TESTING "Build the unit tests (requires GTest)" ON) +if(BUILD_TESTING) + enable_testing() + add_subdirectory(test) +endif() + include(GNUInstallDirs) install(TARGETS piper_cpp EXPORT piper_cppTargets diff --git a/piper_cpp/README.md b/piper_cpp/README.md index 5748b86..f917830 100644 --- a/piper_cpp/README.md +++ b/piper_cpp/README.md @@ -33,13 +33,14 @@ bytes. - Linux with SocketCAN (any recent kernel). - `libsocketcan-dev` and `pkg-config`. +- `libgtest-dev` for the unit tests (pass `-DBUILD_TESTING=OFF` to skip them). - A C++17 compiler (`build-essential`). - `cmake >= 3.10`. - `can-utils` and `iproute2` are useful for bringing up the bus and sniffing frames (`candump`, `ip link`). ```bash -sudo apt install build-essential cmake pkg-config libsocketcan-dev can-utils iproute2 +sudo apt install build-essential cmake pkg-config libsocketcan-dev libgtest-dev can-utils iproute2 ``` ## Installation @@ -76,6 +77,16 @@ sudo cmake --install build The CLI binaries land in `build/` (or `${CMAKE_INSTALL_PREFIX}/bin` after install). The library installs as `libpiper_cpp.a` with headers under `include/piper_cpp/`. +### Running the tests + +The unit tests drive the CAN transport through a socketpair, so they need no adapter or arm: + +```bash +ctest --test-dir build --output-on-failure +# or, inside a colcon workspace: +colcon test --packages-select piper_cpp +``` + ## Bringing up the CAN bus The Piper expects a 1 Mbit/s bus on the standard `can` interface naming. If your CAN device 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 ba9a872..56ecf2b 100644 --- a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h +++ b/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h @@ -44,6 +44,14 @@ class PiperInterfaceV2 bool sdk_joint_limit = false, bool sdk_gripper_limit = false ); + /// Construct over an already-open transport, e.g. one adopting a socket the caller opened. + /// Mainly for tests, which drive the interface through a socketpair instead of a CAN bus. + /// The remaining parameters are as above. + PiperInterfaceV2( + std::unique_ptr can, bool dh_offset = true, bool sdk_joint_limit = false, + bool sdk_gripper_limit = false + ); + /// Destructor; calls `disconnectPort()` if it hasn't been called already. ~PiperInterfaceV2(); @@ -65,9 +73,9 @@ class PiperInterfaceV2 /// @return true if the port opened (or was already open); false on initialisation failure. bool connectPort(bool can_init = false, bool piper_init = true, bool start_threads = true); - /// Stop the background read thread (if running) and close the CAN port. Joins the read - /// thread synchronously up to `timeout`; the destructor calls this with the default. - /// Idempotent. + /// Stop the background read thread (if running) and close the CAN port. Wakes the read + /// thread out of its blocking receive and joins it, so this returns promptly even on a + /// silent bus. `timeout` is currently unused and kept for API compatibility. Idempotent. void disconnectPort(std::chrono::milliseconds timeout = std::chrono::milliseconds{100}); /// Fire the one-shot startup queries: per-motor angle/speed limits, per-motor acceleration diff --git a/piper_cpp/include/piper_cpp/std_can_interface.h b/piper_cpp/include/piper_cpp/std_can_interface.h index 452f1da..cb379c6 100644 --- a/piper_cpp/include/piper_cpp/std_can_interface.h +++ b/piper_cpp/include/piper_cpp/std_can_interface.h @@ -2,6 +2,7 @@ #define PIPER_CPP_STD_CAN_INTERFACE_H #include "piper_cpp/can_utils.h" +#include #include #include #include @@ -11,7 +12,9 @@ #include #include #include +#include #include +#include #include #include #include @@ -29,8 +32,8 @@ class StdCanInterface std::string channel_name = "can0", int expected_bitrate = 1000000, bool judge_flag = true, bool auto_init = true, CanCallback callback = nullptr ) - : channel_name_(std::move(channel_name)), expected_bitrate_(expected_bitrate), callback_(std::move(callback)), - sock_(-1) + : channel_name_(std::move(channel_name)), expected_bitrate_(expected_bitrate), sock_(-1), + wake_fd_(makeWakeFd()), callback_(std::move(callback)) { if (judge_flag) judgeCanInfo(); @@ -38,7 +41,23 @@ class StdCanInterface init(); } - ~StdCanInterface() { close(); } + /// Adopt an already-open socket that delivers one `can_frame` per datagram (a raw CAN socket, + /// or a socketpair in tests). Takes ownership of `socket_fd`; `init()` is a no-op. + explicit StdCanInterface(int socket_fd, CanCallback callback = nullptr) + : channel_name_(), expected_bitrate_(0), sock_(socket_fd), wake_fd_(makeWakeFd()), + callback_(std::move(callback)) + { + } + + // Owns file descriptors, so copying would double-close them. + StdCanInterface(const StdCanInterface&) = delete; + StdCanInterface& operator=(const StdCanInterface&) = delete; + + ~StdCanInterface() + { + close(); + ::close(wake_fd_); + } bool init() { @@ -70,6 +89,7 @@ class StdCanInterface perror("bind"); return false; } + drainWake(); // an interrupt left over from a previous session must not cancel the first read return true; } @@ -96,11 +116,30 @@ class StdCanInterface ); } + /// Block until one frame arrives and hand it to the callback, or until `interruptRead()` is + /// called. Returns true only when a frame was delivered to the callback. bool readCanMessage() { if (sock_ < 0) return false; + struct pollfd fds[2] = { + {sock_, POLLIN, 0}, + {wake_fd_, POLLIN, 0} + }; + int ready; + do + { + ready = poll(fds, 2, -1); + } while (ready < 0 && errno == EINTR); + if (ready < 0) + return false; + if (fds[1].revents & POLLIN) + { + drainWake(); + return false; + } + struct can_frame frame; struct msghdr msg = {}; struct iovec iov; @@ -139,6 +178,14 @@ class StdCanInterface return false; } + /// Wake a thread blocked in `readCanMessage()`, which then returns false. Safe to call from + /// any thread. If no read is in progress, the next `readCanMessage()` returns false once. + void interruptRead() { eventfd_write(wake_fd_, 1); } + + /// Replace the receive callback. Not synchronised with `readCanMessage()`; set it before + /// reading starts. + void setCallback(CanCallback callback) { callback_ = std::move(callback); } + bool sendCanMessage(uint32_t id, const std::vector& data) { if (sock_ < 0) @@ -178,9 +225,24 @@ class StdCanInterface return f.good(); } + static int makeWakeFd() + { + int fd = eventfd(0, EFD_NONBLOCK | EFD_CLOEXEC); + if (fd < 0) + throw std::runtime_error(std::string("eventfd: ") + strerror(errno)); + return fd; + } + + void drainWake() + { + eventfd_t value; + (void)eventfd_read(wake_fd_, &value); + } + std::string channel_name_; int expected_bitrate_; int sock_; + int wake_fd_; struct sockaddr_can addr_ { }; diff --git a/piper_cpp/src/interface/piper_interface_v2.cpp b/piper_cpp/src/interface/piper_interface_v2.cpp index 8469a47..c7a8e66 100644 --- a/piper_cpp/src/interface/piper_interface_v2.cpp +++ b/piper_cpp/src/interface/piper_interface_v2.cpp @@ -43,6 +43,15 @@ PiperInterfaceV2::PiperInterfaceV2( /// TODO: initialise other members as in python } +PiperInterfaceV2::PiperInterfaceV2( + std::unique_ptr can, bool dh_offset, bool sdk_joint_limit, bool sdk_gripper_limit +) + : PiperInterfaceV2("", /*judge_flag=*/false, /*auto_init=*/false, dh_offset, sdk_joint_limit, sdk_gripper_limit) +{ + arm_can_ = std::move(can); + arm_can_->setCallback([this](const struct can_frame &frame, double timestamp) { parseCANFrame(frame, timestamp); }); +} + PiperInterfaceV2::~PiperInterfaceV2() { disconnectPort(); } bool PiperInterfaceV2::connectPort(bool can_init, bool piper_init, bool start_threads) { @@ -82,14 +91,11 @@ void PiperInterfaceV2::piperInit() void PiperInterfaceV2::readLoop() { + // readCanMessage() blocks until a frame arrives or disconnectPort() interrupts it, so no + // sleep is needed and queued frames are drained as fast as they are parsed. while (!stop_read_.load(std::memory_order_relaxed)) { - if (arm_can_->readCanMessage()) - { - // Process the received CAN message - // The callback will handle parsing and updating state - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); // TODO: Adjust sleep duration as needed + arm_can_->readCanMessage(); } } @@ -100,6 +106,7 @@ void PiperInterfaceV2::disconnectPort(std::chrono::milliseconds timeout) return; // Already disconnected } stop_read_.store(true, std::memory_order_relaxed); + arm_can_->interruptRead(); if (read_thread_.joinable()) { read_thread_.join(); diff --git a/piper_cpp/test/CMakeLists.txt b/piper_cpp/test/CMakeLists.txt new file mode 100644 index 0000000..e8d2677 --- /dev/null +++ b/piper_cpp/test/CMakeLists.txt @@ -0,0 +1,12 @@ +find_package(GTest REQUIRED) +include(GoogleTest) + +# A regression in the shutdown path shows up as a hang, so bound every test. +function(add_piper_test name) + add_executable(${name} ${name}.cpp) + target_link_libraries(${name} PRIVATE piper_cpp GTest::gtest_main) + gtest_discover_tests(${name} PROPERTIES TIMEOUT 30) +endfunction() + +add_piper_test(std_can_interface_test) +add_piper_test(piper_interface_v2_test) diff --git a/piper_cpp/test/piper_interface_v2_test.cpp b/piper_cpp/test/piper_interface_v2_test.cpp new file mode 100644 index 0000000..c345eaa --- /dev/null +++ b/piper_cpp/test/piper_interface_v2_test.cpp @@ -0,0 +1,106 @@ +// Drives PiperInterfaceV2 through a socketpair in place of the CAN bus: frames written to the +// peer end reach the interface's read thread exactly as they would from a raw CAN socket. + +#include "piper_cpp/interface/piper_interface_v2.h" + +#include + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; +using piper_cpp::PiperInterfaceV2; +using piper_cpp::StdCanInterface; +using std::chrono::steady_clock; + +namespace +{ + +struct Bus +{ + int peer; + std::unique_ptr piper; // destroyed first, so the read thread is gone before peer closes + ~Bus() { ::close(peer); } +}; + +Bus makeBus() +{ + int sv[2]; + if (socketpair(AF_UNIX, SOCK_DGRAM, 0, sv) != 0) + throw std::runtime_error("socketpair failed"); + return {sv[1], std::make_unique(std::make_unique(sv[0]))}; +} + +// Joint feedback frames carry two big-endian int32 angles in 0.001 deg. +can_frame jointFeedbackFrame(uint32_t id, int32_t a, int32_t b) +{ + can_frame f{}; + f.can_id = id; + f.can_dlc = 8; + const auto ua = static_cast(a); + const auto ub = static_cast(b); + for (int i = 0; i < 4; ++i) + { + f.data[i] = static_cast(ua >> (24 - 8 * i)); + f.data[4 + i] = static_cast(ub >> (24 - 8 * i)); + } + return f; +} + +void sendFrame(int fd, const can_frame& f, steady_clock::time_point deadline) +{ + while (::send(fd, &f, sizeof(f), MSG_DONTWAIT) < 0 && errno == EAGAIN && steady_clock::now() < deadline) + std::this_thread::yield(); +} + +} // namespace + +TEST(PiperInterfaceV2, DisconnectReturnsPromptlyOnSilentBus) +{ + auto bus = makeBus(); + ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true)); + std::this_thread::sleep_for(50ms); // let the read thread block on traffic that never comes + + auto done = std::async(std::launch::async, [&] { bus.piper->disconnectPort(); }); + ASSERT_EQ(done.wait_for(2s), std::future_status::ready) << "disconnectPort() hung on a silent bus"; + EXPECT_FALSE(bus.piper->isConnected()); +} + +TEST(PiperInterfaceV2, ReadLoopKeepsUpWithBurst) +{ + // 3000 frames is well beyond what the socket buffer holds, so the sender only finishes + // quickly if the read thread drains as fast as frames arrive. With a 1 ms sleep per frame + // this took several seconds; the bound below excludes that by an order of magnitude. + constexpr int kSets = 1000; + auto bus = makeBus(); + ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true)); + + const auto deadline = steady_clock::now() + 10s; + const auto t0 = steady_clock::now(); + for (int n = 1; n <= kSets; ++n) + { + sendFrame(bus.peer, jointFeedbackFrame(0x2A5, n, -n), deadline); + sendFrame(bus.peer, jointFeedbackFrame(0x2A6, 2 * n, -2 * n), deadline); + sendFrame(bus.peer, jointFeedbackFrame(0x2A7, 3 * n, -3 * n), deadline); + } + const auto send_ms = std::chrono::duration_cast(steady_clock::now() - t0).count(); + EXPECT_LT(send_ms, 1000); + + // Every set must reach the state cache, the last one winning. + auto js = bus.piper->getArmJointStates(); + for (auto t = steady_clock::now(); js.value.joint_6 != -3 * kSets && steady_clock::now() - t < 2s;) + { + std::this_thread::sleep_for(1ms); + js = bus.piper->getArmJointStates(); + } + EXPECT_TRUE(js.is_valid); + EXPECT_EQ(js.value.joint_1, kSets); + EXPECT_EQ(js.value.joint_2, -kSets); + EXPECT_EQ(js.value.joint_3, 2 * kSets); + EXPECT_EQ(js.value.joint_4, -2 * kSets); + EXPECT_EQ(js.value.joint_5, 3 * kSets); + EXPECT_EQ(js.value.joint_6, -3 * kSets); +} diff --git a/piper_cpp/test/std_can_interface_test.cpp b/piper_cpp/test/std_can_interface_test.cpp new file mode 100644 index 0000000..fb7ecbb --- /dev/null +++ b/piper_cpp/test/std_can_interface_test.cpp @@ -0,0 +1,117 @@ +// Exercises StdCanInterface over a socketpair instead of a CAN adapter: each datagram written +// to the peer end arrives at the adopted end as one can_frame, which is exactly what a raw +// CAN socket delivers. + +#include "piper_cpp/std_can_interface.h" + +#include + +#include +#include +#include +#include +#include + +using namespace std::chrono_literals; +using piper_cpp::StdCanInterface; + +namespace +{ + +// Returns {adopted, peer}: the first is handed to a StdCanInterface, the second stays raw. +std::pair makeSocketPair() +{ + int sv[2]; + if (socketpair(AF_UNIX, SOCK_DGRAM, 0, sv) != 0) + throw std::runtime_error("socketpair failed"); + return {sv[0], sv[1]}; +} + +struct RawFd +{ + int fd; + ~RawFd() { ::close(fd); } +}; + +} // namespace + +TEST(StdCanInterface, InterruptReadUnblocksSilentSocket) +{ + auto [adopted, peer] = makeSocketPair(); + RawFd peer_guard{peer}; + StdCanInterface can(adopted, [](const can_frame&, double) {}); + + auto result = std::async(std::launch::async, [&] { return can.readCanMessage(); }); + ASSERT_EQ(result.wait_for(100ms), std::future_status::timeout) << "read should block while the bus is silent"; + + can.interruptRead(); + ASSERT_EQ(result.wait_for(1s), std::future_status::ready) << "interruptRead() must wake the blocked read"; + EXPECT_FALSE(result.get()); +} + +TEST(StdCanInterface, PendingInterruptCancelsOnlyTheNextRead) +{ + auto [adopted, peer] = makeSocketPair(); + RawFd peer_guard{peer}; + int frames = 0; + StdCanInterface can(adopted, [&](const can_frame&, double) { ++frames; }); + + can.interruptRead(); + can_frame f{}; + f.can_dlc = 1; + ASSERT_EQ(::send(peer, &f, sizeof(f), 0), static_cast(sizeof(f))); + + // The interrupt wins over queued traffic, so a disconnect on a busy bus still stops the + // reader; it is then consumed and must not swallow the frame behind it. + EXPECT_FALSE(can.readCanMessage()); + EXPECT_TRUE(can.readCanMessage()); + EXPECT_EQ(frames, 1); +} + +TEST(StdCanInterface, DeliversEveryFrameInOrder) +{ + constexpr uint32_t kFrames = 2000; + const auto deadline = std::chrono::steady_clock::now() + 5s; + + auto [adopted, peer] = makeSocketPair(); + RawFd peer_guard{peer}; + std::vector received; + StdCanInterface can(adopted, [&](const can_frame& f, double) { received.push_back(f.can_id); }); + + // The socket buffer holds far fewer than kFrames datagrams, so the producer only finishes + // if the reader keeps draining; MSG_DONTWAIT plus the deadline keep it from hanging if not. + std::thread producer( + [&, peer = peer] + { + can_frame f{}; + f.can_dlc = 1; + for (uint32_t id = 0; id < kFrames; ++id) + { + f.can_id = id; + while (::send(peer, &f, sizeof(f), MSG_DONTWAIT) < 0 && errno == EAGAIN && + std::chrono::steady_clock::now() < deadline) + std::this_thread::yield(); + } + } + ); + + std::promise done; + std::thread watchdog( + [&, finished = done.get_future()] + { + if (finished.wait_until(deadline) == std::future_status::timeout) + can.interruptRead(); + } + ); + + while (received.size() < kFrames && can.readCanMessage()) + { + } + done.set_value(); + watchdog.join(); + producer.join(); + + ASSERT_EQ(received.size(), kFrames); + for (uint32_t id = 0; id < kFrames; ++id) + ASSERT_EQ(received[id], id) << "frame " << id << " arrived out of order"; +} From 7a375d4389282b9c78f995a10f6c271e77ff877e Mon Sep 17 00:00:00 2001 From: justagist Date: Thu, 15 Oct 2026 09:15:49 +0000 Subject: [PATCH 2/7] keep socket closed when init fails --- piper_cpp/include/piper_cpp/std_can_interface.h | 15 ++++++++++----- piper_cpp/test/std_can_interface_test.cpp | 8 ++++++++ 2 files changed, 18 insertions(+), 5 deletions(-) diff --git a/piper_cpp/include/piper_cpp/std_can_interface.h b/piper_cpp/include/piper_cpp/std_can_interface.h index cb379c6..3d9af83 100644 --- a/piper_cpp/include/piper_cpp/std_can_interface.h +++ b/piper_cpp/include/piper_cpp/std_can_interface.h @@ -59,37 +59,42 @@ class StdCanInterface ::close(wake_fd_); } + /// Open and bind the raw CAN socket. On failure nothing is kept, so a later call retries + /// from scratch instead of reporting an unbound socket as open. bool init() { if (sock_ != -1) return true; - sock_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); - if (sock_ < 0) + int fd = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (fd < 0) { perror("socket"); return false; } int enable = 1; - if (setsockopt(sock_, SOL_SOCKET, SO_TIMESTAMP, &enable, sizeof(enable)) < 0) + if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMP, &enable, sizeof(enable)) < 0) perror("setsockopt SO_TIMESTAMP"); struct ifreq ifr; strncpy(ifr.ifr_name, channel_name_.c_str(), IFNAMSIZ); - if (ioctl(sock_, SIOCGIFINDEX, &ifr) < 0) + if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) { perror("ioctl"); + ::close(fd); return false; } addr_.can_family = AF_CAN; addr_.can_ifindex = ifr.ifr_ifindex; - if (bind(sock_, (struct sockaddr*)&addr_, sizeof(addr_)) < 0) + if (bind(fd, (struct sockaddr*)&addr_, sizeof(addr_)) < 0) { perror("bind"); + ::close(fd); return false; } drainWake(); // an interrupt left over from a previous session must not cancel the first read + sock_ = fd; return true; } diff --git a/piper_cpp/test/std_can_interface_test.cpp b/piper_cpp/test/std_can_interface_test.cpp index fb7ecbb..48d4e2b 100644 --- a/piper_cpp/test/std_can_interface_test.cpp +++ b/piper_cpp/test/std_can_interface_test.cpp @@ -115,3 +115,11 @@ TEST(StdCanInterface, DeliversEveryFrameInOrder) for (uint32_t id = 0; id < kFrames; ++id) ASSERT_EQ(received[id], id) << "frame " << id << " arrived out of order"; } + +TEST(StdCanInterface, InitFailureCanBeRetried) +{ + // No such interface, so init() fails at the ifindex lookup. Before the fix the half-open + // socket was kept and a second init() reported success. + StdCanInterface can("piper_cpp_no_such_iface", 1000000, /*judge_flag=*/false, /*auto_init=*/true); + EXPECT_FALSE(can.init()); +} From b121293ab86f7a66380d5265f5bb6b6c226003da Mon Sep 17 00:00:00 2001 From: justagist Date: Thu, 15 Oct 2026 09:33:47 +0000 Subject: [PATCH 3/7] link tests against the GTest target available on CMake 3.10 --- piper_cpp/test/CMakeLists.txt | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/piper_cpp/test/CMakeLists.txt b/piper_cpp/test/CMakeLists.txt index e8d2677..185c926 100644 --- a/piper_cpp/test/CMakeLists.txt +++ b/piper_cpp/test/CMakeLists.txt @@ -1,10 +1,12 @@ find_package(GTest REQUIRED) include(GoogleTest) -# A regression in the shutdown path shows up as a hang, so bound every test. +# A regression in the shutdown path shows up as a hang, so bound every test. GTest::Main is +# the target FindGTest provides on every CMake version this project supports; the newer +# gtest_main target only appeared in 3.20. function(add_piper_test name) add_executable(${name} ${name}.cpp) - target_link_libraries(${name} PRIVATE piper_cpp GTest::gtest_main) + target_link_libraries(${name} PRIVATE piper_cpp GTest::Main) gtest_discover_tests(${name} PROPERTIES TIMEOUT 30) endfunction() From f8cc70f08c4534aef13390196cf798bf82b95b08 Mon Sep 17 00:00:00 2001 From: justagist Date: Thu, 15 Oct 2026 09:33:51 +0000 Subject: [PATCH 4/7] create the wake fd after CAN validation can throw --- piper_cpp/include/piper_cpp/std_can_interface.h | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/piper_cpp/include/piper_cpp/std_can_interface.h b/piper_cpp/include/piper_cpp/std_can_interface.h index 3d9af83..fb7fe40 100644 --- a/piper_cpp/include/piper_cpp/std_can_interface.h +++ b/piper_cpp/include/piper_cpp/std_can_interface.h @@ -32,11 +32,13 @@ class StdCanInterface std::string channel_name = "can0", int expected_bitrate = 1000000, bool judge_flag = true, bool auto_init = true, CanCallback callback = nullptr ) - : channel_name_(std::move(channel_name)), expected_bitrate_(expected_bitrate), sock_(-1), - wake_fd_(makeWakeFd()), callback_(std::move(callback)) + : channel_name_(std::move(channel_name)), expected_bitrate_(expected_bitrate), sock_(-1), wake_fd_(-1), + callback_(std::move(callback)) { + // Validate before creating the eventfd: a throwing constructor runs no destructor. if (judge_flag) judgeCanInfo(); + wake_fd_ = makeWakeFd(); if (auto_init) init(); } From 206e9518c23d116cfe18ec8f7c75db1ce2504c7a Mon Sep 17 00:00:00 2001 From: justagist Date: Thu, 15 Oct 2026 09:33:56 +0000 Subject: [PATCH 5/7] close the CAN socket on receive errors instead of spinning --- .../include/piper_cpp/std_can_interface.h | 19 +++++++++- .../src/interface/piper_interface_v2.cpp | 6 ++- piper_cpp/test/piper_interface_v2_test.cpp | 2 +- piper_cpp/test/std_can_interface_test.cpp | 38 +++++++++++++++++-- 4 files changed, 58 insertions(+), 7 deletions(-) diff --git a/piper_cpp/include/piper_cpp/std_can_interface.h b/piper_cpp/include/piper_cpp/std_can_interface.h index fb7fe40..0e0697b 100644 --- a/piper_cpp/include/piper_cpp/std_can_interface.h +++ b/piper_cpp/include/piper_cpp/std_can_interface.h @@ -124,7 +124,9 @@ class StdCanInterface } /// Block until one frame arrives and hand it to the callback, or until `interruptRead()` is - /// called. Returns true only when a frame was delivered to the callback. + /// called. Returns true only when a frame was delivered to the callback: false after an + /// interrupt, for a datagram that is not a whole `can_frame` (dropped), and once the socket + /// has died (interface down, peer gone), after which `isOpen()` is false. bool readCanMessage() { if (sock_ < 0) @@ -161,7 +163,18 @@ class StdCanInterface msg.msg_controllen = sizeof(ctrlmsg); ssize_t nbytes = recvmsg(sock_, &msg, 0); - if (nbytes < 0) + if (nbytes < 0 && (errno == EAGAIN || errno == EINTR)) + return false; + if (nbytes <= 0) + { + // Error or EOF: the socket will report this on every poll, so close it rather than + // let callers spin on it. + if (nbytes < 0) + perror("recvmsg"); + close(); + return false; + } + if (nbytes != static_cast(sizeof(frame))) return false; double timestamp = 0.0; @@ -185,6 +198,8 @@ class StdCanInterface return false; } + bool isOpen() const { return sock_ >= 0; } + /// Wake a thread blocked in `readCanMessage()`, which then returns false. Safe to call from /// any thread. If no read is in progress, the next `readCanMessage()` returns false once. void interruptRead() { eventfd_write(wake_fd_, 1); } diff --git a/piper_cpp/src/interface/piper_interface_v2.cpp b/piper_cpp/src/interface/piper_interface_v2.cpp index c7a8e66..a6e4760 100644 --- a/piper_cpp/src/interface/piper_interface_v2.cpp +++ b/piper_cpp/src/interface/piper_interface_v2.cpp @@ -93,10 +93,14 @@ void PiperInterfaceV2::readLoop() { // readCanMessage() blocks until a frame arrives or disconnectPort() interrupts it, so no // sleep is needed and queued frames are drained as fast as they are parsed. - while (!stop_read_.load(std::memory_order_relaxed)) + while (!stop_read_.load(std::memory_order_relaxed) && arm_can_->isOpen()) { arm_can_->readCanMessage(); } + if (!stop_read_.load(std::memory_order_relaxed)) + { + std::cerr << "Error: CAN socket closed unexpectedly; read thread stopped.\n"; + } } void PiperInterfaceV2::disconnectPort(std::chrono::milliseconds timeout) diff --git a/piper_cpp/test/piper_interface_v2_test.cpp b/piper_cpp/test/piper_interface_v2_test.cpp index c345eaa..a87a2d5 100644 --- a/piper_cpp/test/piper_interface_v2_test.cpp +++ b/piper_cpp/test/piper_interface_v2_test.cpp @@ -29,7 +29,7 @@ struct Bus Bus makeBus() { int sv[2]; - if (socketpair(AF_UNIX, SOCK_DGRAM, 0, sv) != 0) + if (socketpair(AF_UNIX, SOCK_SEQPACKET, 0, sv) != 0) throw std::runtime_error("socketpair failed"); return {sv[1], std::make_unique(std::make_unique(sv[0]))}; } diff --git a/piper_cpp/test/std_can_interface_test.cpp b/piper_cpp/test/std_can_interface_test.cpp index 48d4e2b..e3e2e81 100644 --- a/piper_cpp/test/std_can_interface_test.cpp +++ b/piper_cpp/test/std_can_interface_test.cpp @@ -1,6 +1,7 @@ -// Exercises StdCanInterface over a socketpair instead of a CAN adapter: each datagram written +// Exercises StdCanInterface over a socketpair instead of a CAN adapter: each record written // to the peer end arrives at the adopted end as one can_frame, which is exactly what a raw -// CAN socket delivers. +// CAN socket delivers. SOCK_SEQPACKET keeps record boundaries like a datagram socket and, +// unlike SOCK_DGRAM, tells the survivor when the peer closes. #include "piper_cpp/std_can_interface.h" @@ -22,7 +23,7 @@ namespace std::pair makeSocketPair() { int sv[2]; - if (socketpair(AF_UNIX, SOCK_DGRAM, 0, sv) != 0) + if (socketpair(AF_UNIX, SOCK_SEQPACKET, 0, sv) != 0) throw std::runtime_error("socketpair failed"); return {sv[0], sv[1]}; } @@ -68,6 +69,37 @@ TEST(StdCanInterface, PendingInterruptCancelsOnlyTheNextRead) EXPECT_EQ(frames, 1); } +TEST(StdCanInterface, ShortRecordIsDroppedWithoutClosing) +{ + auto [adopted, peer] = makeSocketPair(); + RawFd peer_guard{peer}; + int frames = 0; + StdCanInterface can(adopted, [&](const can_frame&, double) { ++frames; }); + + const uint8_t partial[5] = {}; + ASSERT_EQ(::send(peer, partial, sizeof(partial), 0), 5); + EXPECT_FALSE(can.readCanMessage()); + EXPECT_EQ(frames, 0); + EXPECT_TRUE(can.isOpen()); + + can_frame f{}; + ASSERT_EQ(::send(peer, &f, sizeof(f), 0), static_cast(sizeof(f))); + EXPECT_TRUE(can.readCanMessage()); + EXPECT_EQ(frames, 1); +} + +TEST(StdCanInterface, PeerCloseClosesSocketInsteadOfSpinning) +{ + auto [adopted, peer] = makeSocketPair(); + int frames = 0; + StdCanInterface can(adopted, [&](const can_frame&, double) { ++frames; }); + + ::close(peer); + EXPECT_FALSE(can.readCanMessage()); + EXPECT_FALSE(can.isOpen()) << "a dead socket must be closed, or every later read would return immediately"; + EXPECT_EQ(frames, 0); +} + TEST(StdCanInterface, DeliversEveryFrameInOrder) { constexpr uint32_t kFrames = 2000; From 8cbbeeb324498dc9ed9229b5d86f106ed5d539fb Mon Sep 17 00:00:00 2001 From: justagist Date: Thu, 15 Oct 2026 09:33:58 +0000 Subject: [PATCH 6/7] enable timestamps on adopted sockets and document their limits --- piper_cpp/include/piper_cpp/interface/piper_interface_v2.h | 3 ++- piper_cpp/include/piper_cpp/std_can_interface.h | 6 +++++- 2 files changed, 7 insertions(+), 2 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 56ecf2b..8d0daa6 100644 --- a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h +++ b/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h @@ -46,7 +46,8 @@ class PiperInterfaceV2 /// Construct over an already-open transport, e.g. one adopting a socket the caller opened. /// Mainly for tests, which drive the interface through a socketpair instead of a CAN bus. - /// The remaining parameters are as above. + /// `can` must not be null and is single-use: once disconnected, `connectPort()` cannot + /// reopen it. The remaining parameters are as above. PiperInterfaceV2( std::unique_ptr can, bool dh_offset = true, bool sdk_joint_limit = false, bool sdk_gripper_limit = false diff --git a/piper_cpp/include/piper_cpp/std_can_interface.h b/piper_cpp/include/piper_cpp/std_can_interface.h index 0e0697b..f9b483d 100644 --- a/piper_cpp/include/piper_cpp/std_can_interface.h +++ b/piper_cpp/include/piper_cpp/std_can_interface.h @@ -44,11 +44,15 @@ class StdCanInterface } /// Adopt an already-open socket that delivers one `can_frame` per datagram (a raw CAN socket, - /// or a socketpair in tests). Takes ownership of `socket_fd`; `init()` is a no-op. + /// or a socketpair in tests). Takes ownership of `socket_fd`. The transport is single-use: + /// `init()` is a no-op while it is open and cannot reopen it after `close()`. explicit StdCanInterface(int socket_fd, CanCallback callback = nullptr) : channel_name_(), expected_bitrate_(0), sock_(socket_fd), wake_fd_(makeWakeFd()), callback_(std::move(callback)) { + int enable = 1; + if (setsockopt(sock_, SOL_SOCKET, SO_TIMESTAMP, &enable, sizeof(enable)) < 0) + perror("setsockopt SO_TIMESTAMP"); } // Owns file descriptors, so copying would double-close them. From 81892cf127b05e3944b011019da961df00ac3ec1 Mon Sep 17 00:00:00 2001 From: justagist Date: Thu, 15 Oct 2026 09:33:59 +0000 Subject: [PATCH 7/7] skip the init retry test without raw CAN sockets --- piper_cpp/test/std_can_interface_test.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/piper_cpp/test/std_can_interface_test.cpp b/piper_cpp/test/std_can_interface_test.cpp index e3e2e81..4ac757e 100644 --- a/piper_cpp/test/std_can_interface_test.cpp +++ b/piper_cpp/test/std_can_interface_test.cpp @@ -7,8 +7,11 @@ #include +#include #include +#include #include +#include #include #include #include @@ -150,8 +153,15 @@ TEST(StdCanInterface, DeliversEveryFrameInOrder) TEST(StdCanInterface, InitFailureCanBeRetried) { + // Without raw CAN sockets init() fails before the step under test and proves nothing. + int probe = socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (probe < 0) + GTEST_SKIP() << "PF_CAN sockets unavailable: " << strerror(errno); + ::close(probe); + // No such interface, so init() fails at the ifindex lookup. Before the fix the half-open - // socket was kept and a second init() reported success. - StdCanInterface can("piper_cpp_no_such_iface", 1000000, /*judge_flag=*/false, /*auto_init=*/true); + // socket was kept and the second init() reported success. + StdCanInterface can("piper_cpp_no_such_iface", 1000000, /*judge_flag=*/false, /*auto_init=*/false); + EXPECT_FALSE(can.init()); EXPECT_FALSE(can.init()); }