Make CAN receive interruptible and bounded

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

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

piper_cpp/include/piper_cpp/interface/piper_interface_v2.h

@@ -44,6 +44,15 @@ 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.
+    /// `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<StdCanInterface> 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 +74,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

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 <cerrno>
 #include <cstdint>
 #include <cstring>
 #include <fstream>
@@ -11,7 +12,9 @@
 #include <linux/can/raw.h>
 #include <net/if.h>
 #include <optional>
+#include <poll.h>
 #include <string>
+#include <sys/eventfd.h>
 #include <sys/ioctl.h>
 #include <sys/socket.h>
 #include <unistd.h>
@@ -29,47 +32,75 @@ 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_(-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();
     }
 
-    ~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`. 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.
+    StdCanInterface(const StdCanInterface&) = delete;
+    StdCanInterface& operator=(const StdCanInterface&) = delete;
+
+    ~StdCanInterface()
+    {
+        close();
+        ::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;
     }
 
@@ -96,11 +127,32 @@ 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: 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)
             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;
@@ -115,7 +167,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<ssize_t>(sizeof(frame)))
             return false;
 
         double timestamp = 0.0;
@@ -139,6 +202,16 @@ 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); }
+
+    /// 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<uint8_t>& data)
     {
         if (sock_ < 0)
@@ -178,9 +251,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_
     {
     };

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<StdCanInterface> 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,15 @@ void PiperInterfaceV2::piperInit()
 
 void PiperInterfaceV2::readLoop()
 {
-    while (!stop_read_.load(std::memory_order_relaxed))
+    // 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) && arm_can_->isOpen())
     {
-        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();
+    }
+    if (!stop_read_.load(std::memory_order_relaxed))
+    {
+        std::cerr << "Error: CAN socket closed unexpectedly; read thread stopped.\n";
     }
 }
 
@@ -100,6 +110,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();

piper_cpp/test/CMakeLists.txt

@@ -0,0 +1,14 @@
+find_package(GTest REQUIRED)
+include(GoogleTest)
+
+# 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::Main)
+    gtest_discover_tests(${name} PROPERTIES TIMEOUT 30)
+endfunction()
+
+add_piper_test(std_can_interface_test)
+add_piper_test(piper_interface_v2_test)