diff --git a/axiomatic_adapter/CMakeLists.txt b/axiomatic_adapter/CMakeLists.txt index fe6b375..3b66a29 100644 --- a/axiomatic_adapter/CMakeLists.txt +++ b/axiomatic_adapter/CMakeLists.txt @@ -43,9 +43,15 @@ find_package(ament_cmake_ros REQUIRED) find_package(Boost REQUIRED COMPONENTS system) find_package(CLI11 REQUIRED) find_package(socketcan_adapter REQUIRED) +find_package(Threads REQUIRED) # Axiomatic Adapter Library -add_library(axiomatic_adapter SHARED src/axiomatic_adapter.cpp) +add_library(axiomatic_adapter SHARED + src/axiomatic_adapter.cpp + src/axiomatic_protocol.cpp + src/udp_client.cpp + src/axiomatic_fd_backend.cpp +) target_compile_features(axiomatic_adapter PUBLIC c_std_99 cxx_std_17) target_include_directories(axiomatic_adapter PUBLIC @@ -57,6 +63,7 @@ target_link_libraries(axiomatic_adapter PUBLIC socketcan_adapter::socketcan_adapter PRIVATE Boost::system + Threads::Threads ) install(DIRECTORY include/ DESTINATION include) @@ -113,6 +120,17 @@ if(BUILD_TESTING) add_test(NAME test_axiomatic_adapter COMMAND test_axiomatic_adapter) endif() endif() + + # Tests for the new FD-backend codec + transport use GoogleTest; the legacy + # tests above use Catch2. Both frameworks coexist during the transition. + find_package(ament_cmake_gtest QUIET) + if(ament_cmake_gtest_FOUND) + ament_add_gtest(test_axiomatic_protocol test/test_axiomatic_protocol.cpp) + target_link_libraries(test_axiomatic_protocol axiomatic_adapter) + + ament_add_gtest(test_udp_client test/test_udp_client.cpp) + target_link_libraries(test_udp_client axiomatic_adapter) + endif() endif() ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) diff --git a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp index 61a48e6..9f182eb 100644 --- a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp +++ b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp @@ -25,28 +25,36 @@ #include #include "socketcan_adapter/can_frame.hpp" +#include "socketcan_adapter/i_can_backend.hpp" +#include "socketcan_adapter/socketcan_adapter.hpp" // for polymath::socketcan::SocketState namespace polymath { namespace can { -/// @brief State of TCP socket, error, open or closed -enum class TCPSocketState -{ - ERROR = -1, - OPEN = 0, - CLOSED = 1, -}; +/// @brief State of TCP socket — kept as an alias of polymath::socketcan::SocketState +/// since the legacy enum's values matched the interface enum exactly. The alias +/// preserves source compatibility for existing callers (e.g. the bridge node +/// using `TCPSocketState::OPEN`) while letting AxiomaticAdapter implement the +/// ICanBackend::get_socket_state() contract. +using TCPSocketState = polymath::socketcan::SocketState; /// @class polymath::can::AxiomaticAdapter /// @brief Creates and manages a tcp connection and simplifies the interface. -/// Generally does not throw, but returns booleans to tell you success -class AxiomaticAdapter : public std::enable_shared_from_this +/// Generally does not throw, but returns booleans to tell you success. +/// +/// As of 2026-05 AxiomaticAdapter implements polymath::socketcan::ICanBackend +/// so that consumers can hold a non-owning ICanBackend* and select the +/// transport (kernel SocketCAN, this Axiomatic ETH-CAN bridge) at construction +/// time. The retrofit is purely additive: the existing constructor that takes +/// receive + error callbacks remains the canonical setup path, and runtime +/// callback setters were added to satisfy the interface — set them BEFORE +/// startReceptionThread() to avoid a benign race with the rx thread. +class AxiomaticAdapter : public polymath::socketcan::ICanBackend, public std::enable_shared_from_this { public: - /// @brief Mapped to std lib, but should be remapped to Polymath Safety compatible versions - using socket_error_string_t = std::string; + using socket_error_string_t = polymath::socketcan::ICanBackend::socket_error_string_t; static constexpr std::chrono::milliseconds DEFAULT_SOCKET_RECEIVE_TIMEOUT_MS{100}; static constexpr std::chrono::milliseconds JOIN_RECEPTION_TIMEOUT_MS{100}; @@ -65,20 +73,20 @@ class AxiomaticAdapter : public std::enable_shared_from_this const std::chrono::milliseconds & receive_timeout_ms = AxiomaticAdapter::DEFAULT_SOCKET_RECEIVE_TIMEOUT_MS); /// @brief Destructor for AxiomaticAdapter - virtual ~AxiomaticAdapter(); + ~AxiomaticAdapter() override; /// @brief Open TCP Socket /// @return bool success for opening socket - bool openSocket(); + bool openSocket() override; /// @brief Close TCP Socket /// @return bool success for closing socket - bool closeSocket(); + bool closeSocket() override; /// @brief Receive with a reference to a CanFrame to fill /// @param frame OUTPUT CanFrame to fill /// @return optional error string filled with an error message if any - std::optional receive(polymath::socketcan::CanFrame & can_frame); + std::optional receive(polymath::socketcan::CanFrame & can_frame) override; /// @brief Receive returns the received CanFrame /// @return optional CanFrame @@ -87,17 +95,27 @@ class AxiomaticAdapter : public std::enable_shared_from_this /// @brief Start a reception thread (calls callback) /// @return success on started - bool startReceptionThread(); + bool startReceptionThread() override; - /// @brief Stop and join reception thread - /// @param timeout_s INPUT timeout in seconds, <=0 means no timeout + /// @brief Stop and join reception thread (legacy ms-based overload). + /// @param timeout_s INPUT timeout in milliseconds, <=0 means no timeout /// @return success on closed and joined thread bool joinReceptionThread(const std::chrono::milliseconds & timeout_s = AxiomaticAdapter::JOIN_RECEPTION_TIMEOUT_MS); + /// @brief Stop and join reception thread (ICanBackend interface signature). + /// @param timeout_s INPUT timeout as a chrono::duration; cast to ms internally. + /// @return success on closed and joined thread + bool joinReceptionThread(const std::chrono::duration & timeout_s) override; + /// @brief Transmit a can frame via socket /// @param frame INPUT const reference to the frame /// @return optional error string filled with an error message if any - std::optional send(const polymath::socketcan::CanFrame & frame); + std::optional send(const polymath::socketcan::CanFrame & frame) override; + + /// @brief Transmit a can frame via socket + /// @param frame INPUT shared_ptr to a frame to send (ICanBackend overload). + /// @return optional error string filled with an error message if any + std::optional send(const std::shared_ptr frame) override; /// @brief Transmit a can frame via socket /// @param frame Linux CAN frame to send @@ -105,12 +123,26 @@ class AxiomaticAdapter : public std::enable_shared_from_this std::optional send(const can_frame & frame); /// @brief Get state of socket - /// @return TCPSocketState data type detailing OPEN or CLOSED - TCPSocketState get_socket_state(); + /// @return SocketState data type detailing OPEN or CLOSED + /// + /// (TCPSocketState is now an alias of polymath::socketcan::SocketState, so + /// callers that hold the result as TCPSocketState continue to work.) + polymath::socketcan::SocketState get_socket_state() override; /// @brief Checks if the receive thread is running /// @return True if the thread is running, false otherwise - bool is_thread_running(); + bool is_thread_running() override; + + /// @brief Set the receive callback after construction (ICanBackend interface). + /// Set BEFORE calling startReceptionThread() to avoid racing the rx thread. + /// @return true on success. + bool setOnReceiveCallback( + std::function frame)> && callback_function) override; + + /// @brief Set the error callback after construction (ICanBackend interface). + /// Set BEFORE calling startReceptionThread() to avoid racing the rx thread. + /// @return true on success. + bool setOnErrorCallback(std::function && callback_function) override; private: /// @brief use Implemention (pimpl) to avoid including boost/asio.hpp in header + linking in CMake diff --git a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_fd_backend.hpp b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_fd_backend.hpp new file mode 100644 index 0000000..3015f10 --- /dev/null +++ b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_fd_backend.hpp @@ -0,0 +1,135 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef AXIOMATIC_ADAPTER__AXIOMATIC_FD_BACKEND_HPP_ +#define AXIOMATIC_ADAPTER__AXIOMATIC_FD_BACKEND_HPP_ + +#include "axiomatic_adapter/axiomatic_protocol.hpp" +#include "axiomatic_adapter/udp_client.hpp" +#include "socketcan_adapter/can_frame.hpp" +#include "socketcan_adapter/i_can_backend.hpp" +#include "socketcan_adapter/socketcan_adapter.hpp" // for SocketState enum + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace polymath +{ +namespace can +{ + +/// @class polymath::can::AxiomaticFdBackend +/// @brief ICanBackend implementation for the Axiomatic AX140970 Dual CAN FD to +/// Ethernet converter (UDP transport). +/// +/// Wraps polymath::axiomatic::UdpClient and translates between the Axiomatic +/// wire protocol (envelope + CAN FD Frame Stream, see axiomatic_protocol.hpp) +/// and the polymath::socketcan::CanFrame value type that ICanBackend exposes +/// to callers. +/// +/// Classical-CAN ONLY in this revision. CanFrame currently wraps a Linux +/// `struct can_frame` with an 8-byte payload limit. The AX140970 itself +/// supports up to 64-byte CAN FD frames with BRS/ESI, but those cannot fit +/// through CanFrame yet. Until CanFrame is generalized (separate PR), this +/// backend drops inbound frames whose length > 8 or whose FDF flag is set; +/// those drops are counted on `fd_frames_dropped()` so callers can detect +/// the limitation rather than silently lose data. +/// +/// Lifecycle mirrors the SocketcanAdapter pattern: +/// +/// AxiomaticFdBackend backend(opts); +/// backend.openSocket(); // resolves + binds UDP socket +/// backend.setOnReceiveCallback(...); // optional; set before start +/// backend.setOnErrorCallback(...); // optional +/// backend.startReceptionThread(); // begins delivering callbacks +/// ... +/// backend.joinReceptionThread(timeout); +/// backend.closeSocket(); +class AxiomaticFdBackend : public polymath::socketcan::ICanBackend +{ +public: + using socket_error_string_t = polymath::socketcan::ICanBackend::socket_error_string_t; + + struct Options + { + std::string device_ip; + uint16_t device_port = 4000; + /// Local channel address advertised in our heartbeats; the device uses + /// this to filter which CAN frames it forwards to us. Default (0,0) = + /// NULL filter = accept all. + polymath::axiomatic::ChannelAddress local_address{0u, 0u}; + /// Destination address for transmitted frames. (CG=0, CIDS=0x1) is CAN1 + /// in the device's default routing; (0, 0x2) is CAN2. Override per-channel. + polymath::axiomatic::ChannelAddress tx_address{0u, 0x1u}; + std::chrono::milliseconds heartbeat_interval{1000}; + }; + + explicit AxiomaticFdBackend(Options opts); + ~AxiomaticFdBackend() override; + + AxiomaticFdBackend(const AxiomaticFdBackend &) = delete; + AxiomaticFdBackend & operator=(const AxiomaticFdBackend &) = delete; + + // ---- ICanBackend ---- + bool openSocket() override; + bool closeSocket() override; + polymath::socketcan::SocketState get_socket_state() override; + + std::optional send( + const std::shared_ptr frame) override; + std::optional send( + const polymath::socketcan::CanFrame & frame) override; + std::optional receive( + polymath::socketcan::CanFrame & frame) override; + + bool startReceptionThread() override; + bool joinReceptionThread(const std::chrono::duration & timeout_s) override; + bool is_thread_running() override; + + bool setOnReceiveCallback( + std::function frame)> && + callback_function) override; + bool setOnErrorCallback( + std::function && callback_function) override; + + /// @brief Count of inbound frames dropped because they exceed CanFrame's + /// 8-byte Classical-CAN payload limit (length > 8 or FDF flag set). When + /// CanFrame is generalized to CAN FD this will stay at 0. + uint64_t fd_frames_dropped() const noexcept; + +private: + void onUdpFrame(const polymath::axiomatic::CanFdFrameRecord & frame); + void onUdpErrorFrame(uint8_t error_code); + + Options opts_; + std::unique_ptr udp_client_; + std::atomic rx_dispatch_active_{false}; + std::atomic fd_frames_dropped_{0}; + + std::mutex callback_mu_; + std::function frame)> + on_receive_; + std::function on_error_; +}; + +} // namespace can +} // namespace polymath + +#endif // AXIOMATIC_ADAPTER__AXIOMATIC_FD_BACKEND_HPP_ diff --git a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_protocol.hpp b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_protocol.hpp new file mode 100644 index 0000000..8410443 --- /dev/null +++ b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_protocol.hpp @@ -0,0 +1,228 @@ +// Axiomatic AX140970 Dual CAN FD to Ethernet Converter — wire protocol codec. +// +// Pure data-in / data-out: no sockets, no threads, no ROS, no logging. +// Every parse function returns std::optional and rejects malformed input. +// Every encode function appends to a caller-provided buffer. +// +// Reference: "Ethernet to CAN Converter Communication Protocol", v6, Apr 2025. + +#ifndef AXIOMATIC_ADAPTER__AXIOMATIC_PROTOCOL_HPP_ +#define AXIOMATIC_ADAPTER__AXIOMATIC_PROTOCOL_HPP_ + +#include +#include +#include +#include +#include + +namespace polymath::axiomatic { + +// --------------------------------------------------------------------------- +// Constants +// --------------------------------------------------------------------------- + +inline constexpr std::array kAxiomaticTag{'A', 'X', 'I', 'O'}; +inline constexpr uint16_t kProtocolId = 14010; // 0x36BA, LSB-first on the wire +inline constexpr std::size_t kEnvelopeHeaderSize = 11; +inline constexpr std::size_t kMaxMessageSize = 256; // envelope cap +inline constexpr std::size_t kMaxMessageDataLen = kMaxMessageSize - kEnvelopeHeaderSize; +inline constexpr std::size_t kCanFdFrameHeaderSize = 17; +inline constexpr std::size_t kCanFdMaxDataLen = 64; + +// --------------------------------------------------------------------------- +// Message IDs (Protocol ID 14010, current versions per spec table) +// --------------------------------------------------------------------------- + +enum class MessageId : uint16_t { + Undefined = 0, + CanNotificationStream = 1, // deprecated; do not emit + StatusRequest = 2, + StatusResponse = 3, + Heartbeat = 4, + CanFdStream = 5, +}; + +// --------------------------------------------------------------------------- +// CAN flag bits (CANFB byte in the CAN FD Frame) +// --------------------------------------------------------------------------- + +namespace can_flag { +inline constexpr uint8_t kError = 0x80; // ERR_Bit: error/notification message +inline constexpr uint8_t kExtId = 0x40; // EID_Bit: 29-bit identifier +inline constexpr uint8_t kRemote = 0x20; // RF_Bit: remote transmission request +inline constexpr uint8_t kFd = 0x10; // FDF_Bit: CAN FD frame +inline constexpr uint8_t kBrs = 0x08; // BRS_Bit: bit rate switch (data phase) +inline constexpr uint8_t kEsi = 0x04; // ESI_Bit: error state indicator +} // namespace can_flag + +// --------------------------------------------------------------------------- +// CAN ID limits +// --------------------------------------------------------------------------- + +inline constexpr uint32_t kStdIdMax = 0x7FF; // 11-bit +inline constexpr uint32_t kExtIdMax = 0x1FFFFFFF; // 29-bit + +// --------------------------------------------------------------------------- +// Supported Features bitmasks (32-bit, LSB-first on the wire) +// --------------------------------------------------------------------------- + +namespace supported_features { +inline constexpr uint32_t kCanFdStream = 0x00000001; // node supports Msg ID 5 +inline constexpr uint32_t kOneFramePerMessage = 0x00000002; // request: 1 frame per stream message +} // namespace supported_features + +// --------------------------------------------------------------------------- +// Heartbeat (Message ID 4) — Message Version 2 +// --------------------------------------------------------------------------- +// +// 22-byte data field per spec. Used host→device for keepalive (UDP idles out +// at 10 s of inactivity device-side) and device→host for status (1 Hz). +// +// Versions 1 and 3 exist (v1 has no SFB/CGMB/CIDSMB; v3 adds variable-length +// additional filter addresses). We standardize on v2 as the universal common +// denominator: the device speaks it natively (verified by probe), and v2 is +// long-lived — newer protocols stay backward-compatible per spec. + +inline constexpr uint8_t kHeartbeatVersion = 2; +inline constexpr std::size_t kHeartbeatV2DataSize = 22; + +struct HeartbeatV2 { + uint32_t message_number = 0; // free-running counter, increments per outbound HB + uint32_t time_interval_ms = 1000; // ms since previous heartbeat (≈1000 for 1 Hz) + uint32_t health_data = 0; // 4-byte Health Data field; host: 0 (no health to report) + uint8_t converter_type = 0; // we are a host, not a converter; spec says 0 in that case + uint32_t supported_features = 0; // bitmask from supported_features::* + uint8_t filter_channel_group = 0; // Main CAN Port Input Filter Address — Channel Group + uint32_t filter_channel_id_set = 0;// …and Channel ID Set; (0,0) means NULL filter (accept all) +}; + +// --------------------------------------------------------------------------- +// Types +// --------------------------------------------------------------------------- + +// Envelope header (11 bytes on the wire). +struct MessageHeader { + uint16_t protocol_id; + uint16_t message_id; + uint8_t message_version; + uint16_t data_length; +}; + +// CAN routing address: (Channel Group, Channel ID Set bitmap). +// The CIDS is a 32-bit OR-able bitmap; channel N is selected by bit (N - 1). +struct ChannelAddress { + uint8_t channel_group; + uint32_t channel_id_set; +}; + +inline constexpr bool addressesMatch(ChannelAddress a, ChannelAddress b) { + return a.channel_group == b.channel_group && + (a.channel_id_set & b.channel_id_set) != 0u; +} + +// Decoded CAN FD frame (the 17-byte header + payload). +struct CanFdFrameRecord { + uint16_t physical_channel = 0; // 0..8191; 0 means "unused/undefined" + uint8_t reserved_flags = 0; // top 3 bits of PCNFB; spec-reserved, kept for fidelity + ChannelAddress address{}; + uint32_t timestamp_ms = 0; // device free-running ms counter + uint8_t flags = 0; // bitmask of can_flag::* + uint8_t length = 0; // payload length (validated against flags) + uint32_t can_id = 0; // std (≤0x7FF) or ext (≤0x1FFFFFFF) per kExtId flag + std::array data{}; + + bool isError() const { return (flags & can_flag::kError) != 0u; } + bool isExtId() const { return (flags & can_flag::kExtId) != 0u; } + bool isRemote() const { return (flags & can_flag::kRemote) != 0u; } + bool isFd() const { return (flags & can_flag::kFd) != 0u; } + bool isBrs() const { return (flags & can_flag::kBrs) != 0u; } + bool isEsi() const { return (flags & can_flag::kEsi) != 0u; } +}; + +// --------------------------------------------------------------------------- +// Endianness helpers (LSB-first, the protocol default) +// --------------------------------------------------------------------------- + +uint16_t readLE16(const uint8_t * src) noexcept; +uint32_t readLE32(const uint8_t * src) noexcept; +void writeLE16(uint8_t * dst, uint16_t v) noexcept; +void writeLE32(uint8_t * dst, uint32_t v) noexcept; + +// --------------------------------------------------------------------------- +// CAN FD length validation +// --------------------------------------------------------------------------- + +// Returns true iff `len` is a valid CAN FD payload length (0,1..8,12,16,20,24,32,48,64). +bool isValidCanFdLength(uint8_t len) noexcept; + +// --------------------------------------------------------------------------- +// Envelope codec +// --------------------------------------------------------------------------- + +// Writes the 11-byte envelope header into `out` (which must have ≥ 11 bytes). +// Returns the number of bytes written (always 11). +std::size_t writeMessageHeader(uint8_t * out, + MessageId message_id, + uint8_t message_version, + uint16_t data_length) noexcept; + +// Parses an envelope header. Validates AXIO tag + Protocol ID. Returns nullopt if: +// - buffer < 11 bytes +// - tag mismatch +// - protocol_id != 14010 +// - declared data_length would exceed kMaxMessageDataLen (245) +std::optional parseMessageHeader(const uint8_t * data, + std::size_t len) noexcept; + +// --------------------------------------------------------------------------- +// CAN FD Frame codec (one record within a CAN FD Stream message) +// --------------------------------------------------------------------------- + +// Encodes a CAN FD Frame record (17-byte header + length data bytes) into `out`. +// Returns the number of bytes written, or 0 on validation failure (bad ID range, +// bad DLC, etc.) — caller MUST check and handle. +std::size_t writeCanFdFrame(std::vector & out, + const CanFdFrameRecord & frame); + +// Parses a CAN FD Frame record. Returns nullopt if buffer too short, length +// inconsistent with flags, or reserved bits set. +std::optional parseCanFdFrame(const uint8_t * data, + std::size_t len) noexcept; + +// --------------------------------------------------------------------------- +// Convenience: full single-frame CAN FD Stream message +// --------------------------------------------------------------------------- + +// Encodes a complete CAN FD Stream message (envelope + exactly one CAN FD Frame). +// Sets the One-Frame-per-Message scenario implied by Q1-locked design choices. +// Returns the encoded bytes, or empty vector on encode failure. +std::vector encodeCanFdStreamSingleFrame(const CanFdFrameRecord & frame); + +// Parses a CAN FD Stream message containing exactly one CAN FD Frame. +// Returns nullopt if envelope is malformed, message_id is not CanFdStream, or the +// embedded frame is malformed. +std::optional parseCanFdStreamSingleFrame(const uint8_t * data, + std::size_t len) noexcept; + +// --------------------------------------------------------------------------- +// Heartbeat codec +// --------------------------------------------------------------------------- + +// Encodes a complete Heartbeat (Msg ID 4, Version 2) message: envelope + 22-byte +// data field. Returns the 33-byte wire buffer. Never fails (no validation hooks). +std::vector encodeHeartbeatV2(const HeartbeatV2 & hb); + +// Parses just the 22-byte Heartbeat v2 data field (caller has already validated +// envelope + sliced out the data section). Returns nullopt if `len` < 22. +std::optional parseHeartbeatV2DataField(const uint8_t * data, + std::size_t len) noexcept; + +// Convenience: validates envelope, requires Msg ID = 4 and Version ≥ 2, and +// returns the parsed Heartbeat. Returns nullopt on any failure. Versions > 2 +// are accepted (forward compatibility); only the v2-defined fields are decoded. +std::optional parseHeartbeat(const uint8_t * data, + std::size_t len) noexcept; + +} // namespace polymath::axiomatic + +#endif // AXIOMATIC_ADAPTER__AXIOMATIC_PROTOCOL_HPP_ diff --git a/axiomatic_adapter/include/axiomatic_adapter/udp_client.hpp b/axiomatic_adapter/include/axiomatic_adapter/udp_client.hpp new file mode 100644 index 0000000..fb159dc --- /dev/null +++ b/axiomatic_adapter/include/axiomatic_adapter/udp_client.hpp @@ -0,0 +1,118 @@ +// UdpClient — host-side UDP transport to an Axiomatic AX140970. +// +// Owns one connected UDP socket plus two threads: +// tx_thread: emits a 1 Hz HeartbeatV2 to keep the device-side connection alive +// (10 s idle timeout per spec §2.3.1.1). +// rx_thread: blocks on poll(), recvfrom()s every datagram, parses the envelope, +// dispatches by Message ID to user-supplied callbacks. +// +// Callbacks run on the rx thread. Keep them fast — anything blocking will back +// up the device's internal queue and eventually drop frames. +// +// All public methods are thread-safe except setOn*() which must be called +// before start(). +// +// This is *not* yet the ICanBackend implementation from DESIGN.md — it's the +// transport layer it will sit on. Minimal by intent. + +#ifndef AXIOMATIC_ADAPTER__UDP_CLIENT_HPP_ +#define AXIOMATIC_ADAPTER__UDP_CLIENT_HPP_ + +#include "axiomatic_adapter/axiomatic_protocol.hpp" + +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace polymath::axiomatic { + +class UdpClient { + public: + struct Options { + std::string device_ip; + uint16_t device_port = 4000; + // Bitmask of supported_features::* advertised in our outbound heartbeat. + uint32_t supported_features = + supported_features::kCanFdStream | supported_features::kOneFramePerMessage; + std::chrono::milliseconds heartbeat_interval{1000}; + // If true, send a one-shot Status Request immediately after socket bind. + // The bench probe confirmed this is the cleanest registration handshake. + bool send_initial_status_request = true; + }; + + struct Stats { + std::atomic heartbeats_sent{0}; + std::atomic heartbeats_received{0}; + std::atomic frames_sent{0}; + std::atomic frames_received{0}; + std::atomic error_frames_received{0}; + std::atomic parse_errors{0}; + std::atomic tx_errors{0}; + }; + + // Callbacks. All invoked from the rx thread. Default: do nothing. + using FrameCallback = std::function; + using ErrorFrameCallback = std::function; // Warning/Passive/Bus-Off + using HeartbeatCallback = std::function; + + explicit UdpClient(Options opts); + ~UdpClient(); + + UdpClient(const UdpClient &) = delete; + UdpClient & operator=(const UdpClient &) = delete; + UdpClient(UdpClient &&) = delete; + UdpClient & operator=(UdpClient &&) = delete; + + // Open the socket, register with the device, start tx + rx threads. + // Returns true on success, false on socket / bind / connect failure. + bool start(); + + // Signal threads to exit, join, close the socket. Safe to call repeatedly. + void stop(); + + // Send a CAN FD Stream message containing exactly one CAN FD Frame to the device. + // Thread-safe with respect to the heartbeat thread (mutex around sendto). + // Returns false on encode failure (bad frame) or socket error. + bool sendFrame(const CanFdFrameRecord & frame); + + // Setters — call before start(). + void setOnFrame(FrameCallback cb); + void setOnErrorFrame(ErrorFrameCallback cb); + void setOnHeartbeat(HeartbeatCallback cb); + + // Local socket port the kernel assigned us (after start()), or 0 if not running. + uint16_t localPort() const; + + // Read-only stats. + const Stats & stats() const noexcept { return stats_; } + + private: + void txLoop(); + void rxLoop(); + bool sendBytes(const uint8_t * data, std::size_t len); + + Options opts_; + int sock_ = -1; + sockaddr_in dst_{}; + uint16_t local_port_ = 0; + std::atomic running_{false}; + std::atomic hb_msg_num_{0}; + std::thread tx_thread_; + std::thread rx_thread_; + std::mutex tx_mutex_; // serializes sendto across hb + frames + Stats stats_; + + FrameCallback on_frame_; + ErrorFrameCallback on_error_frame_; + HeartbeatCallback on_heartbeat_; +}; + +} // namespace polymath::axiomatic + +#endif // AXIOMATIC_ADAPTER__UDP_CLIENT_HPP_ diff --git a/axiomatic_adapter/package.xml b/axiomatic_adapter/package.xml index fb3278e..fc79888 100644 --- a/axiomatic_adapter/package.xml +++ b/axiomatic_adapter/package.xml @@ -15,6 +15,7 @@ socketcan_adapter catch2 + ament_cmake_gtest ament_cmake diff --git a/axiomatic_adapter/src/axiomatic_adapter.cpp b/axiomatic_adapter/src/axiomatic_adapter.cpp index 8fb1491..8c12fd3 100644 --- a/axiomatic_adapter/src/axiomatic_adapter.cpp +++ b/axiomatic_adapter/src/axiomatic_adapter.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #include #include #include @@ -345,6 +346,22 @@ class AxiomaticAdapter::AxiomaticAdapterImpl return thread_running_; } + // Runtime callback setters — added 2026-05 for the ICanBackend retrofit. + // Mirrors SocketcanAdapter's behavior: plain assignment, no synchronization. + // The convention is "set before startReceptionThread()" to avoid racing the + // rx thread. + bool setOnReceiveCallback(std::function frame)> && cb) + { + receive_callback_ = std::move(cb); + return true; + } + + bool setOnErrorCallback(std::function && cb) + { + error_callback_ = std::move(cb); + return true; + } + private: static constexpr std::array AXIOMATIC_CAN_MESSAGE_HEADER = {'A', 'X', 'I', 'O', 0xBA, 0x36, 0x01}; static constexpr std::chrono::milliseconds TCP_IP_CONNECTION_TIMEOUT_MS{3000}; @@ -429,7 +446,7 @@ std::optional AxiomaticAdapter::send(co return send(polymath::socketcan::CanFrame(frame)); } -TCPSocketState AxiomaticAdapter::get_socket_state() +polymath::socketcan::SocketState AxiomaticAdapter::get_socket_state() { return pimpl_->get_socket_state(); } @@ -439,5 +456,34 @@ bool AxiomaticAdapter::is_thread_running() return pimpl_->is_thread_running(); } +// --------------------------------------------------------------------------- +// ICanBackend bridge methods — added 2026-05 for the AxiomaticAdapter retrofit. +// --------------------------------------------------------------------------- + +bool AxiomaticAdapter::joinReceptionThread(const std::chrono::duration & timeout_s) +{ + return pimpl_->joinReceptionThread(std::chrono::duration_cast(timeout_s)); +} + +std::optional AxiomaticAdapter::send( + const std::shared_ptr frame) +{ + if (!frame) { + return std::optional{"send: null frame"}; + } + return pimpl_->send(*frame); +} + +bool AxiomaticAdapter::setOnReceiveCallback( + std::function frame)> && callback_function) +{ + return pimpl_->setOnReceiveCallback(std::move(callback_function)); +} + +bool AxiomaticAdapter::setOnErrorCallback(std::function && callback_function) +{ + return pimpl_->setOnErrorCallback(std::move(callback_function)); +} + } // namespace can } // namespace polymath diff --git a/axiomatic_adapter/src/axiomatic_fd_backend.cpp b/axiomatic_adapter/src/axiomatic_fd_backend.cpp new file mode 100644 index 0000000..4dafb9b --- /dev/null +++ b/axiomatic_adapter/src/axiomatic_fd_backend.cpp @@ -0,0 +1,299 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "axiomatic_adapter/axiomatic_fd_backend.hpp" + +#include + +#include +#include + +namespace polymath +{ +namespace can +{ + +namespace ax = polymath::axiomatic; + +// --------------------------------------------------------------------------- +// Helpers: convert between socketcan::CanFrame and axiomatic::CanFdFrameRecord +// --------------------------------------------------------------------------- +// +// Classical-CAN-only. CanFrame currently wraps `struct can_frame` (8-byte data +// limit). When CanFrame is generalized to support `canfd_frame`, this is the +// translation layer that grows to handle 64-byte payloads + BRS / ESI flags. + +namespace +{ + +/// Pack a CanFrame into an outbound CanFdFrameRecord. Returns nullopt if the +/// source frame doesn't fit our current Classical-CAN-only constraint. +std::optional toFdRecord( + const polymath::socketcan::CanFrame & src, + ax::ChannelAddress tx_addr) +{ + const auto len = src.get_len(); + if (len > 8u) { + return std::nullopt; // shouldn't happen for CanFrame, but defensive + } + ax::CanFdFrameRecord out{}; + out.address = tx_addr; + out.length = len; + out.can_id = src.get_id(); + // Map FrameType / IdType → CAN flag bits. + uint8_t flags = 0; + if (src.get_id_type() == polymath::socketcan::IdType::EXTENDED) { + flags |= ax::can_flag::kExtId; + } + switch (src.get_frame_type()) { + case polymath::socketcan::FrameType::REMOTE: + flags |= ax::can_flag::kRemote; + break; + case polymath::socketcan::FrameType::ERROR: + flags |= ax::can_flag::kError; + break; + case polymath::socketcan::FrameType::DATA: + default: + break; + } + out.flags = flags; + const auto data = src.get_data(); + for (uint8_t i = 0; i < len; ++i) { + out.data[i] = data[i]; + } + return out; +} + +/// Unpack an inbound CanFdFrameRecord into a CanFrame. Returns nullopt if the +/// record exceeds CanFrame's Classical-CAN limits (length > 8 or FDF flag set); +/// caller must count those drops as a known limitation pending CanFrame +/// generalization. +std::optional toCanFrame( + const ax::CanFdFrameRecord & src) +{ + if (src.isFd() || src.length > CAN_MAX_DLC) { + return std::nullopt; + } + struct can_frame raw {}; + raw.can_id = src.can_id; + if (src.isExtId()) { + raw.can_id |= CAN_EFF_FLAG; + } + if (src.isRemote()) { + raw.can_id |= CAN_RTR_FLAG; + } + if (src.isError()) { + raw.can_id |= CAN_ERR_FLAG; + } + raw.len = src.length; + for (uint8_t i = 0; i < src.length; ++i) { + raw.data[i] = src.data[i]; + } + return polymath::socketcan::CanFrame(raw); +} + +} // namespace + +// --------------------------------------------------------------------------- +// Lifecycle +// --------------------------------------------------------------------------- + +AxiomaticFdBackend::AxiomaticFdBackend(Options opts) : opts_(std::move(opts)) {} + +AxiomaticFdBackend::~AxiomaticFdBackend() +{ + closeSocket(); +} + +bool AxiomaticFdBackend::openSocket() +{ + if (udp_client_) { + return true; // already open + } + ax::UdpClient::Options uopts; + uopts.device_ip = opts_.device_ip; + uopts.device_port = opts_.device_port; + uopts.heartbeat_interval = opts_.heartbeat_interval; + uopts.supported_features = + ax::supported_features::kCanFdStream | ax::supported_features::kOneFramePerMessage; + udp_client_ = std::make_unique(uopts); + + // Wire up our internal dispatchers BEFORE start() so we never miss frames. + udp_client_->setOnFrame( + [this](const ax::CanFdFrameRecord & f) { onUdpFrame(f); }); + udp_client_->setOnErrorFrame( + [this](uint8_t code) { onUdpErrorFrame(code); }); + + return udp_client_->start(); +} + +bool AxiomaticFdBackend::closeSocket() +{ + rx_dispatch_active_.store(false); + if (udp_client_) { + udp_client_->stop(); + udp_client_.reset(); + } + return true; +} + +polymath::socketcan::SocketState AxiomaticFdBackend::get_socket_state() +{ + if (!udp_client_) { + return polymath::socketcan::SocketState::CLOSED; + } + return (udp_client_->localPort() != 0) + ? polymath::socketcan::SocketState::OPEN + : polymath::socketcan::SocketState::CLOSED; +} + +// --------------------------------------------------------------------------- +// Send / receive +// --------------------------------------------------------------------------- + +std::optional AxiomaticFdBackend::send( + const polymath::socketcan::CanFrame & frame) +{ + if (!udp_client_) { + return socket_error_string_t{"send: socket not open"}; + } + const auto record = toFdRecord(frame, opts_.tx_address); + if (!record) { + return socket_error_string_t{"send: frame doesn't fit Classical-CAN constraint"}; + } + if (!udp_client_->sendFrame(*record)) { + return socket_error_string_t{"send: UdpClient::sendFrame failed"}; + } + return std::nullopt; +} + +std::optional AxiomaticFdBackend::send( + const std::shared_ptr frame) +{ + if (!frame) { + return socket_error_string_t{"send: null frame"}; + } + return send(*frame); +} + +std::optional AxiomaticFdBackend::receive( + polymath::socketcan::CanFrame & /*frame*/) +{ + // Synchronous receive isn't a natural fit for the device's asynchronous UDP + // stream — the UdpClient delivers frames via the rx-thread callback. Most + // ICanBackend consumers (including the ROS2 bridge node) only use the + // callback path; the SocketcanAdapter's blocking `receive()` is a legacy + // affordance. Leaving this unimplemented until a real caller surfaces. + return socket_error_string_t{"receive(CanFrame&) not yet implemented; " + "use setOnReceiveCallback + startReceptionThread"}; +} + +// --------------------------------------------------------------------------- +// Reception thread + callbacks +// --------------------------------------------------------------------------- + +bool AxiomaticFdBackend::startReceptionThread() +{ + if (!udp_client_) { + return false; + } + rx_dispatch_active_.store(true); + return true; +} + +bool AxiomaticFdBackend::joinReceptionThread( + const std::chrono::duration & /*timeout_s*/) +{ + rx_dispatch_active_.store(false); + // The UdpClient's rx thread keeps running until closeSocket() — we only + // stop dispatching to our caller's callback. That matches SocketcanAdapter's + // join semantics: the OS socket stays open, just no more callbacks. + return true; +} + +bool AxiomaticFdBackend::is_thread_running() +{ + return rx_dispatch_active_.load(); +} + +bool AxiomaticFdBackend::setOnReceiveCallback( + std::function frame)> && cb) +{ + std::lock_guard lk(callback_mu_); + on_receive_ = std::move(cb); + return true; +} + +bool AxiomaticFdBackend::setOnErrorCallback( + std::function && cb) +{ + std::lock_guard lk(callback_mu_); + on_error_ = std::move(cb); + return true; +} + +uint64_t AxiomaticFdBackend::fd_frames_dropped() const noexcept +{ + return fd_frames_dropped_.load(); +} + +// --------------------------------------------------------------------------- +// Internal: UdpClient → caller bridging +// --------------------------------------------------------------------------- + +void AxiomaticFdBackend::onUdpFrame(const ax::CanFdFrameRecord & frame) +{ + if (!rx_dispatch_active_.load(std::memory_order_relaxed)) { + return; + } + auto cf = toCanFrame(frame); + if (!cf) { + fd_frames_dropped_.fetch_add(1, std::memory_order_relaxed); + return; + } + std::function)> cb; + { + std::lock_guard lk(callback_mu_); + cb = on_receive_; + } + if (cb) { + cb(std::make_unique(std::move(*cf))); + } +} + +void AxiomaticFdBackend::onUdpErrorFrame(uint8_t error_code) +{ + if (!rx_dispatch_active_.load(std::memory_order_relaxed)) { + return; + } + std::function cb; + { + std::lock_guard lk(callback_mu_); + cb = on_error_; + } + if (cb) { + const char * name = nullptr; + switch (error_code) { + case 0: name = "CAN error undefined"; break; + case 1: name = "CAN warning"; break; + case 2: name = "CAN passive"; break; + case 3: name = "CAN bus-off"; break; + default: name = "CAN error"; break; + } + cb(socket_error_string_t{name}); + } +} + +} // namespace can +} // namespace polymath diff --git a/axiomatic_adapter/src/axiomatic_protocol.cpp b/axiomatic_adapter/src/axiomatic_protocol.cpp new file mode 100644 index 0000000..f48406f --- /dev/null +++ b/axiomatic_adapter/src/axiomatic_protocol.cpp @@ -0,0 +1,345 @@ +#include "axiomatic_adapter/axiomatic_protocol.hpp" + +#include +#include + +namespace polymath::axiomatic { + +// --------------------------------------------------------------------------- +// Endianness helpers +// --------------------------------------------------------------------------- + +uint16_t readLE16(const uint8_t * src) noexcept { + return static_cast(src[0]) | + static_cast(static_cast(src[1]) << 8); +} + +uint32_t readLE32(const uint8_t * src) noexcept { + return static_cast(src[0]) | + (static_cast(src[1]) << 8) | + (static_cast(src[2]) << 16) | + (static_cast(src[3]) << 24); +} + +void writeLE16(uint8_t * dst, uint16_t v) noexcept { + dst[0] = static_cast(v & 0xFF); + dst[1] = static_cast((v >> 8) & 0xFF); +} + +void writeLE32(uint8_t * dst, uint32_t v) noexcept { + dst[0] = static_cast(v & 0xFF); + dst[1] = static_cast((v >> 8) & 0xFF); + dst[2] = static_cast((v >> 16) & 0xFF); + dst[3] = static_cast((v >> 24) & 0xFF); +} + +// --------------------------------------------------------------------------- +// Length validation +// --------------------------------------------------------------------------- + +bool isValidCanFdLength(uint8_t len) noexcept { + // Per spec: 0,1,2,3,4,5,6,7,8,12,16,20,24,32,48,64. + if (len <= 8u) { + return true; + } + switch (len) { + case 12: case 16: case 20: case 24: case 32: case 48: case 64: + return true; + default: + return false; + } +} + +// --------------------------------------------------------------------------- +// Envelope codec +// --------------------------------------------------------------------------- + +std::size_t writeMessageHeader(uint8_t * out, + MessageId message_id, + uint8_t message_version, + uint16_t data_length) noexcept { + std::memcpy(out, kAxiomaticTag.data(), 4); + writeLE16(out + 4, kProtocolId); + writeLE16(out + 6, static_cast(message_id)); + out[8] = message_version; + writeLE16(out + 9, data_length); + return kEnvelopeHeaderSize; +} + +std::optional parseMessageHeader(const uint8_t * data, + std::size_t len) noexcept { + if (data == nullptr || len < kEnvelopeHeaderSize) { + return std::nullopt; + } + if (!std::equal(kAxiomaticTag.begin(), kAxiomaticTag.end(), data)) { + return std::nullopt; + } + MessageHeader h{}; + h.protocol_id = readLE16(data + 4); + if (h.protocol_id != kProtocolId) { + return std::nullopt; + } + h.message_id = readLE16(data + 6); + h.message_version = data[8]; + h.data_length = readLE16(data + 9); + if (h.data_length > kMaxMessageDataLen) { + return std::nullopt; + } + return h; +} + +// --------------------------------------------------------------------------- +// CAN FD Frame codec +// --------------------------------------------------------------------------- + +namespace { + +// Returns true iff the length field is consistent with the flag bits. +bool lengthIsConsistentWithFlags(uint8_t length, uint8_t flags) noexcept { + const bool is_error = (flags & can_flag::kError) != 0u; + const bool is_fd = (flags & can_flag::kFd) != 0u; + const bool is_rtr = (flags & can_flag::kRemote) != 0u; + + if (is_error) { + // Error/notification messages: 1..64. + return length >= 1u && length <= 64u; + } + if (is_fd) { + // CAN FD data frame: 0,1..8,12,16,20,24,32,48,64. + return isValidCanFdLength(length); + } + if (is_rtr) { + // Classical CAN remote frame request: 0..8 (data section is empty regardless, + // but length carries the requested DLC). + return length <= 8u; + } + // Classical CAN data frame: 0..8. + return length <= 8u; +} + +// Returns the number of payload bytes that should follow the 17-byte header. +// For RTR frames, payload is empty regardless of `length`. +uint8_t payloadBytesFromLengthAndFlags(uint8_t length, uint8_t flags) noexcept { + if ((flags & can_flag::kRemote) != 0u && (flags & can_flag::kError) == 0u) { + return 0u; + } + return length; +} + +bool canIdInRangeForFlags(uint32_t id, uint8_t flags) noexcept { + return ((flags & can_flag::kExtId) != 0u) ? (id <= kExtIdMax) : (id <= kStdIdMax); +} + +} // namespace + +std::size_t writeCanFdFrame(std::vector & out, + const CanFdFrameRecord & f) { + // Validate before emitting any bytes. + if (!lengthIsConsistentWithFlags(f.length, f.flags)) { + return 0u; + } + if (!canIdInRangeForFlags(f.can_id, f.flags)) { + return 0u; + } + if (f.physical_channel > 0x1FFFu) { + return 0u; + } + if ((f.reserved_flags & 0xF8u) != 0u) { + // reserved_flags only has 3 meaningful bits (high bits in PCNFB2) + return 0u; + } + + const uint8_t payload_bytes = payloadBytesFromLengthAndFlags(f.length, f.flags); + const std::size_t prev_size = out.size(); + out.resize(prev_size + kCanFdFrameHeaderSize + payload_bytes); + uint8_t * p = out.data() + prev_size; + + // PCNFB1, PCNFB2: low 13 bits = physical channel, high 3 bits = reserved flags. + const uint16_t pcn_word = + static_cast(f.physical_channel & 0x1FFFu) | + static_cast((f.reserved_flags & 0x07u) << 13); + writeLE16(p, pcn_word); + // CGB + p[2] = f.address.channel_group; + // CIDSB1..4 + writeLE32(p + 3, f.address.channel_id_set); + // ATB1..4 + writeLE32(p + 7, f.timestamp_ms); + // CANFB + p[11] = f.flags; + // CANLB + p[12] = f.length; + // CANID1..4 + writeLE32(p + 13, f.can_id); + // Payload + if (payload_bytes > 0u) { + std::memcpy(p + kCanFdFrameHeaderSize, f.data.data(), payload_bytes); + } + return kCanFdFrameHeaderSize + payload_bytes; +} + +std::optional parseCanFdFrame(const uint8_t * data, + std::size_t len) noexcept { + if (data == nullptr || len < kCanFdFrameHeaderSize) { + return std::nullopt; + } + CanFdFrameRecord f{}; + const uint16_t pcn_word = readLE16(data); + f.physical_channel = static_cast(pcn_word & 0x1FFFu); + f.reserved_flags = static_cast((pcn_word >> 13) & 0x07u); + f.address.channel_group = data[2]; + f.address.channel_id_set = readLE32(data + 3); + f.timestamp_ms = readLE32(data + 7); + f.flags = data[11]; + f.length = data[12]; + f.can_id = readLE32(data + 13); + + if (!lengthIsConsistentWithFlags(f.length, f.flags)) { + return std::nullopt; + } + if (!canIdInRangeForFlags(f.can_id, f.flags)) { + return std::nullopt; + } + // Reject reserved CAN ID bits set: standard ID must have bits [11..31] = 0; + // extended ID must have bits [29..31] = 0. + if ((f.flags & can_flag::kExtId) == 0u) { + if ((f.can_id & ~uint32_t{kStdIdMax}) != 0u) { + return std::nullopt; + } + } else { + if ((f.can_id & ~uint32_t{kExtIdMax}) != 0u) { + return std::nullopt; + } + } + + const uint8_t payload_bytes = payloadBytesFromLengthAndFlags(f.length, f.flags); + if (len < kCanFdFrameHeaderSize + payload_bytes) { + return std::nullopt; + } + if (payload_bytes > 0u) { + std::memcpy(f.data.data(), data + kCanFdFrameHeaderSize, payload_bytes); + } + return f; +} + +// --------------------------------------------------------------------------- +// Single-frame CAN FD Stream convenience +// --------------------------------------------------------------------------- + +std::vector encodeCanFdStreamSingleFrame(const CanFdFrameRecord & frame) { + std::vector buf; + buf.reserve(kEnvelopeHeaderSize + kCanFdFrameHeaderSize + kCanFdMaxDataLen); + + // Reserve the envelope header; we'll backfill data_length once we know it. + buf.resize(kEnvelopeHeaderSize); + const std::size_t frame_size = writeCanFdFrame(buf, frame); + if (frame_size == 0u) { + return {}; + } + if (frame_size > kMaxMessageDataLen) { + return {}; // not reachable today — header(17) + max data(64) = 81 < 245 + } + writeMessageHeader(buf.data(), + MessageId::CanFdStream, + /*message_version=*/0, + static_cast(frame_size)); + return buf; +} + +std::optional parseCanFdStreamSingleFrame(const uint8_t * data, + std::size_t len) noexcept { + const auto header = parseMessageHeader(data, len); + if (!header) { + return std::nullopt; + } + if (header->message_id != static_cast(MessageId::CanFdStream)) { + return std::nullopt; + } + if (len < kEnvelopeHeaderSize + header->data_length) { + return std::nullopt; + } + // We require exactly one CAN FD Frame fills the data region. + const auto frame = parseCanFdFrame(data + kEnvelopeHeaderSize, header->data_length); + if (!frame) { + return std::nullopt; + } + // Sanity: declared data_length should match the frame's full size. + const uint8_t payload_bytes = payloadBytesFromLengthAndFlags(frame->length, frame->flags); + if (header->data_length != kCanFdFrameHeaderSize + payload_bytes) { + return std::nullopt; + } + return frame; +} + +// --------------------------------------------------------------------------- +// Heartbeat codec (Message ID 4, Message Version 2) +// --------------------------------------------------------------------------- +// +// Wire layout of the 22-byte data field (LSB-first throughout): +// offset 0 MNB[1..4] 4 Message Number +// offset 4 TIB[1..4] 4 Time Interval (ms) +// offset 8 HDB[1..4] 4 Health Data +// offset 12 CTB 1 Converter Type +// offset 13 SFB[1..4] 4 Supported Features +// offset 17 CGMB 1 Channel Group of Main CAN Port Input Filter +// offset 18 CIDSMB[1..4] 4 Channel ID Set (4-byte CIDS for the Main filter) +// total 22 + +std::vector encodeHeartbeatV2(const HeartbeatV2 & hb) { + std::vector buf(kEnvelopeHeaderSize + kHeartbeatV2DataSize); + writeMessageHeader(buf.data(), + MessageId::Heartbeat, + /*message_version=*/kHeartbeatVersion, + static_cast(kHeartbeatV2DataSize)); + uint8_t * p = buf.data() + kEnvelopeHeaderSize; + writeLE32(p + 0, hb.message_number); + writeLE32(p + 4, hb.time_interval_ms); + writeLE32(p + 8, hb.health_data); + p[12] = hb.converter_type; + writeLE32(p + 13, hb.supported_features); + p[17] = hb.filter_channel_group; + writeLE32(p + 18, hb.filter_channel_id_set); + return buf; +} + +std::optional parseHeartbeatV2DataField(const uint8_t * data, + std::size_t len) noexcept { + if (data == nullptr || len < kHeartbeatV2DataSize) { + return std::nullopt; + } + HeartbeatV2 hb{}; + hb.message_number = readLE32(data + 0); + hb.time_interval_ms = readLE32(data + 4); + hb.health_data = readLE32(data + 8); + hb.converter_type = data[12]; + hb.supported_features = readLE32(data + 13); + hb.filter_channel_group = data[17]; + hb.filter_channel_id_set= readLE32(data + 18); + return hb; +} + +std::optional parseHeartbeat(const uint8_t * data, + std::size_t len) noexcept { + const auto header = parseMessageHeader(data, len); + if (!header) { + return std::nullopt; + } + if (header->message_id != static_cast(MessageId::Heartbeat)) { + return std::nullopt; + } + if (header->message_version < kHeartbeatVersion) { + // v1 lacks SFB/CGMB/CIDSMB; we don't try to upcast. + return std::nullopt; + } + if (header->data_length < kHeartbeatV2DataSize) { + return std::nullopt; + } + if (len < kEnvelopeHeaderSize + header->data_length) { + return std::nullopt; + } + // Forward-compat: extra bytes after the v2 region (e.g. v3 NAFB + 10 filters) + // are tolerated; we ignore them and return only the v2-defined fields. + return parseHeartbeatV2DataField(data + kEnvelopeHeaderSize, kHeartbeatV2DataSize); +} + +} // namespace polymath::axiomatic diff --git a/axiomatic_adapter/src/udp_client.cpp b/axiomatic_adapter/src/udp_client.cpp new file mode 100644 index 0000000..a0f747f --- /dev/null +++ b/axiomatic_adapter/src/udp_client.cpp @@ -0,0 +1,214 @@ +#include "axiomatic_adapter/udp_client.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace polymath::axiomatic { + +UdpClient::UdpClient(Options opts) : opts_(std::move(opts)) {} + +UdpClient::~UdpClient() { stop(); } + +bool UdpClient::start() { + if (running_.load()) { + return true; + } + + sock_ = ::socket(AF_INET, SOCK_DGRAM, 0); + if (sock_ < 0) { + return false; + } + + // Bind to ephemeral local port; kernel picks one. + sockaddr_in local{}; + local.sin_family = AF_INET; + local.sin_addr.s_addr = htonl(INADDR_ANY); + local.sin_port = 0; + if (::bind(sock_, reinterpret_cast(&local), sizeof(local)) != 0) { + ::close(sock_); + sock_ = -1; + return false; + } + sockaddr_in bound{}; + socklen_t bound_len = sizeof(bound); + ::getsockname(sock_, reinterpret_cast(&bound), &bound_len); + local_port_ = ntohs(bound.sin_port); + + // Resolve destination. + std::memset(&dst_, 0, sizeof(dst_)); + dst_.sin_family = AF_INET; + dst_.sin_port = htons(opts_.device_port); + if (::inet_pton(AF_INET, opts_.device_ip.c_str(), &dst_.sin_addr) != 1) { + ::close(sock_); + sock_ = -1; + return false; + } + + // Optional one-shot Status Request to register the connection device-side. + if (opts_.send_initial_status_request) { + std::array sr{}; + writeMessageHeader(sr.data(), MessageId::StatusRequest, 0, 0); + sendBytes(sr.data(), sr.size()); + } + + running_.store(true); + tx_thread_ = std::thread(&UdpClient::txLoop, this); + rx_thread_ = std::thread(&UdpClient::rxLoop, this); + return true; +} + +void UdpClient::stop() { + if (!running_.exchange(false)) { + return; + } + // Shut the socket down to break any blocked recvfrom() in rx_thread. + if (sock_ >= 0) { + ::shutdown(sock_, SHUT_RDWR); + } + if (tx_thread_.joinable()) tx_thread_.join(); + if (rx_thread_.joinable()) rx_thread_.join(); + if (sock_ >= 0) { + ::close(sock_); + sock_ = -1; + } + local_port_ = 0; +} + +bool UdpClient::sendFrame(const CanFdFrameRecord & frame) { + const auto wire = encodeCanFdStreamSingleFrame(frame); + if (wire.empty()) { + stats_.tx_errors.fetch_add(1, std::memory_order_relaxed); + return false; + } + if (!sendBytes(wire.data(), wire.size())) { + return false; + } + stats_.frames_sent.fetch_add(1, std::memory_order_relaxed); + return true; +} + +void UdpClient::setOnFrame(FrameCallback cb) { on_frame_ = std::move(cb); } +void UdpClient::setOnErrorFrame(ErrorFrameCallback cb) { on_error_frame_ = std::move(cb); } +void UdpClient::setOnHeartbeat(HeartbeatCallback cb) { on_heartbeat_ = std::move(cb); } + +uint16_t UdpClient::localPort() const { return local_port_; } + +bool UdpClient::sendBytes(const uint8_t * data, std::size_t len) { + if (sock_ < 0) { + stats_.tx_errors.fetch_add(1, std::memory_order_relaxed); + return false; + } + std::lock_guard lk(tx_mutex_); + ssize_t n = ::sendto(sock_, data, len, 0, + reinterpret_cast(&dst_), sizeof(dst_)); + if (n != static_cast(len)) { + stats_.tx_errors.fetch_add(1, std::memory_order_relaxed); + return false; + } + return true; +} + +void UdpClient::txLoop() { + auto next = std::chrono::steady_clock::now(); + while (running_.load(std::memory_order_relaxed)) { + HeartbeatV2 hb{}; + hb.message_number = hb_msg_num_.fetch_add(1, std::memory_order_relaxed); + hb.time_interval_ms = + static_cast(opts_.heartbeat_interval.count()); + hb.supported_features = opts_.supported_features; + const auto wire = encodeHeartbeatV2(hb); + if (sendBytes(wire.data(), wire.size())) { + stats_.heartbeats_sent.fetch_add(1, std::memory_order_relaxed); + } + + // Pace at exactly heartbeat_interval using absolute deadlines. + next += opts_.heartbeat_interval; + while (running_.load(std::memory_order_relaxed)) { + const auto now = std::chrono::steady_clock::now(); + if (now >= next) break; + const auto chunk = std::min( + next - now, std::chrono::milliseconds(100)); + std::this_thread::sleep_for(chunk); + } + } +} + +void UdpClient::rxLoop() { + uint8_t buf[2048]; + while (running_.load(std::memory_order_relaxed)) { + pollfd pfd{sock_, POLLIN, 0}; + int rc = ::poll(&pfd, 1, /*timeout_ms=*/100); + if (rc < 0) { + if (errno == EINTR) continue; + break; + } + if (rc == 0) continue; + if ((pfd.revents & (POLLERR | POLLHUP | POLLNVAL)) != 0) { + // Socket shut down (likely by stop()). + break; + } + if ((pfd.revents & POLLIN) == 0) continue; + + sockaddr_in src{}; + socklen_t src_len = sizeof(src); + ssize_t n = ::recvfrom(sock_, buf, sizeof(buf), 0, + reinterpret_cast(&src), &src_len); + if (n <= 0) { + if (n < 0 && errno == EINTR) continue; + // n == 0 or other error: treat as shutdown. + break; + } + + auto hdr = parseMessageHeader(buf, static_cast(n)); + if (!hdr) { + stats_.parse_errors.fetch_add(1, std::memory_order_relaxed); + continue; + } + switch (static_cast(hdr->message_id)) { + case MessageId::Heartbeat: { + auto hb = parseHeartbeat(buf, static_cast(n)); + if (hb) { + stats_.heartbeats_received.fetch_add(1, std::memory_order_relaxed); + if (on_heartbeat_) on_heartbeat_(*hb); + } else { + stats_.parse_errors.fetch_add(1, std::memory_order_relaxed); + } + break; + } + case MessageId::CanFdStream: { + auto frame = parseCanFdStreamSingleFrame(buf, static_cast(n)); + if (frame) { + if (frame->isError()) { + stats_.error_frames_received.fetch_add(1, std::memory_order_relaxed); + if (on_error_frame_ && frame->length >= 1) { + on_error_frame_(frame->data[0]); + } + } else { + stats_.frames_received.fetch_add(1, std::memory_order_relaxed); + if (on_frame_) on_frame_(*frame); + } + } else { + stats_.parse_errors.fetch_add(1, std::memory_order_relaxed); + } + break; + } + case MessageId::StatusRequest: + case MessageId::StatusResponse: + // Tracked but no callback yet — health/status decoding is its own concern. + break; + default: + stats_.parse_errors.fetch_add(1, std::memory_order_relaxed); + break; + } + } +} + +} // namespace polymath::axiomatic diff --git a/axiomatic_adapter/test/test_axiomatic_protocol.cpp b/axiomatic_adapter/test/test_axiomatic_protocol.cpp new file mode 100644 index 0000000..6709e6b --- /dev/null +++ b/axiomatic_adapter/test/test_axiomatic_protocol.cpp @@ -0,0 +1,680 @@ +#include "axiomatic_adapter/axiomatic_protocol.hpp" + +#include + +#include +#include +#include +#include + +namespace ax = polymath::axiomatic; + +namespace { + +ax::CanFdFrameRecord makeClassicalDataFrame(uint32_t id, + std::initializer_list payload, + bool extended = false) { + ax::CanFdFrameRecord f{}; + f.address = {0u, 0x00000001u}; // CG=0, CID=1 (CAN1 default) + f.timestamp_ms = 12345u; + f.flags = extended ? ax::can_flag::kExtId : 0u; + f.length = static_cast(payload.size()); + f.can_id = id; + std::size_t i = 0; + for (uint8_t b : payload) { + f.data[i++] = b; + } + return f; +} + +ax::CanFdFrameRecord makeFdFrame(uint32_t id, uint8_t length, + bool extended = true, + bool brs = true, + bool esi = false) { + ax::CanFdFrameRecord f{}; + f.address = {0u, 0x00000001u}; + f.timestamp_ms = 0xCAFEBABEu; + uint8_t flags = ax::can_flag::kFd; + if (extended) flags |= ax::can_flag::kExtId; + if (brs) flags |= ax::can_flag::kBrs; + if (esi) flags |= ax::can_flag::kEsi; + f.flags = flags; + f.length = length; + f.can_id = id; + for (uint8_t i = 0; i < length; ++i) { + f.data[i] = static_cast(i ^ 0xA5); + } + return f; +} + +} // namespace + +// --------------------------------------------------------------------------- +// Endianness helpers +// --------------------------------------------------------------------------- + +TEST(Endianness, ReadWriteLE16) { + uint8_t buf[2] = {}; + ax::writeLE16(buf, 0x1234); + EXPECT_EQ(buf[0], 0x34); + EXPECT_EQ(buf[1], 0x12); + EXPECT_EQ(ax::readLE16(buf), 0x1234); +} + +TEST(Endianness, ReadWriteLE32) { + uint8_t buf[4] = {}; + ax::writeLE32(buf, 0xDEADBEEF); + EXPECT_EQ(buf[0], 0xEF); + EXPECT_EQ(buf[1], 0xBE); + EXPECT_EQ(buf[2], 0xAD); + EXPECT_EQ(buf[3], 0xDE); + EXPECT_EQ(ax::readLE32(buf), 0xDEADBEEFu); +} + +TEST(Endianness, ProtocolIdOnWireIsBA36) { + // Per spec: Protocol ID 14010 = 0x36BA, presented LSB-first as bytes (0xBA, 0x36). + uint8_t buf[2] = {}; + ax::writeLE16(buf, ax::kProtocolId); + EXPECT_EQ(buf[0], 0xBAu); + EXPECT_EQ(buf[1], 0x36u); +} + +// --------------------------------------------------------------------------- +// CAN FD length validation +// --------------------------------------------------------------------------- + +TEST(Length, AllValidFdLengths) { + for (uint8_t l = 0; l <= 8; ++l) { + EXPECT_TRUE(ax::isValidCanFdLength(l)) << "len=" << +l; + } + for (uint8_t l : std::array{12, 16, 20, 24, 32, 48, 64}) { + EXPECT_TRUE(ax::isValidCanFdLength(l)) << "len=" << +l; + } +} + +TEST(Length, RejectsInvalidFdLengths) { + for (uint8_t l : std::array{9, 10, 11, 13, 17, 25, 33, 49, 63, 65, 100, 255}) { + EXPECT_FALSE(ax::isValidCanFdLength(l)) << "len=" << +l; + } +} + +// --------------------------------------------------------------------------- +// Envelope codec +// --------------------------------------------------------------------------- + +TEST(Envelope, WriteThenParseRoundTrip) { + std::array buf{}; + const std::size_t n = ax::writeMessageHeader(buf.data(), + ax::MessageId::Heartbeat, + /*version=*/2, + /*data_length=*/42); + ASSERT_EQ(n, ax::kEnvelopeHeaderSize); + + // Wire-level byte checks. + EXPECT_EQ(buf[0], 'A'); + EXPECT_EQ(buf[1], 'X'); + EXPECT_EQ(buf[2], 'I'); + EXPECT_EQ(buf[3], 'O'); + EXPECT_EQ(buf[4], 0xBAu); // protocol id LSB + EXPECT_EQ(buf[5], 0x36u); // protocol id MSB + EXPECT_EQ(buf[6], 0x04u); // message id LSB (Heartbeat = 4) + EXPECT_EQ(buf[7], 0x00u); + EXPECT_EQ(buf[8], 2u); // version + EXPECT_EQ(buf[9], 42u); // data length LSB + EXPECT_EQ(buf[10], 0u); // data length MSB + + const auto h = ax::parseMessageHeader(buf.data(), buf.size()); + ASSERT_TRUE(h.has_value()); + EXPECT_EQ(h->protocol_id, ax::kProtocolId); + EXPECT_EQ(h->message_id, static_cast(ax::MessageId::Heartbeat)); + EXPECT_EQ(h->message_version, 2u); + EXPECT_EQ(h->data_length, 42u); +} + +TEST(Envelope, RejectsTruncatedBuffer) { + std::array buf{}; + ax::writeMessageHeader(buf.data(), ax::MessageId::CanFdStream, 0, 0); + for (std::size_t n = 0; n < ax::kEnvelopeHeaderSize; ++n) { + EXPECT_FALSE(ax::parseMessageHeader(buf.data(), n).has_value()) << "n=" << n; + } +} + +TEST(Envelope, RejectsBadAxioTag) { + std::array buf{}; + ax::writeMessageHeader(buf.data(), ax::MessageId::CanFdStream, 0, 0); + buf[0] = 'B'; // corrupt + EXPECT_FALSE(ax::parseMessageHeader(buf.data(), buf.size()).has_value()); +} + +TEST(Envelope, RejectsWrongProtocolId) { + std::array buf{}; + ax::writeMessageHeader(buf.data(), ax::MessageId::CanFdStream, 0, 0); + buf[4] = 0x00; // protocol id LSB → 0x3600, not 0x36BA + EXPECT_FALSE(ax::parseMessageHeader(buf.data(), buf.size()).has_value()); +} + +TEST(Envelope, RejectsOversizedDataLength) { + std::array buf{}; + ax::writeMessageHeader(buf.data(), ax::MessageId::CanFdStream, 0, 246); + EXPECT_FALSE(ax::parseMessageHeader(buf.data(), buf.size()).has_value()); +} + +TEST(Envelope, AcceptsNullData) { + EXPECT_FALSE(ax::parseMessageHeader(nullptr, 11).has_value()); +} + +// --------------------------------------------------------------------------- +// CAN FD Frame: standard CAN ID +// --------------------------------------------------------------------------- + +TEST(CanFdFrame, ClassicalStdIdRoundTrip_AllDlcs) { + for (uint8_t dlc = 0; dlc <= 8; ++dlc) { + ax::CanFdFrameRecord src{}; + src.address = {7, 0x00000005u}; + src.timestamp_ms = 1000u + dlc; + src.flags = 0u; // classical, std id + src.length = dlc; + src.can_id = 0x123u; + for (uint8_t i = 0; i < dlc; ++i) { + src.data[i] = static_cast(0x10 + i); + } + std::vector wire; + const std::size_t n = ax::writeCanFdFrame(wire, src); + ASSERT_EQ(n, ax::kCanFdFrameHeaderSize + dlc) << "dlc=" << +dlc; + + const auto got = ax::parseCanFdFrame(wire.data(), wire.size()); + ASSERT_TRUE(got.has_value()) << "dlc=" << +dlc; + EXPECT_EQ(got->address.channel_group, src.address.channel_group); + EXPECT_EQ(got->address.channel_id_set, src.address.channel_id_set); + EXPECT_EQ(got->timestamp_ms, src.timestamp_ms); + EXPECT_EQ(got->flags, src.flags); + EXPECT_EQ(got->length, src.length); + EXPECT_EQ(got->can_id, src.can_id); + for (uint8_t i = 0; i < dlc; ++i) { + EXPECT_EQ(got->data[i], src.data[i]); + } + } +} + +TEST(CanFdFrame, StdIdBoundary) { + for (uint32_t id : {0u, 1u, ax::kStdIdMax}) { + auto f = makeClassicalDataFrame(id, {0xAA, 0xBB}, /*extended=*/false); + std::vector wire; + ASSERT_GT(ax::writeCanFdFrame(wire, f), 0u); + auto parsed = ax::parseCanFdFrame(wire.data(), wire.size()); + ASSERT_TRUE(parsed.has_value()); + EXPECT_EQ(parsed->can_id, id); + EXPECT_FALSE(parsed->isExtId()); + } +} + +TEST(CanFdFrame, RejectsStdIdOutOfRange) { + auto f = makeClassicalDataFrame(0x800u, {}, /*extended=*/false); + std::vector wire; + EXPECT_EQ(ax::writeCanFdFrame(wire, f), 0u); +} + +// --------------------------------------------------------------------------- +// CAN FD Frame: extended CAN ID +// --------------------------------------------------------------------------- + +TEST(CanFdFrame, ExtIdBoundary) { + for (uint32_t id : {0u, 0x800u, ax::kExtIdMax}) { + auto f = makeClassicalDataFrame(id, {1, 2, 3, 4, 5, 6, 7, 8}, /*extended=*/true); + std::vector wire; + ASSERT_GT(ax::writeCanFdFrame(wire, f), 0u); + auto parsed = ax::parseCanFdFrame(wire.data(), wire.size()); + ASSERT_TRUE(parsed.has_value()); + EXPECT_EQ(parsed->can_id, id); + EXPECT_TRUE(parsed->isExtId()); + } +} + +TEST(CanFdFrame, RejectsExtIdOutOfRange) { + auto f = makeClassicalDataFrame(0x20000000u, {}, /*extended=*/true); + std::vector wire; + EXPECT_EQ(ax::writeCanFdFrame(wire, f), 0u); +} + +TEST(CanFdFrame, ParseRejectsReservedBitsSetInStdId) { + // Build a valid-ish wire frame, then poke a reserved bit in CANID. + auto f = makeClassicalDataFrame(0x123u, {0x55}, /*extended=*/false); + std::vector wire; + ASSERT_GT(ax::writeCanFdFrame(wire, f), 0u); + // CANID lives at bytes [13..16]; bits [11..31] must be 0 for std ID. + wire[14] |= 0x80u; // poke bit 15 (0x8000 in the 16-bit-equivalent) + EXPECT_FALSE(ax::parseCanFdFrame(wire.data(), wire.size()).has_value()); +} + +// --------------------------------------------------------------------------- +// CAN FD: all FD DLCs and flag combinations +// --------------------------------------------------------------------------- + +TEST(CanFdFrame, FdDataAllValidDlcs) { + for (uint8_t dlc : {uint8_t{0}, uint8_t{1}, uint8_t{8}, uint8_t{12}, + uint8_t{16}, uint8_t{20}, uint8_t{24}, uint8_t{32}, + uint8_t{48}, uint8_t{64}}) { + auto src = makeFdFrame(0x1ABCDEFu, dlc); + std::vector wire; + ASSERT_EQ(ax::writeCanFdFrame(wire, src), ax::kCanFdFrameHeaderSize + dlc); + + auto got = ax::parseCanFdFrame(wire.data(), wire.size()); + ASSERT_TRUE(got.has_value()) << "dlc=" << +dlc; + EXPECT_EQ(got->length, dlc); + EXPECT_TRUE(got->isFd()); + EXPECT_TRUE(got->isExtId()); + EXPECT_TRUE(got->isBrs()); + EXPECT_FALSE(got->isEsi()); + for (uint8_t i = 0; i < dlc; ++i) { + EXPECT_EQ(got->data[i], static_cast(i ^ 0xA5)); + } + } +} + +TEST(CanFdFrame, RejectsInvalidFdDlcOnWrite) { + auto src = makeFdFrame(0x100u, /*length=*/9); + std::vector wire; + EXPECT_EQ(ax::writeCanFdFrame(wire, src), 0u); +} + +TEST(CanFdFrame, ParseRejectsInvalidDlcInFlags) { + // Write a valid frame, corrupt the length field on the wire. + auto src = makeFdFrame(0x100u, /*length=*/8); + std::vector wire; + ASSERT_GT(ax::writeCanFdFrame(wire, src), 0u); + wire[12] = 9; // CANLB byte; 9 is invalid for FD + EXPECT_FALSE(ax::parseCanFdFrame(wire.data(), wire.size()).has_value()); +} + +TEST(CanFdFrame, EveryFlagBitIndependent) { + struct Case { uint8_t flag; const char * name; }; + // Flags that are testable in isolation under "data frame" semantics. + // ERR is exercised separately (different length semantics). + // RTR is mutually exclusive with FD per CAN spec, so we pair it with classical. + const Case classical_cases[] = { + {ax::can_flag::kExtId, "EID"}, + {ax::can_flag::kRemote, "RTR"}, + }; + for (auto c : classical_cases) { + ax::CanFdFrameRecord f{}; + f.address = {0, 1u}; + f.flags = c.flag; + f.length = 0; // empty payload ok for classical and rtr + f.can_id = (c.flag == ax::can_flag::kExtId) ? 0x12345u : 0x123u; + std::vector wire; + ASSERT_GT(ax::writeCanFdFrame(wire, f), 0u) << c.name; + auto got = ax::parseCanFdFrame(wire.data(), wire.size()); + ASSERT_TRUE(got.has_value()) << c.name; + EXPECT_EQ(got->flags, c.flag) << c.name; + } + + // FD-specific flags layered on top of FDF. + const uint8_t fd_only_flags[] = {ax::can_flag::kFd, + ax::can_flag::kFd | ax::can_flag::kBrs, + ax::can_flag::kFd | ax::can_flag::kEsi, + ax::can_flag::kFd | ax::can_flag::kBrs | + ax::can_flag::kEsi}; + for (uint8_t fl : fd_only_flags) { + ax::CanFdFrameRecord f{}; + f.address = {0, 1u}; + f.flags = fl; + f.length = 12; + f.can_id = 0x456u; + std::vector wire; + ASSERT_GT(ax::writeCanFdFrame(wire, f), 0u) << "flags=" << +fl; + auto got = ax::parseCanFdFrame(wire.data(), wire.size()); + ASSERT_TRUE(got.has_value()) << "flags=" << +fl; + EXPECT_EQ(got->flags, fl); + } +} + +TEST(CanFdFrame, RemoteFrameCarriesNoPayloadBytes) { + // RTR with length=8 means "remote request for 8 data bytes" — wire payload still empty. + ax::CanFdFrameRecord f{}; + f.address = {0, 1u}; + f.flags = ax::can_flag::kRemote; + f.length = 8; + f.can_id = 0x111u; + std::vector wire; + const std::size_t n = ax::writeCanFdFrame(wire, f); + EXPECT_EQ(n, ax::kCanFdFrameHeaderSize); // no data bytes appended + auto got = ax::parseCanFdFrame(wire.data(), wire.size()); + ASSERT_TRUE(got.has_value()); + EXPECT_EQ(got->length, 8u); + EXPECT_TRUE(got->isRemote()); +} + +TEST(CanFdFrame, ErrorMessageWithLength1To64) { + for (uint8_t len : {uint8_t{1}, uint8_t{32}, uint8_t{64}}) { + ax::CanFdFrameRecord f{}; + f.address = {0, 1u}; + f.flags = ax::can_flag::kError; + f.length = len; + f.can_id = 0; + f.data[0] = 0xFF; // first error code byte + std::vector wire; + ASSERT_EQ(ax::writeCanFdFrame(wire, f), ax::kCanFdFrameHeaderSize + len); + auto got = ax::parseCanFdFrame(wire.data(), wire.size()); + ASSERT_TRUE(got.has_value()); + EXPECT_EQ(got->length, len); + EXPECT_TRUE(got->isError()); + } +} + +TEST(CanFdFrame, RejectsErrorMessageLengthZero) { + ax::CanFdFrameRecord f{}; + f.address = {0, 1u}; + f.flags = ax::can_flag::kError; + f.length = 0; + std::vector wire; + EXPECT_EQ(ax::writeCanFdFrame(wire, f), 0u); +} + +// --------------------------------------------------------------------------- +// CAN FD Frame: routing + physical channel +// --------------------------------------------------------------------------- + +TEST(CanFdFrame, ChannelAddressIsRoundTripped) { + ax::CanFdFrameRecord f{}; + f.address = {255, 0xFFFFFFFFu}; + f.flags = 0; + f.length = 0; + f.can_id = 0; + std::vector wire; + ASSERT_GT(ax::writeCanFdFrame(wire, f), 0u); + auto got = ax::parseCanFdFrame(wire.data(), wire.size()); + ASSERT_TRUE(got.has_value()); + EXPECT_EQ(got->address.channel_group, 255u); + EXPECT_EQ(got->address.channel_id_set, 0xFFFFFFFFu); +} + +TEST(CanFdFrame, PhysicalChannelMaxBoundary) { + ax::CanFdFrameRecord f{}; + f.physical_channel = 0x1FFFu; // max 13-bit + f.address = {0, 1u}; + f.flags = 0; + f.length = 0; + f.can_id = 0; + std::vector wire; + ASSERT_GT(ax::writeCanFdFrame(wire, f), 0u); + auto got = ax::parseCanFdFrame(wire.data(), wire.size()); + ASSERT_TRUE(got.has_value()); + EXPECT_EQ(got->physical_channel, 0x1FFFu); +} + +TEST(CanFdFrame, RejectsPhysicalChannelOverflow) { + ax::CanFdFrameRecord f{}; + f.physical_channel = 0x2000u; // > 13 bits + f.address = {0, 1u}; + f.length = 0; + std::vector wire; + EXPECT_EQ(ax::writeCanFdFrame(wire, f), 0u); +} + +TEST(Routing, AddressesMatchOnlyWhenGroupEqualAndAnyChannelOverlap) { + EXPECT_TRUE(ax::addressesMatch({0, 0b0011u}, {0, 0b0010u})); + EXPECT_TRUE(ax::addressesMatch({5, 0b0001u}, {5, 0b0001u})); + EXPECT_FALSE(ax::addressesMatch({0, 0b0001u}, {1, 0b0001u})) << "different group"; + EXPECT_FALSE(ax::addressesMatch({0, 0b0001u}, {0, 0b0010u})) << "no overlap"; + EXPECT_FALSE(ax::addressesMatch({0, 0u}, {0, 0xFFFFFFFFu})) << "null address never matches"; +} + +// --------------------------------------------------------------------------- +// Truncated / malformed parse +// --------------------------------------------------------------------------- + +TEST(CanFdFrame, RejectsHeaderShorterThan17) { + std::vector wire(16, 0xFF); + EXPECT_FALSE(ax::parseCanFdFrame(wire.data(), wire.size()).has_value()); +} + +TEST(CanFdFrame, RejectsTruncatedPayload) { + auto src = makeFdFrame(0x100u, /*length=*/16); + std::vector wire; + ax::writeCanFdFrame(wire, src); + for (std::size_t n = ax::kCanFdFrameHeaderSize; + n < ax::kCanFdFrameHeaderSize + 16u; ++n) { + EXPECT_FALSE(ax::parseCanFdFrame(wire.data(), n).has_value()) << "n=" << n; + } +} + +// --------------------------------------------------------------------------- +// Single-frame stream message (envelope + frame, end-to-end) +// --------------------------------------------------------------------------- + +TEST(StreamSingleFrame, RoundTripFd64) { + auto src = makeFdFrame(0x1FFFFFFFu, 64); + const auto wire = ax::encodeCanFdStreamSingleFrame(src); + ASSERT_FALSE(wire.empty()); + EXPECT_EQ(wire.size(), ax::kEnvelopeHeaderSize + ax::kCanFdFrameHeaderSize + 64u); + EXPECT_EQ(wire[0], 'A'); + EXPECT_EQ(wire[6], 0x05u); // CAN FD Stream message id LSB + EXPECT_EQ(wire[7], 0x00u); + + auto got = ax::parseCanFdStreamSingleFrame(wire.data(), wire.size()); + ASSERT_TRUE(got.has_value()); + EXPECT_EQ(got->can_id, src.can_id); + EXPECT_EQ(got->length, 64u); + EXPECT_TRUE(got->isFd()); + for (uint8_t i = 0; i < 64; ++i) { + EXPECT_EQ(got->data[i], static_cast(i ^ 0xA5)); + } +} + +TEST(StreamSingleFrame, RoundTripClassical) { + auto src = makeClassicalDataFrame(0x7FFu, {0xDE, 0xAD, 0xBE, 0xEF}); + const auto wire = ax::encodeCanFdStreamSingleFrame(src); + ASSERT_FALSE(wire.empty()); + + auto got = ax::parseCanFdStreamSingleFrame(wire.data(), wire.size()); + ASSERT_TRUE(got.has_value()); + EXPECT_FALSE(got->isFd()); + EXPECT_EQ(got->length, 4u); + EXPECT_EQ(got->data[0], 0xDEu); + EXPECT_EQ(got->data[3], 0xEFu); +} + +TEST(StreamSingleFrame, RejectsWrongMessageId) { + auto src = makeFdFrame(0x100u, 8); + auto wire = ax::encodeCanFdStreamSingleFrame(src); + ASSERT_FALSE(wire.empty()); + // Flip message id from 5 (CanFdStream) to 4 (Heartbeat). + wire[6] = 0x04; + EXPECT_FALSE(ax::parseCanFdStreamSingleFrame(wire.data(), wire.size()).has_value()); +} + +TEST(StreamSingleFrame, RejectsLengthMismatch) { + auto src = makeFdFrame(0x100u, 8); + auto wire = ax::encodeCanFdStreamSingleFrame(src); + ASSERT_FALSE(wire.empty()); + // Lie about envelope data length (claim 1 byte instead of 17 + 8). + wire[9] = 1; + wire[10] = 0; + EXPECT_FALSE(ax::parseCanFdStreamSingleFrame(wire.data(), wire.size()).has_value()); +} + +// --------------------------------------------------------------------------- +// Fuzzy round-trip (deterministic) +// --------------------------------------------------------------------------- + +// --------------------------------------------------------------------------- +// Heartbeat +// --------------------------------------------------------------------------- + +TEST(Heartbeat, EncodeRoundTrip) { + ax::HeartbeatV2 src{}; + src.message_number = 0x12345678u; + src.time_interval_ms = 1000u; + src.health_data = 0x05559556u; // matches what the device sent us + src.converter_type = 0; + src.supported_features = + ax::supported_features::kCanFdStream | ax::supported_features::kOneFramePerMessage; + src.filter_channel_group = 0; + src.filter_channel_id_set = 0; + + const auto wire = ax::encodeHeartbeatV2(src); + ASSERT_EQ(wire.size(), ax::kEnvelopeHeaderSize + ax::kHeartbeatV2DataSize); + + // Envelope sanity. + EXPECT_EQ(wire[0], 'A'); + EXPECT_EQ(wire[6], 0x04u); // Heartbeat msg id LSB + EXPECT_EQ(wire[8], ax::kHeartbeatVersion); + EXPECT_EQ(wire[9], 22u); // data length LSB + + const auto got = ax::parseHeartbeat(wire.data(), wire.size()); + ASSERT_TRUE(got.has_value()); + EXPECT_EQ(got->message_number, src.message_number); + EXPECT_EQ(got->time_interval_ms, src.time_interval_ms); + EXPECT_EQ(got->health_data, src.health_data); + EXPECT_EQ(got->converter_type, src.converter_type); + EXPECT_EQ(got->supported_features, src.supported_features); + EXPECT_EQ(got->filter_channel_group, src.filter_channel_group); + EXPECT_EQ(got->filter_channel_id_set, src.filter_channel_id_set); +} + +// Parity test: bytes captured from real device (S/N 0010525274, FW V1.00) by +// `axiomatic_probe` on 2026-05-06. Heartbeat #3 from the probe transcript: +// +// ← 33 bytes from 10.74.28.133:4000 +// envelope: AXIO ba 36 04 00 02 16 00 (Heartbeat v2, data_len=22) +// data: f6 2a 01 00 e8 03 00 00 56 95 55 05 +// 00 01 00 00 00 00 00 00 00 00 +// +// This guards against any regression where our codec stops matching real +// device wire bytes. +TEST(Heartbeat, ParseCapturedDeviceFixture) { + const std::array wire{ + // Envelope + 0x41, 0x58, 0x49, 0x4F, // AXIO + 0xBA, 0x36, // Protocol ID = 14010 + 0x04, 0x00, // Message ID = 4 (Heartbeat) + 0x02, // Message Version = 2 + 0x16, 0x00, // Data Length = 22 + // Data + 0xF6, 0x2A, 0x01, 0x00, // Message Number = 0x00012AF6 (76534) + 0xE8, 0x03, 0x00, 0x00, // Time Interval = 0x000003E8 (1000 ms) + 0x56, 0x95, 0x55, 0x05, // Health Data + 0x00, // Converter Type = 0 (Ethernet to CAN w/ Voltage Out) + 0x01, 0x00, 0x00, 0x00, // Supported Features = 0x00000001 (CAN FD Stream) + 0x00, // Filter CGMB = 0 + 0x00, 0x00, 0x00, 0x00, // Filter CIDSMB = 0 (NULL filter, accept all) + }; + const auto hb = ax::parseHeartbeat(wire.data(), wire.size()); + ASSERT_TRUE(hb.has_value()); + EXPECT_EQ(hb->message_number, 0x00012AF6u); + EXPECT_EQ(hb->time_interval_ms, 1000u); + EXPECT_EQ(hb->health_data, 0x05559556u); + EXPECT_EQ(hb->converter_type, 0u); + EXPECT_EQ(hb->supported_features, ax::supported_features::kCanFdStream); + EXPECT_EQ(hb->filter_channel_group, 0u); + EXPECT_EQ(hb->filter_channel_id_set, 0u); +} + +TEST(Heartbeat, RejectsTruncated) { + ax::HeartbeatV2 src{}; + src.supported_features = ax::supported_features::kCanFdStream; + const auto wire = ax::encodeHeartbeatV2(src); + for (std::size_t n = 0; n < wire.size(); ++n) { + EXPECT_FALSE(ax::parseHeartbeat(wire.data(), n).has_value()) + << "n=" << n; + } +} + +TEST(Heartbeat, RejectsWrongMessageId) { + ax::HeartbeatV2 src{}; + auto wire = ax::encodeHeartbeatV2(src); + wire[6] = 0x05; // change Heartbeat → CanFdStream id + EXPECT_FALSE(ax::parseHeartbeat(wire.data(), wire.size()).has_value()); +} + +TEST(Heartbeat, RejectsVersionBelowTwo) { + ax::HeartbeatV2 src{}; + auto wire = ax::encodeHeartbeatV2(src); + wire[8] = 1; // claim Message Version 1 + EXPECT_FALSE(ax::parseHeartbeat(wire.data(), wire.size()).has_value()); +} + +TEST(Heartbeat, AcceptsFutureVersion) { + // Forward-compat: Message Version > 2 with a longer data field is accepted; + // we read only the v2-defined bytes and ignore the rest. + ax::HeartbeatV2 src{}; + src.message_number = 42; + auto wire = ax::encodeHeartbeatV2(src); + // Pretend it's version 3 with extra bytes appended. + wire[8] = 3; + // data_length needs to grow to include the extra padding. + wire[9] = static_cast((ax::kHeartbeatV2DataSize + 5u) & 0xFFu); + wire[10] = 0; + for (int i = 0; i < 5; ++i) wire.push_back(0xAB); + const auto hb = ax::parseHeartbeat(wire.data(), wire.size()); + ASSERT_TRUE(hb.has_value()); + EXPECT_EQ(hb->message_number, 42u); // v2 fields still decoded correctly +} + +TEST(Heartbeat, DataFieldOnlyParse) { + // `parseHeartbeatV2DataField` is for callers that have already sliced out + // the data region (e.g., a streaming parser that handled the envelope upstream). + ax::HeartbeatV2 src{}; + src.message_number = 1; + src.time_interval_ms = 1000; + src.supported_features = ax::supported_features::kCanFdStream; + const auto wire = ax::encodeHeartbeatV2(src); + const uint8_t * data = wire.data() + ax::kEnvelopeHeaderSize; + const auto hb = ax::parseHeartbeatV2DataField(data, ax::kHeartbeatV2DataSize); + ASSERT_TRUE(hb.has_value()); + EXPECT_EQ(hb->message_number, 1u); + EXPECT_EQ(hb->supported_features, ax::supported_features::kCanFdStream); +} + +// --------------------------------------------------------------------------- +// Fuzzy round-trip +// --------------------------------------------------------------------------- + +TEST(Fuzzy, RandomFramesRoundTrip) { + std::mt19937 rng{0xC0FFEE}; + const uint8_t valid_lens[] = {0, 1, 2, 3, 4, 5, 6, 7, 8, + 12, 16, 20, 24, 32, 48, 64}; + for (int iter = 0; iter < 500; ++iter) { + ax::CanFdFrameRecord f{}; + f.physical_channel = + static_cast(std::uniform_int_distribution(0u, 0x1FFFu)(rng)); + f.address.channel_group = + static_cast(std::uniform_int_distribution(0u, 255u)(rng)); + f.address.channel_id_set = std::uniform_int_distribution{}(rng); + f.timestamp_ms = std::uniform_int_distribution{}(rng); + + const bool fd = (rng() & 1u) == 1u; + const bool ext = (rng() & 1u) == 1u; + uint8_t flags = 0; + if (fd) flags |= ax::can_flag::kFd; + if (ext) flags |= ax::can_flag::kExtId; + if (fd && (rng() & 1u)) flags |= ax::can_flag::kBrs; + if (fd && (rng() & 1u)) flags |= ax::can_flag::kEsi; + f.flags = flags; + f.length = fd + ? valid_lens[std::uniform_int_distribution(0u, 15u)(rng)] + : static_cast(std::uniform_int_distribution(0u, 8u)(rng)); + f.can_id = ext + ? std::uniform_int_distribution(0u, ax::kExtIdMax)(rng) + : std::uniform_int_distribution(0u, ax::kStdIdMax)(rng); + for (uint8_t i = 0; i < f.length; ++i) { + f.data[i] = static_cast(rng() & 0xFFu); + } + + std::vector wire; + const std::size_t n = ax::writeCanFdFrame(wire, f); + ASSERT_GT(n, 0u); + auto got = ax::parseCanFdFrame(wire.data(), wire.size()); + ASSERT_TRUE(got.has_value()); + EXPECT_EQ(got->physical_channel, f.physical_channel); + EXPECT_EQ(got->address.channel_group, f.address.channel_group); + EXPECT_EQ(got->address.channel_id_set, f.address.channel_id_set); + EXPECT_EQ(got->timestamp_ms, f.timestamp_ms); + EXPECT_EQ(got->flags, f.flags); + EXPECT_EQ(got->length, f.length); + EXPECT_EQ(got->can_id, f.can_id); + for (uint8_t i = 0; i < f.length; ++i) { + EXPECT_EQ(got->data[i], f.data[i]) << "iter=" << iter << " byte=" << +i; + } + } +} diff --git a/axiomatic_adapter/test/test_udp_client.cpp b/axiomatic_adapter/test/test_udp_client.cpp new file mode 100644 index 0000000..a3f5c1e --- /dev/null +++ b/axiomatic_adapter/test/test_udp_client.cpp @@ -0,0 +1,145 @@ +// Lifecycle tests for UdpClient — no hardware required. +// +// We start against a private TEST-NET-1 address (192.0.2.1, RFC 5737) that +// is guaranteed never to respond. UDP is connectionless so sendto() just +// queues bytes that go nowhere; the rx thread blocks on poll() until stop(). +// This is sufficient to catch races/leaks in start/stop, double-start, +// destructor cleanup, and sendFrame() encode-validation paths. + +#include "axiomatic_adapter/udp_client.hpp" + +#include + +#include +#include + +namespace ax = polymath::axiomatic; + +namespace { +constexpr const char * kBlackHoleIp = "192.0.2.1"; // RFC 5737 TEST-NET-1 +} + +TEST(UdpClient, ConstructWithoutStartIsSafe) { + ax::UdpClient::Options opts; + opts.device_ip = kBlackHoleIp; + ax::UdpClient client(opts); + // Destructor runs without start(). +} + +TEST(UdpClient, StartStopRoundTrip) { + ax::UdpClient::Options opts; + opts.device_ip = kBlackHoleIp; + opts.heartbeat_interval = std::chrono::milliseconds(50); + ax::UdpClient client(opts); + ASSERT_TRUE(client.start()); + EXPECT_NE(client.localPort(), 0u); + // Let the heartbeat thread fire at least once. + std::this_thread::sleep_for(std::chrono::milliseconds(120)); + client.stop(); + EXPECT_GE(client.stats().heartbeats_sent.load(), 1u); + EXPECT_EQ(client.localPort(), 0u); +} + +TEST(UdpClient, DoubleStartIsIdempotent) { + ax::UdpClient::Options opts; + opts.device_ip = kBlackHoleIp; + ax::UdpClient client(opts); + ASSERT_TRUE(client.start()); + EXPECT_TRUE(client.start()); // already running + client.stop(); +} + +TEST(UdpClient, DoubleStopIsSafe) { + ax::UdpClient::Options opts; + opts.device_ip = kBlackHoleIp; + ax::UdpClient client(opts); + ASSERT_TRUE(client.start()); + client.stop(); + client.stop(); // no-op +} + +TEST(UdpClient, RestartAfterStop) { + ax::UdpClient::Options opts; + opts.device_ip = kBlackHoleIp; + ax::UdpClient client(opts); + ASSERT_TRUE(client.start()); + const uint16_t port_a = client.localPort(); + client.stop(); + ASSERT_TRUE(client.start()); + const uint16_t port_b = client.localPort(); + EXPECT_NE(port_b, 0u); + // ports may or may not be reused; we only assert non-zero on the second go. + (void)port_a; + client.stop(); +} + +TEST(UdpClient, RejectsBadIp) { + ax::UdpClient::Options opts; + opts.device_ip = "not-an-ip"; + ax::UdpClient client(opts); + EXPECT_FALSE(client.start()); +} + +TEST(UdpClient, SendFrameWithoutStartFails) { + ax::UdpClient::Options opts; + opts.device_ip = kBlackHoleIp; + ax::UdpClient client(opts); + ax::CanFdFrameRecord f{}; + f.address = {0u, 0x01u}; + f.flags = 0; + f.length = 0; + f.can_id = 0x123; + EXPECT_FALSE(client.sendFrame(f)); + EXPECT_EQ(client.stats().tx_errors.load(), 1u); +} + +TEST(UdpClient, SendFrameValidatesEncode) { + ax::UdpClient::Options opts; + opts.device_ip = kBlackHoleIp; + ax::UdpClient client(opts); + ASSERT_TRUE(client.start()); + // Out-of-range standard ID — encoder rejects. + ax::CanFdFrameRecord bad{}; + bad.address = {0u, 0x01u}; + bad.flags = 0; + bad.length = 0; + bad.can_id = 0x800; // > 0x7FF and EID flag not set + EXPECT_FALSE(client.sendFrame(bad)); + EXPECT_GE(client.stats().tx_errors.load(), 1u); + // Good frame succeeds (sendto to black hole still returns OK; bytes go to /dev/null). + ax::CanFdFrameRecord good{}; + good.address = {0u, 0x01u}; + good.flags = 0; + good.length = 0; + good.can_id = 0x123; + EXPECT_TRUE(client.sendFrame(good)); + EXPECT_EQ(client.stats().frames_sent.load(), 1u); + client.stop(); +} + +TEST(UdpClient, CallbacksNotSetIsSafe) { + // No on-frame, on-error, on-heartbeat callbacks set — rx thread must not crash + // even though we won't receive anything from the black hole anyway. + ax::UdpClient::Options opts; + opts.device_ip = kBlackHoleIp; + ax::UdpClient client(opts); + ASSERT_TRUE(client.start()); + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + client.stop(); +} + +TEST(UdpClient, HeartbeatPacingApprox1Hz) { + // Run with a tighter interval to keep the test fast, verify the count. + ax::UdpClient::Options opts; + opts.device_ip = kBlackHoleIp; + opts.heartbeat_interval = std::chrono::milliseconds(50); + opts.send_initial_status_request = false; + ax::UdpClient client(opts); + ASSERT_TRUE(client.start()); + std::this_thread::sleep_for(std::chrono::milliseconds(525)); + client.stop(); + const auto sent = client.stats().heartbeats_sent.load(); + // 525 ms / 50 ms = ~10.5 heartbeats. Allow [9, 12] for jitter. + EXPECT_GE(sent, 9u); + EXPECT_LE(sent, 12u); +} diff --git a/socketcan_adapter/include/socketcan_adapter/i_can_backend.hpp b/socketcan_adapter/include/socketcan_adapter/i_can_backend.hpp new file mode 100644 index 0000000..6a0c814 --- /dev/null +++ b/socketcan_adapter/include/socketcan_adapter/i_can_backend.hpp @@ -0,0 +1,110 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SOCKETCAN_ADAPTER__I_CAN_BACKEND_HPP_ +#define SOCKETCAN_ADAPTER__I_CAN_BACKEND_HPP_ + +#include +#include +#include +#include +#include + +#include "socketcan_adapter/can_frame.hpp" + +namespace polymath::socketcan +{ + +/// @brief State of the underlying transport (socket, TCP/UDP connection, etc). +/// SocketState was previously the SocketcanAdapter-only enum; lifting it here +/// so any backend (Linux SocketCAN, Axiomatic Ethernet/CAN, future variants) +/// reports its open/closed/error state through one type. +enum class SocketState; // forward — defined in socketcan_adapter.hpp + +/// @brief FilterMode for how to add filters +enum class FilterMode; // forward — defined in socketcan_adapter.hpp + +/// @class polymath::socketcan::ICanBackend +/// @brief Cross-cutting interface implemented by every CAN transport backend. +/// +/// Carved out of SocketcanAdapter's public surface in 2026-05 to allow +/// non-SocketCAN backends (e.g. the Axiomatic AX140970 Dual CAN FD over +/// Ethernet converter) to plug into the same ROS2 bridge node without forking +/// the runtime path. Linux-specific knobs (CAN_RAW_FILTER vector, ERR mask, +/// JOIN flag) stay on the concrete SocketcanAdapter — they have no analog in +/// the Axiomatic protocol and forcing every backend to no-op them muddies +/// the contract. +/// +/// All implementations must: +/// * be safe to construct without opening any sockets (open happens in +/// openSocket()), +/// * tolerate openSocket()/closeSocket() being called multiple times, +/// * deliver received frames to the on-receive callback set by +/// setOnReceiveCallback() once startReceptionThread() has been called. +class ICanBackend +{ +public: + /// @brief Mapped to std lib, but should be remapped to Polymath Safety compatible versions + using socket_error_string_t = std::string; + + virtual ~ICanBackend() = default; + + // --------------------------------------------------------------------------- + // Lifecycle + // --------------------------------------------------------------------------- + + /// @brief Open the underlying transport. Returns true on success. + virtual bool openSocket() = 0; + + /// @brief Close the underlying transport. Returns true on success. + virtual bool closeSocket() = 0; + + /// @brief Get the current transport state. + virtual SocketState get_socket_state() = 0; + + // --------------------------------------------------------------------------- + // Send / receive + // --------------------------------------------------------------------------- + + /// @brief Transmit a frame. + /// @param frame INPUT shared_ptr to a frame to send. + /// @return optional error string if any. + virtual std::optional send(const std::shared_ptr frame) = 0; + + /// @brief Transmit a frame. + virtual std::optional send(const CanFrame & frame) = 0; + + /// @brief Block-receive a frame into `frame`. Subject to the receive timeout + /// configured via the constructor / set_receive_timeout(). + /// @return optional error string if any. + virtual std::optional receive(CanFrame & frame) = 0; + + // --------------------------------------------------------------------------- + // Reception thread + callbacks + // --------------------------------------------------------------------------- + + virtual bool startReceptionThread() = 0; + + virtual bool joinReceptionThread(const std::chrono::duration & timeout_s) = 0; + + virtual bool is_thread_running() = 0; + + virtual bool setOnReceiveCallback(std::function frame)> && callback) = 0; + + virtual bool setOnErrorCallback(std::function && callback) = 0; +}; + +} // namespace polymath::socketcan + +#endif // SOCKETCAN_ADAPTER__I_CAN_BACKEND_HPP_ diff --git a/socketcan_adapter/include/socketcan_adapter/socketcan_adapter.hpp b/socketcan_adapter/include/socketcan_adapter/socketcan_adapter.hpp index aea13b3..a370da2 100644 --- a/socketcan_adapter/include/socketcan_adapter/socketcan_adapter.hpp +++ b/socketcan_adapter/include/socketcan_adapter/socketcan_adapter.hpp @@ -28,6 +28,7 @@ #include #include "socketcan_adapter/can_frame.hpp" +#include "socketcan_adapter/i_can_backend.hpp" namespace polymath::socketcan { @@ -55,12 +56,17 @@ constexpr nfds_t NUM_SOCKETS_IN_ADAPTER = 1; /// @class polymath::socketcan::SocketcanAdapter /// @brief Creates and manages a socketcan instance and simplifies the interface. -/// Generally does not throw, but returns booleans to tell you success -class SocketcanAdapter : public std::enable_shared_from_this +/// Generally does not throw, but returns booleans to tell you success. +/// +/// As of 2026-05 SocketcanAdapter implements ICanBackend so the ROS2 bridge +/// node and other consumers can be retargeted to alternate transports (e.g. +/// the Axiomatic AX140970 over Ethernet) without changes to call sites. +/// Linux-specific knobs (filter vector, error mask, JOIN flag) remain +/// concrete-only since they have no analog on non-SocketCAN backends. +class SocketcanAdapter : public ICanBackend, public std::enable_shared_from_this { public: - /// @brief Mapped to std lib, but should be remapped to Polymath Safety compatible versions - using socket_error_string_t = std::string; + using socket_error_string_t = ICanBackend::socket_error_string_t; using filter_vector_t = std::vector; /// @brief SocketcanAdapter Class Init @@ -70,15 +76,15 @@ class SocketcanAdapter : public std::enable_shared_from_this const std::chrono::duration & receive_timeout_s = SOCKET_RECEIVE_TIMEOUT_S); /// @brief Destructor for SocketcanAdapter - virtual ~SocketcanAdapter(); + ~SocketcanAdapter() override; /// @brief Open Socket /// @return bool successfully opened socket - bool openSocket(); + bool openSocket() override; /// @brief Close Socket /// @return bool successfully closed socket - bool closeSocket(); + bool closeSocket() override; /// @brief Set a number of filters, vectorized /// @param filters reference to a vector of can filters to set for the socket @@ -108,7 +114,7 @@ class SocketcanAdapter : public std::enable_shared_from_this /// @brief Receive with a reference to a CanFrame to fill /// @param frame OUTPUT CanFrame to fill /// @return optional error string filled with an error message if any - std::optional receive(CanFrame & can_frame); + std::optional receive(CanFrame & can_frame) override; /// TODO: Switch to unique ptr /// https://gitlab.com/polymathrobotics/polymath_core/-/issues/8 @@ -124,27 +130,27 @@ class SocketcanAdapter : public std::enable_shared_from_this /// @brief Start a reception thread (calls callback) /// @return success on started - bool startReceptionThread(); + bool startReceptionThread() override; /// @brief Stop and join reception thread /// @param timeout_s INPUT timeout in seconds, <=0 means no timeout /// @return success on closed and joined thread - bool joinReceptionThread(const std::chrono::duration & timeout_s = JOIN_RECEPTION_TIMEOUT_S); + bool joinReceptionThread(const std::chrono::duration & timeout_s = JOIN_RECEPTION_TIMEOUT_S) override; /// @brief Set receive callback function if thread is used /// @param callback_function INPUT To be called on receipt of a can frame /// @return success on receive callback set - bool setOnReceiveCallback(std::function frame)> && callback_function); + bool setOnReceiveCallback(std::function frame)> && callback_function) override; /// @brief Set receive callback function if thread is used /// @param callback_function INPUT To be called on receipt of a can frame /// @return success on error callback set - bool setOnErrorCallback(std::function && callback_function); + bool setOnErrorCallback(std::function && callback_function) override; /// @brief Transmit a can frame via socket /// @param frame INPUT const reference to the frame /// @return optional error string filled with an error message if any - std::optional send(const CanFrame & frame); + std::optional send(const CanFrame & frame) override; /// @brief Transmit a can frame via socket /// @param frame INPUT shared_ptr to frame. Convert non-const to const by doing @@ -154,7 +160,7 @@ class SocketcanAdapter : public std::enable_shared_from_this /// std::shared_ptr frame = frame /// ``` /// @return optional error string filled with an error message if any - std::optional send(const std::shared_ptr frame); + std::optional send(const std::shared_ptr frame) override; /// @brief Transmit a can frame via socket /// @param frame Linux CAN frame to send @@ -167,7 +173,7 @@ class SocketcanAdapter : public std::enable_shared_from_this /// @brief Get state of socket /// @return SocketState data type detailing OPEN or CLOSED - SocketState get_socket_state(); + SocketState get_socket_state() override; /// @brief Get interface /// @return Can interface @@ -175,7 +181,7 @@ class SocketcanAdapter : public std::enable_shared_from_this /// @brief Checks if the receive thread is running /// @return True if the thread is running, false otherwise - bool is_thread_running(); + bool is_thread_running() override; private: /// @brief Wraps C level socket operations to set can_filter frames