diff --git a/axiomatic_adapter/CMakeLists.txt b/axiomatic_adapter/CMakeLists.txt index e453477..d7d7e61 100644 --- a/axiomatic_adapter/CMakeLists.txt +++ b/axiomatic_adapter/CMakeLists.txt @@ -78,6 +78,7 @@ install(TARGETS axiomatic_adapter add_executable(axiomatic_socketcan_bridge src/axiomatic_socketcan_bridge.cpp src/axiomatic_socketcan_bridge_node.cpp + src/serial_port.cpp ) target_include_directories(axiomatic_socketcan_bridge PUBLIC @@ -112,6 +113,23 @@ if(BUILD_TESTING) add_test(NAME test_axiomatic_model COMMAND test_axiomatic_model) endif() + add_executable(test_axiomatic_adapter_loopback test/axiomatic_adapter_loopback_test.cpp) + target_link_libraries(test_axiomatic_adapter_loopback PRIVATE axiomatic_adapter Boost::boost Catch2::Catch2WithMain) + if(COMMAND catch_discover_tests) + catch_discover_tests(test_axiomatic_adapter_loopback) + else() + add_test(NAME test_axiomatic_adapter_loopback COMMAND test_axiomatic_adapter_loopback) + endif() + + add_executable(test_serial_port test/serial_port_test.cpp src/serial_port.cpp) + target_include_directories(test_serial_port PRIVATE ${CMAKE_CURRENT_SOURCE_DIR}/include) + target_link_libraries(test_serial_port PRIVATE Catch2::Catch2WithMain) + if(COMMAND catch_discover_tests) + catch_discover_tests(test_serial_port) + else() + add_test(NAME test_serial_port COMMAND test_serial_port) + endif() + option(HARDWARE_CONNECTED "Enable tests that require a connected Axiomatic device and CAN interface" OFF) if(HARDWARE_CONNECTED) diff --git a/axiomatic_adapter/README.md b/axiomatic_adapter/README.md index 01aabcc..79dfebe 100644 --- a/axiomatic_adapter/README.md +++ b/axiomatic_adapter/README.md @@ -7,29 +7,31 @@ https://www.notion.so/polymathrobotics/Axiomatic-CAN-to-Ethernet-Converter-08e07 ## Supported Models -| `--model` | Device | Protocol reference | -| --- | --- | --- | -| `ax140900` (default) | [AX140900 CAN/Ethernet Converter](https://www.axiomatic.com/product/canethernet-converter-ax140900/) | [Ethernet to CAN Converter Communication Protocol](https://www.axiomatic.com/wp-content/uploads/Ethernet-to-CAN-Converter-Communication-Protocol.pdf) | -| `ax142100a` | [AX142100A Protocol Converter, Ethernet/RS-422/2x RS-232/CAN](https://www.axiomatic.com/product/protocol-converter-ethernet-rs-422-2-rs-232-can-sae-j1939-ax142100a/) | [UMAX142100A](https://www.axiomatic.com/wp-content/uploads/UMAX142100A.pdf), section 4.2 | +| `--model` | Device | Raw (serial) data | Protocol reference | +| --- | --- | --- | --- | +| `ax140900` (default) | [AX140900 CAN/Ethernet Converter](https://www.axiomatic.com/product/canethernet-converter-ax140900/) | no | [Ethernet to CAN Converter Communication Protocol](https://www.axiomatic.com/wp-content/uploads/Ethernet-to-CAN-Converter-Communication-Protocol.pdf) | +| `ax142100a` | [AX142100A Protocol Converter, Ethernet/RS-422/2x RS-232/CAN](https://www.axiomatic.com/product/protocol-converter-ethernet-rs-422-2-rs-232-can-sae-j1939-ax142100a/) | yes | [UMAX142100A](https://www.axiomatic.com/wp-content/uploads/UMAX142100A.pdf), section 4.2 | Every model shares the 11-byte `AXIO` message header in `axiomatic_protocol.hpp`. Each model is a subclass of `AxiomaticModel` (`axiomatic_model.hpp`) in `include/axiomatic_adapter/models/` and `src/models/`. A model takes bytes and `CanFrame`s only, so it can be used without a socket. +`decode` returns CAN frames, raw data payloads, and the number of bytes consumed; pass the unconsumed tail again with the next bytes received. ### Adding a model 1. Add `include/axiomatic_adapter/models/.hpp` and `src/models/.cpp` with a `::` class deriving `AxiomaticModel`. Override `protocolId`, `encode` (build on `protocol::encodeMessage`), and `decodeMessage` (one message body; return `false` for Message IDs that carry no CAN frames). The base class `decode` splits the buffer into messages and calls `decodeMessage` for each. + If the model forwards raw (serial) data, also override `encodeRawData` and append decoded payloads to `DecodeResult::raw_data`. 2. Add the source to the `axiomatic_adapter` library in `CMakeLists.txt`. 3. Add its entry to `modelFactories` in `src/axiomatic_model.cpp`. -4. Add a decode test of the manual's example bytes in `test/axiomatic_model_test.cpp`; the round trip test covers every name in `modelNames`. +4. Add a decode test of the manual's example bytes in `test/axiomatic_model_test.cpp`; the round trip tests cover every name in `modelNames`. ## Usage ### Socketcan-Axiomatic Bridge ```bash -ros2 run axiomatic_adapter axiomatic_socketcan_bridge [CAN_INTERFACE_NAME] [IP_ADDRESS] [PORT] [OPTIONAL]--model[-m] [OPTIONAL]--retry-connection[-r] [OPTIONAL]--max-retry-attempts [OPTIONAL]--verbose[-v] [OPTIONAL]--no-tcp-nodelay +ros2 run axiomatic_adapter axiomatic_socketcan_bridge [CAN_INTERFACE_NAME] [IP_ADDRESS] [PORT] [OPTIONAL]--model[-m] [OPTIONAL]--retry-connection[-r] [OPTIONAL]--max-retry-attempts [OPTIONAL]--verbose[-v] [OPTIONAL]--no-tcp-nodelay [OPTIONAL]--serial-port # Examples # generic example to bridge vcan0 with axiomatic using 192.168.50.34:4000 @@ -38,6 +40,9 @@ ros2 run axiomatic_adapter axiomatic_socketcan_bridge vcan0 192.168.50.34 4000 # bridge vcan0 with an AX142100A ros2 run axiomatic_adapter axiomatic_socketcan_bridge vcan0 192.168.50.34 4000 --model ax142100a +# also bridge the AX142100A's serial data to one end of a virtual serial pair (see Serial data below) +ros2 run axiomatic_adapter axiomatic_socketcan_bridge vcan0 192.168.1.20 4000 --model ax142100a --serial-port /dev/ttyAX0 + # this will continue retrying to connect forever and not exit on first failure. It will also print more detailed logs ros2 run axiomatic_adapter axiomatic_socketcan_bridge vcan0 192.168.50.34 4000 -r -v @@ -50,6 +55,45 @@ ros2 run axiomatic_adapter axiomatic_socketcan_bridge vcan0 192.168.50.34 4000 - ros2 run axiomatic_adapter axiomatic_socketcan_bridge vcan0 192.168.50.34 4000 --no-tcp-nodelay ``` +### Serial data (AX142100A) + +`--serial-port ` opens an existing serial device in raw mode; the bridge does not create it. +Bytes read from the device are sent to the converter as Forwarded Data with the raw data flag set. +Raw data the converter forwards to Ethernet is written to the device. +The bridge does not set a baud rate; the converter's Main Settings page sets each serial port's. + +For an application on the same host, create a virtual serial pair, give one end to the bridge, and open the other end in the application. +Each end must have exactly one reader; the bridge and the application must not open the same end. + +```bash +# create the pair; run this as its own long-lived service +socat -d -d pty,raw,echo=0,link=/dev/ttyAX0 pty,raw,echo=0,link=/dev/ttyAX1 + +# bridge end +ros2 run axiomatic_adapter axiomatic_socketcan_bridge vcan0 192.168.1.20 4000 --model ax142100a --serial-port /dev/ttyAX0 + +# application end +picocom /dev/ttyAX1 +``` + +A pair that outlives the bridge keeps the application's end open across bridge restarts. + +The protocol message carries no serial port number. +The converter's routing configuration (web interface, UMAX142100A section 4) decides which ports the data goes to and comes from. +Interfaces are numbered RS-232 #1 = 1, RS-232 #2 = 2, RS-422 = 3, CAN = 4, Ethernet = 5. +On Main Settings, set Data Port Type to TCP; the bridge does not speak UDP. +In Ethernet and serial routing rules, leave "Forward all data bytes" at its default, or the rule truncates what it forwards. + +- **Ethernet → serial**: on `eth_data_routing.shtml`, list the RS ports in a rule's Output interfaces. + Raw data is forwarded as is, subject only to these rules (UMAX142100A section 4.2). +- **Ethernet → CAN**: CAN frames sent by the bridge pass through the same Ethernet routing rules, so a rule must also list CAN (4). +- **Serial → Ethernet**: on `serial_data_routing.shtml`, list Ethernet (5) in the RS port's rule Output interfaces. +- **CAN → Ethernet**: on `can_data_routing.shtml`, list Ethernet (5) and set Data forwarding to "Forward full message", as in the UMAX142100A CAN routing example. + "Forward all data bytes" appears to forward the data bytes without the CAN ID. + +All routed ports share the one serial device. +To tell ports apart, configure distinct start bytes per port in the serial routing rules. + ### Library ```cpp @@ -72,9 +116,15 @@ polymath::can::AxiomaticAdapter adapter( // open the socket adapter.openSocket(); +// optionally receive raw (serial) data; set before starting the reception thread +adapter.setOnRawDataCallback([](std::vector /*data*/) { /* No-op */ }); + // start the reception thread. At this point, it's running adapter.startReceptionThread(); +// send raw (serial) data; returns an error for a model without raw data +adapter.sendRawData({'h', 'i', '\r', '\n'}); + // on shutdown/destruction, thread will join and socket will close ``` @@ -103,5 +153,5 @@ If you have any doubt, leave it at the default. 2. Axiomatic-specific status messages are ignored and deliberately skipped. In future revisions this should be handled and reported as necessary. 3. CAN FD is not supported 4. Only TCP mode is supported; no UDP support (heartbeats are required for UDP support) -5. AX142100A raw data (serial) payloads are skipped; only CAN frames are delivered. -6. AX142100A 11-bit IDs are assumed to be 2 bytes on the wire; the manual only shows a 29-bit example. +5. AX142100A 11-bit IDs are assumed to be 2 bytes on the wire; the manual only shows a 29-bit example. +6. The AX142100A's maximum raw data payload per message is unconfirmed. diff --git a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp index 5b541b0..396c63b 100644 --- a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp +++ b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp @@ -19,10 +19,12 @@ #include #include +#include #include #include #include #include +#include #include "axiomatic_adapter/axiomatic_model.hpp" #include "axiomatic_adapter/models/ax140900.hpp" @@ -103,6 +105,12 @@ class AxiomaticAdapter : public std::enable_shared_from_this /// @return success on started bool startReceptionThread(); + /// @brief Set the callback for raw (serial) data payloads. + /// Called on the thread that read them: the reception thread, or the caller of receive(). + /// Unset drops raw data. + /// @return false while the reception thread is running + bool setOnRawDataCallback(std::function data)> && callback_function); + /// @brief Stop and join reception thread /// @param timeout_s INPUT timeout in seconds, <=0 means no timeout /// @return success on closed and joined thread @@ -118,6 +126,14 @@ class AxiomaticAdapter : public std::enable_shared_from_this /// @return optional error string filled with an error message if any std::optional send(const can_frame & frame); + /// @brief Transmit raw (serial) data via socket + /// @param data payload, at most the model's raw data limit + /// @return optional error string, set when the model has no raw data channel or the write fails + std::optional sendRawData(const std::vector & data); + + /// @brief Whether the model carries raw (serial) data + bool supportsRawData() const; + /// @brief Get state of socket /// @return TCPSocketState data type detailing OPEN or CLOSED TCPSocketState get_socket_state(); diff --git a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_model.hpp b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_model.hpp index f0bc466..1f0c8be 100644 --- a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_model.hpp +++ b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_model.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -29,7 +30,7 @@ namespace polymath::can /// @class polymath::can::AxiomaticModel /// @brief One Axiomatic converter model and its wire format. -/// Subclasses override protocolId, encode, and decodeMessage. +/// Subclasses override protocolId, encode, and decodeMessage, and encodeRawData when the model forwards raw data. /// Methods are const and keep no state between calls; one instance may be shared across threads. class AxiomaticModel { @@ -39,8 +40,16 @@ class AxiomaticModel /// @brief Encode one CAN frame as one complete protocol message virtual std::vector encode(const polymath::socketcan::CanFrame & frame) const = 0; - /// @brief Decode every CAN frame in a buffer of back-to-back protocol messages. - /// Messages with a Message ID that carries no CAN frames are skipped. + /// @brief Encode raw (serial) data as one complete protocol message + /// @return std::nullopt when the model has no raw data channel + virtual std::optional> encodeRawData(const std::vector & data) const; + + /// @brief Whether encodeRawData and decode carry raw data + bool supportsRawData() const; + + /// @brief Decode every CAN frame and raw data payload in a buffer of back-to-back protocol messages. + /// Messages with a Message ID that carries neither are skipped. + /// A trailing incomplete message is left unconsumed; pass it again with the bytes that follow. /// @param data buffer start /// @param size buffer length in bytes protocol::DecodeResult decode(const uint8_t * data, size_t size) const; @@ -49,8 +58,8 @@ class AxiomaticModel /// @brief Protocol ID every message of this model carries virtual uint16_t protocolId() const = 0; - /// @brief Append the CAN frames in one message to result - /// @return false when the Message ID carries no CAN frames + /// @brief Append the CAN frames and raw data in one message to result + /// @return false when the Message ID carries neither virtual bool decodeMessage(const protocol::MessageView & message, protocol::DecodeResult & result) const = 0; }; diff --git a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_protocol.hpp b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_protocol.hpp index a3034e5..b96c7f0 100644 --- a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_protocol.hpp +++ b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_protocol.hpp @@ -55,13 +55,19 @@ struct ParsedMessages { std::vector messages; std::vector diagnostics; + /// @brief Leading bytes parsed or dropped; the rest is an incomplete message to retry with more data + size_t consumed{0}; }; -/// @brief CAN frames decoded from a buffer, plus a description of every byte range skipped or dropped +/// @brief CAN frames and raw data decoded from a buffer, plus a description of every byte range skipped or dropped struct DecodeResult { std::vector frames; + /// @brief Raw data payloads, in arrival order + std::vector> raw_data; std::vector diagnostics; + /// @brief Leading bytes decoded or dropped; the rest is an incomplete message to retry with more data + size_t consumed{0}; }; /// @brief Read an unsigned little-endian integer @@ -82,8 +88,8 @@ std::vector encodeMessage( uint16_t protocol_id, uint16_t message_id, uint8_t message_version, const std::vector & body); /// @brief Split a buffer into back-to-back protocol messages carrying protocol_id. -/// Scanning stops at the first header whose tag or Protocol ID does not match. -/// A message whose declared length runs past the buffer is returned truncated to the bytes present. +/// Scanning stops at the first header whose tag or Protocol ID does not match, and the rest of the buffer is consumed. +/// A trailing message or header that runs past the buffer is not returned or consumed. /// @param protocol_id Protocol ID every message must carry /// @param data buffer start /// @param size buffer length in bytes diff --git a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_socketcan_bridge.hpp b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_socketcan_bridge.hpp index 271d3b7..7d37c54 100644 --- a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_socketcan_bridge.hpp +++ b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_socketcan_bridge.hpp @@ -15,11 +15,15 @@ #ifndef AXIOMATIC_ADAPTER__AXIOMATIC_SOCKETCAN_BRIDGE_NODE_HPP_ #define AXIOMATIC_ADAPTER__AXIOMATIC_SOCKETCAN_BRIDGE_NODE_HPP_ +#include #include +#include #include +#include #include "axiomatic_adapter/axiomatic_adapter.hpp" #include "axiomatic_adapter/models/ax140900.hpp" +#include "axiomatic_adapter/serial_port.hpp" #include "socketcan_adapter/can_frame.hpp" #include "socketcan_adapter/socketcan_adapter.hpp" @@ -27,7 +31,8 @@ namespace polymath::can { /// @class polymath::can::AxiomaticSocketcanBridge -/// @brief This class will bridge a socketcan connection and read/write to the can-eth device tcp connection +/// @brief This class will bridge a socketcan connection and read/write to the can-eth device tcp connection. +/// Optionally bridges the device's raw (serial) data to a local serial port. class AxiomaticSocketcanBridge { public: @@ -40,19 +45,21 @@ class AxiomaticSocketcanBridge /// AxiomaticAdapter socket. See AxiomaticAdapter for the /// full tradeoff discussion. /// @param model converter model, e.g. std::make_unique(); must not be null + /// @param serial_port_path when set, existing serial device bridged to the model's raw data, e.g. /dev/ttyAX0 AxiomaticSocketcanBridge( const std::string & can_interface_name, const std::string & ip, const std::string & port, bool verbose = false, bool tcp_nodelay = true, - std::unique_ptr model = std::make_unique()); + std::unique_ptr model = std::make_unique(), + const std::optional & serial_port_path = std::nullopt); /// @brief Destruct axiomatic socketcan bridge ~AxiomaticSocketcanBridge(); - /// @brief configures and opens the TCP and CAN sockets for the bridge - /// @return success for opening both sockets + /// @brief configures and opens the TCP and CAN sockets, and the serial port when a path was given + /// @return success for opening all of them; false when a serial port is configured for a model without raw data bool on_configure(); /// @brief activates and starts the reception threads for the socketcan and axiomatic adapters @@ -63,18 +70,23 @@ class AxiomaticSocketcanBridge /// @return success for joining bpth threads bool on_deactivate(); - /// @brief Shuts down and closes the TCP and CAN sockets + /// @brief Shuts down and closes the TCP and CAN sockets, and the serial port /// @return success for closing both sockets bool on_shutdown(); private: polymath::socketcan::SocketcanAdapter socketcan_adapter_; polymath::can::AxiomaticAdapter axiomatic_adapter_; + std::optional serial_port_path_; + // open from on_configure until on_shutdown + std::unique_ptr serial_port_; bool verbose_; void socketcanReceiveCallback(std::unique_ptr frame); void ethcanReceiveCallback(std::unique_ptr frame); + void serialPortReceiveCallback(std::vector data); + void ethRawDataReceiveCallback(std::vector data); }; } // namespace polymath::can diff --git a/axiomatic_adapter/include/axiomatic_adapter/models/ax142100a.hpp b/axiomatic_adapter/include/axiomatic_adapter/models/ax142100a.hpp index 8392895..1aa0e1b 100644 --- a/axiomatic_adapter/include/axiomatic_adapter/models/ax142100a.hpp +++ b/axiomatic_adapter/include/axiomatic_adapter/models/ax142100a.hpp @@ -16,6 +16,7 @@ #define AXIOMATIC_ADAPTER__MODELS__AX142100A_HPP_ #include +#include #include #include "axiomatic_adapter/axiomatic_model.hpp" @@ -31,6 +32,10 @@ namespace polymath::can::ax142100a /// @brief Protocol ID 20008, 0x28 0x4E on the wire static constexpr uint16_t PROTOCOL_ID = 0x4E28; +/// @brief Largest raw data payload one message can carry; the status byte takes one byte of the 16-bit body length +/// TODO: (David Tarazi) Confirm the device's own limit on hardware; the UMAX142100A example client uses a 256-byte buffer. +static constexpr size_t MAX_RAW_DATA_BYTES = 0xFFFF - 1; + /// @brief Message IDs (UMAX142100A section 4.2) enum class MessageId : uint16_t { @@ -39,13 +44,16 @@ enum class MessageId : uint16_t }; /// @class polymath::can::ax142100a::Ax142100a -/// @brief Encodes a Forwarded Data message. -/// Decodes Forwarded Data messages; raw data payloads are skipped. +/// @brief Encodes and decodes Forwarded Data messages carrying a CAN frame or raw data. +/// The device's data routing configuration selects which serial port raw data is forwarded to and from. class Ax142100a : public AxiomaticModel { public: std::vector encode(const polymath::socketcan::CanFrame & frame) const override; + /// @throws std::length_error when data exceeds MAX_RAW_DATA_BYTES + std::optional> encodeRawData(const std::vector & data) const override; + protected: uint16_t protocolId() const override; bool decodeMessage(const protocol::MessageView & message, protocol::DecodeResult & result) const override; diff --git a/axiomatic_adapter/include/axiomatic_adapter/serial_port.hpp b/axiomatic_adapter/include/axiomatic_adapter/serial_port.hpp new file mode 100644 index 0000000..4e3b2d5 --- /dev/null +++ b/axiomatic_adapter/include/axiomatic_adapter/serial_port.hpp @@ -0,0 +1,88 @@ +// 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__SERIAL_PORT_HPP_ +#define AXIOMATIC_ADAPTER__SERIAL_PORT_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace polymath::can +{ + +/// @class polymath::can::SerialPort +/// @brief An existing serial device, e.g. one end of a virtual serial port pair, open in raw mode for the object's lifetime. +/// send() may run concurrently with the reception thread. +class SerialPort +{ +public: + using error_string_t = std::string; + + static constexpr std::chrono::milliseconds RECEIVE_POLL_TIMEOUT_MS{100}; + static constexpr std::chrono::milliseconds JOIN_RECEPTION_TIMEOUT_MS{500}; + + /// @brief Open device_path and set it to raw mode + /// @param device_path serial device to open, e.g. /dev/ttyAX0 + /// @throws std::system_error when the device is missing or not a terminal + explicit SerialPort(const std::string & device_path); + + /// @brief Joins the reception thread and closes the device + ~SerialPort(); + + SerialPort(const SerialPort &) = delete; + SerialPort & operator=(const SerialPort &) = delete; + + /// @brief Set the callback for each chunk of bytes read from the device + /// @return false while the reception thread is running + bool setOnReceiveCallback(std::function data)> && callback_function); + + /// @brief Set the callback for read errors and hang-ups; a repeated error is reported once until a read succeeds + /// @return false while the reception thread is running + bool setOnErrorCallback(std::function && callback_function); + + /// @brief Start the thread that calls the receive and error callbacks + /// @return false when already running + bool startReceptionThread(); + + /// @brief Stop and join the reception thread + /// @return success on joined thread within timeout + bool joinReceptionThread(const std::chrono::milliseconds & timeout = JOIN_RECEPTION_TIMEOUT_MS); + + /// @brief Write bytes to the device + /// @return error message; bytes not yet written are dropped, e.g. when nothing drains a full buffer + std::optional send(const std::vector & data); + + const std::string & devicePath() const; + +private: + std::string device_path_; + std::function data)> receive_callback_; + std::function error_callback_; + + int fd_; + + std::thread reception_thread_; + std::atomic thread_running_{false}; + std::atomic stop_thread_requested_{false}; +}; + +} // namespace polymath::can + +#endif // AXIOMATIC_ADAPTER__SERIAL_PORT_HPP_ diff --git a/axiomatic_adapter/src/axiomatic_adapter.cpp b/axiomatic_adapter/src/axiomatic_adapter.cpp index 1a5e381..79b5298 100644 --- a/axiomatic_adapter/src/axiomatic_adapter.cpp +++ b/axiomatic_adapter/src/axiomatic_adapter.cpp @@ -146,6 +146,7 @@ class AxiomaticAdapter::AxiomaticAdapterImpl bool closeSocket() { + rx_carry_.clear(); if (socket_state_ != TCPSocketState::CLOSED) { boost::system::error_code error_code; tcp_socket_.close(error_code); @@ -172,14 +173,14 @@ class AxiomaticAdapter::AxiomaticAdapterImpl tcp_receive_thread_ = std::thread([this]() { while (!stop_thread_requested_) { - polymath::socketcan::CanFrame frame = polymath::socketcan::CanFrame(); - std::optional error = receive(frame); - - if (!error) { - receive_callback_(std::make_unique(frame)); - } else { + std::optional error = readAndDecode(); + if (error) { error_callback_(*error); } + while (!pending_frames_.empty()) { + receive_callback_(std::make_unique(pending_frames_.front())); + pending_frames_.pop_front(); + } } thread_running_ = false; @@ -203,17 +204,81 @@ class AxiomaticAdapter::AxiomaticAdapterImpl return false; } + bool setOnRawDataCallback(std::function data)> && callback_function) + { + if (thread_running_) { + return false; + } + raw_data_callback_ = std::move(callback_function); + return true; + } + std::optional receive(polymath::socketcan::CanFrame & can_frame) { // A previous TCP read may have decoded several CAN frames out of // packed protocol messages — deliver those one at a time before doing // another network read, so packed frames don't get silently dropped - if (!pending_frames_.empty()) { - can_frame = pending_frames_.front(); - pending_frames_.pop_front(); - return std::nullopt; + if (pending_frames_.empty()) { + std::optional error = readAndDecode(); + if (error) { + return error; + } + } + if (pending_frames_.empty()) { + return std::make_optional("No CAN frames in received protocol traffic."); } + can_frame = pending_frames_.front(); + pending_frames_.pop_front(); + return std::nullopt; + } + + std::optional receive() + { + polymath::socketcan::CanFrame can_frame = polymath::socketcan::CanFrame(); + auto result = receive(can_frame); + return !result ? std::optional(can_frame) : std::nullopt; + } + std::optional send(const polymath::socketcan::CanFrame & frame) + { + return write(model_->encode(frame)); + } + + std::optional sendRawData(const std::vector & data) + { + std::optional> message; + try { + message = model_->encodeRawData(data); + } catch (const std::length_error & e) { + return std::optional(e.what()); + } + if (!message) { + return std::optional("Model has no raw data channel"); + } + return write(*message); + } + + bool supportsRawData() const + { + return model_->supportsRawData(); + } + + TCPSocketState get_socket_state() + { + return socket_state_; + } + + bool is_thread_running() + { + return thread_running_; + } + +private: + /// @brief Read once from the socket, queue decoded CAN frames, and pass raw data to the raw data callback. + /// A trailing incomplete message is kept and completed by the next read. + /// @return error on timeout or socket failure, or the last diagnostic when the read decoded nothing + std::optional readAndDecode() + { size_t bytes_received = 0; std::atomic data_received(false); boost::system::error_code error_code; @@ -255,54 +320,42 @@ class AxiomaticAdapter::AxiomaticAdapterImpl "Receive operation failed: " + error_code.message()); } - // NOTE: given the axiomatic documentation claims a deliberate 256-byte design + the large buffer size used in - // rx_buffer_, a mid-tcp-message split is never supposed to occur. In testing this drop has never happened. - // it is possible that with non-standard very low MTU's or in future revisions, this assumption no longer holds - protocol::DecodeResult decoded = model_->decode(rx_buffer_.data(), bytes_received); + const uint8_t * data = rx_buffer_.data(); + size_t size = bytes_received; + if (!rx_carry_.empty()) { + rx_carry_.insert(rx_carry_.end(), rx_buffer_.begin(), rx_buffer_.begin() + bytes_received); + data = rx_carry_.data(); + size = rx_carry_.size(); + } + protocol::DecodeResult decoded = model_->decode(data, size); + rx_carry_ = std::vector(data + decoded.consumed, data + size); + for (const auto & diagnostic : decoded.diagnostics) { std::cerr << "[Axiomatic parser] " << diagnostic << std::endl; } - if (decoded.frames.empty()) { - return std::make_optional( - decoded.diagnostics.empty() ? "No CAN frames in received protocol traffic." : decoded.diagnostics.back()); - } - pending_frames_.insert(pending_frames_.end(), decoded.frames.begin(), decoded.frames.end()); - can_frame = pending_frames_.front(); - pending_frames_.pop_front(); + if (raw_data_callback_) { + for (auto & raw_data : decoded.raw_data) { + raw_data_callback_(std::move(raw_data)); + } + } + if (decoded.frames.empty() && decoded.raw_data.empty() && !decoded.diagnostics.empty()) { + return std::make_optional(decoded.diagnostics.back()); + } return std::nullopt; } - std::optional receive() + std::optional write(const std::vector & message) { - polymath::socketcan::CanFrame can_frame = polymath::socketcan::CanFrame(); - auto result = receive(can_frame); - return !result ? std::optional(can_frame) : std::nullopt; - } - - std::optional send(const polymath::socketcan::CanFrame & frame) - { - const std::vector full_message = model_->encode(frame); - + std::lock_guard guard(write_mutex_); try { - boost::asio::write(tcp_socket_, boost::asio::buffer(full_message.data(), full_message.size())); + boost::asio::write(tcp_socket_, boost::asio::buffer(message.data(), message.size())); } catch (const std::exception & e) { return std::optional(std::string("TCP Send Failed: ") + e.what()); } return std::nullopt; } - TCPSocketState get_socket_state() - { - return socket_state_; - } - - bool is_thread_running() - { - return thread_running_; - } - -private: static constexpr std::chrono::milliseconds TCP_IP_CONNECTION_TIMEOUT_MS{3000}; // receive buffer size for each async_receive call. Larger than the @@ -321,8 +374,11 @@ class AxiomaticAdapter::AxiomaticAdapterImpl TCPSocketState socket_state_{TCPSocketState::CLOSED}; std::thread tcp_receive_thread_; - std::atomic thread_running_; - std::atomic stop_thread_requested_; + std::atomic thread_running_{false}; + std::atomic stop_thread_requested_{false}; + + // serializes socket writes from send() and sendRawData() + std::mutex write_mutex_; // CAN frames decoded from one TCP read but not yet delivered // through receive(). Drained one at a time, ahead of the next TCP read. @@ -338,6 +394,11 @@ class AxiomaticAdapter::AxiomaticAdapterImpl // receive buffer for async_receive — allocated once at construction and reused across every receive() call std::vector rx_buffer_; + // bytes of an incomplete protocol message from the previous read + std::vector rx_carry_; + + std::function data)> raw_data_callback_; + // when true, disable Nagle's algorithm on the TCP socket after connect bool tcp_nodelay_; @@ -409,6 +470,21 @@ std::optional AxiomaticAdapter::send(co return send(polymath::socketcan::CanFrame(frame)); } +bool AxiomaticAdapter::setOnRawDataCallback(std::function data)> && callback_function) +{ + return pimpl_->setOnRawDataCallback(std::move(callback_function)); +} + +std::optional AxiomaticAdapter::sendRawData(const std::vector & data) +{ + return pimpl_->sendRawData(data); +} + +bool AxiomaticAdapter::supportsRawData() const +{ + return pimpl_->supportsRawData(); +} + TCPSocketState AxiomaticAdapter::get_socket_state() { return pimpl_->get_socket_state(); diff --git a/axiomatic_adapter/src/axiomatic_model.cpp b/axiomatic_adapter/src/axiomatic_model.cpp index b36abfe..f60b504 100644 --- a/axiomatic_adapter/src/axiomatic_model.cpp +++ b/axiomatic_adapter/src/axiomatic_model.cpp @@ -15,6 +15,7 @@ #include "axiomatic_adapter/axiomatic_model.hpp" #include +#include #include #include #include @@ -26,15 +27,27 @@ namespace polymath::can { +std::optional> AxiomaticModel::encodeRawData(const std::vector & /*data*/) const +{ + return std::nullopt; +} + +bool AxiomaticModel::supportsRawData() const +{ + return encodeRawData({}).has_value(); +} + protocol::DecodeResult AxiomaticModel::decode(const uint8_t * data, size_t size) const { protocol::ParsedMessages parsed = protocol::parseMessages(protocolId(), data, size); - protocol::DecodeResult result{{}, std::move(parsed.diagnostics)}; + protocol::DecodeResult result; + result.diagnostics = std::move(parsed.diagnostics); + result.consumed = parsed.consumed; for (const auto & message : parsed.messages) { if (!decodeMessage(message, result)) { result.diagnostics.push_back( "SKIP: Message ID " + std::to_string(message.message_id) + " (" + std::to_string(message.body_size) + - "-byte body) carries no CAN frames"); + "-byte body) carries no CAN frames or raw data"); } } return result; diff --git a/axiomatic_adapter/src/axiomatic_protocol.cpp b/axiomatic_adapter/src/axiomatic_protocol.cpp index 681c8ec..fd605f1 100644 --- a/axiomatic_adapter/src/axiomatic_protocol.cpp +++ b/axiomatic_adapter/src/axiomatic_protocol.cpp @@ -62,14 +62,8 @@ std::vector encodeMessage( ParsedMessages parseMessages(uint16_t protocol_id, const uint8_t * data, size_t size) { ParsedMessages parsed; - if (size < HEADER_BYTES) { - parsed.diagnostics.push_back( - "DROP: received " + std::to_string(size) + " bytes, too short to contain a complete protocol header"); - return parsed; - } - - size_t scan_pos = 0; - while (scan_pos + HEADER_BYTES <= size) { + while (parsed.consumed + HEADER_BYTES <= size) { + const size_t scan_pos = parsed.consumed; const uint8_t * header = data + scan_pos; const bool tag_match = std::equal(AXIOMATIC_TAG.begin(), AXIOMATIC_TAG.end(), header); const uint16_t received_protocol_id = @@ -77,7 +71,7 @@ ParsedMessages parseMessages(uint16_t protocol_id, const uint8_t * data, size_t if (!tag_match || protocol_id != received_protocol_id) { std::ostringstream message; message << "DROP: sync prefix mismatch at offset " << scan_pos << " of " << size - << "-byte read; bytes there:" << std::hex; + << "-byte buffer; bytes there:" << std::hex; for (size_t i = 0; i < PROTOCOL_ID_OFFSET + PROTOCOL_ID_BYTES; ++i) { message << ' ' << static_cast(header[i]); } @@ -87,28 +81,21 @@ ParsedMessages parseMessages(uint16_t protocol_id, const uint8_t * data, size_t message << "; the device speaks a different Protocol ID, so the wrong --model may be selected"; } parsed.diagnostics.push_back(message.str()); + parsed.consumed = size; return parsed; } const size_t declared_length = readLittleEndian(header + MESSAGE_DATA_LENGTH_OFFSET, MESSAGE_DATA_LENGTH_BYTES); const size_t body_start = scan_pos + HEADER_BYTES; - const size_t body_end = std::min(body_start + declared_length, size); if (body_start + declared_length > size) { - parsed.diagnostics.push_back( - "TRUNCATED: message at offset " + std::to_string(scan_pos) + " declares " + std::to_string(declared_length) + - " body bytes but only " + std::to_string(body_end - body_start) + " were received"); + break; } parsed.messages.push_back(MessageView{ static_cast(readLittleEndian(header + MESSAGE_ID_OFFSET, MESSAGE_ID_BYTES)), header[MESSAGE_VERSION_OFFSET], data + body_start, - body_end - body_start}); - scan_pos = body_start + declared_length; - } - - if (scan_pos < size) { - parsed.diagnostics.push_back( - "DROP: " + std::to_string(size - scan_pos) + " trailing bytes too short to contain a protocol header"); + declared_length}); + parsed.consumed = body_start + declared_length; } return parsed; } diff --git a/axiomatic_adapter/src/axiomatic_socketcan_bridge.cpp b/axiomatic_adapter/src/axiomatic_socketcan_bridge.cpp index f7198ac..ab3a9b3 100644 --- a/axiomatic_adapter/src/axiomatic_socketcan_bridge.cpp +++ b/axiomatic_adapter/src/axiomatic_socketcan_bridge.cpp @@ -14,21 +14,39 @@ #include "axiomatic_adapter/axiomatic_socketcan_bridge.hpp" +#include #include #include +#include #include +#include #include +#include namespace polymath::can { +namespace +{ +std::string hexBytes(const std::vector & data) +{ + std::ostringstream stream; + stream << std::hex << std::setfill('0'); + for (const uint8_t byte : data) { + stream << ' ' << std::setw(2) << static_cast(byte); + } + return stream.str(); +} +} // namespace + AxiomaticSocketcanBridge::AxiomaticSocketcanBridge( const std::string & can_interface_name, const std::string & ip, const std::string & port, bool verbose, bool tcp_nodelay, - std::unique_ptr model) + std::unique_ptr model, + const std::optional & serial_port_path) : socketcan_adapter_(can_interface_name) , axiomatic_adapter_( ip, @@ -38,10 +56,15 @@ AxiomaticSocketcanBridge::AxiomaticSocketcanBridge( AxiomaticAdapter::DEFAULT_SOCKET_RECEIVE_TIMEOUT_MS, tcp_nodelay, std::move(model)) +, serial_port_path_(serial_port_path) , verbose_(verbose) { socketcan_adapter_.setOnReceiveCallback( std::bind(&AxiomaticSocketcanBridge::socketcanReceiveCallback, this, std::placeholders::_1)); + if (serial_port_path_) { + axiomatic_adapter_.setOnRawDataCallback( + std::bind(&AxiomaticSocketcanBridge::ethRawDataReceiveCallback, this, std::placeholders::_1)); + } } AxiomaticSocketcanBridge::~AxiomaticSocketcanBridge() @@ -52,6 +75,10 @@ AxiomaticSocketcanBridge::~AxiomaticSocketcanBridge() bool AxiomaticSocketcanBridge::on_configure() { + if (serial_port_path_ && !axiomatic_adapter_.supportsRawData()) { + std::cout << "Axiomatic model has no raw data channel for a serial port" << std::endl; + return false; + } // open sockets if (!socketcan_adapter_.openSocket()) { std::cout << "Socketcan Adapter can't open socket..." << std::endl; @@ -61,6 +88,19 @@ bool AxiomaticSocketcanBridge::on_configure() std::cout << "Axiomatic Adapter can't open socket..." << std::endl; return false; } + if (serial_port_path_ && !serial_port_) { + try { + serial_port_ = std::make_unique(*serial_port_path_); + } catch (const std::system_error & e) { + std::cout << "Serial port can't open: " << e.what() << std::endl; + return false; + } + serial_port_->setOnReceiveCallback( + std::bind(&AxiomaticSocketcanBridge::serialPortReceiveCallback, this, std::placeholders::_1)); + serial_port_->setOnErrorCallback( + [](SerialPort::error_string_t error) { std::cerr << "[Serial RX] Error: " << error << std::endl; }); + std::cout << "Serial port " << serial_port_->devicePath() << " opened" << std::endl; + } return true; } @@ -72,6 +112,9 @@ bool AxiomaticSocketcanBridge::on_activate() if (!axiomatic_adapter_.startReceptionThread()) { return false; } + if (serial_port_ && !serial_port_->startReceptionThread()) { + return false; + } return true; } @@ -84,6 +127,9 @@ bool AxiomaticSocketcanBridge::on_deactivate() if (!axiomatic_adapter_.joinReceptionThread()) { success = false; } + if (serial_port_ && !serial_port_->joinReceptionThread()) { + success = false; + } return success; } @@ -96,6 +142,7 @@ bool AxiomaticSocketcanBridge::on_shutdown() if (!axiomatic_adapter_.closeSocket()) { success = false; } + serial_port_.reset(); return success; } @@ -145,4 +192,31 @@ void AxiomaticSocketcanBridge::ethcanReceiveCallback(std::unique_ptr data) +{ + if (verbose_) { + std::cout << "[Serial RX] Received " << std::dec << data.size() << " bytes: [" << hexBytes(data) << " ]" + << std::endl; + } + auto result = axiomatic_adapter_.sendRawData(data); + if (result) { + std::cerr << "[Serial RX] Failed to send raw data with message: " << *result << std::endl; + } +} + +void AxiomaticSocketcanBridge::ethRawDataReceiveCallback(std::vector data) +{ + if (verbose_) { + std::cout << "[EthSerial RX] Received " << std::dec << data.size() << " bytes: [" << hexBytes(data) << " ]" + << std::endl; + } + if (!serial_port_) { + return; + } + auto result = serial_port_->send(data); + if (result) { + std::cerr << "[EthSerial RX] Failed to write raw data with message: " << *result << std::endl; + } +} + } // namespace polymath::can diff --git a/axiomatic_adapter/src/axiomatic_socketcan_bridge_node.cpp b/axiomatic_adapter/src/axiomatic_socketcan_bridge_node.cpp index fdabc06..37e364a 100644 --- a/axiomatic_adapter/src/axiomatic_socketcan_bridge_node.cpp +++ b/axiomatic_adapter/src/axiomatic_socketcan_bridge_node.cpp @@ -16,8 +16,10 @@ #include #include #include +#include #include #include +#include #include @@ -50,7 +52,8 @@ void configureArguments( bool & retry_connection, int & max_retry_attempts, bool & tcp_nodelay, - std::string & model) + std::string & model, + std::string & serial_port) { app.add_option("can_interface", can_interface, "CAN interface to use (default: vcan0)")->default_val("vcan0"); app.add_option("ip", ip, "IP address of the bridge (default: 192.168.0.34)")->default_val("192.168.0.34"); @@ -67,6 +70,10 @@ void configureArguments( app.add_option("-m,--model", model, "Axiomatic converter model (default: ax140900)") ->transform(CLI::IsMember(polymath::can::modelNames(), CLI::ignore_case)) ->default_val("ax140900"); + app.add_option( + "--serial-port", + serial_port, + "Existing serial device bridged to the converter's serial data, e.g. one end of a virtual pair; ax142100a only"); } int main(int argc, char * argv[]) @@ -76,12 +83,26 @@ int main(int argc, char * argv[]) std::string can_interface, ip, port; bool tcp_nodelay = true; std::string model; + std::string serial_port; CLI::App app{"Axiomatic SocketCAN Bridge"}; - configureArguments(app, can_interface, ip, port, verbose, retry_connection, max_retry_attempts, tcp_nodelay, model); + configureArguments( + app, can_interface, ip, port, verbose, retry_connection, max_retry_attempts, tcp_nodelay, model, serial_port); CLI11_PARSE(app, argc, argv); + auto axiomatic_model = polymath::can::makeModel(model); + if (!serial_port.empty() && !axiomatic_model->supportsRawData()) { + std::cerr << "--serial-port needs a model with a raw data channel; " << model << " has none." << std::endl; + return 1; + } + polymath::can::AxiomaticSocketcanBridge bridge( - can_interface, ip, port, verbose, tcp_nodelay, polymath::can::makeModel(model)); + can_interface, + ip, + port, + verbose, + tcp_nodelay, + std::move(axiomatic_model), + serial_port.empty() ? std::nullopt : std::make_optional(serial_port)); std::cout << "Axiomatic Socketcan Bridge configuring with model " << model << "..." << std::endl; diff --git a/axiomatic_adapter/src/models/ax142100a.cpp b/axiomatic_adapter/src/models/ax142100a.cpp index 7091934..7d80b62 100644 --- a/axiomatic_adapter/src/models/ax142100a.cpp +++ b/axiomatic_adapter/src/models/ax142100a.cpp @@ -16,6 +16,8 @@ #include #include +#include +#include #include #include @@ -31,24 +33,22 @@ constexpr size_t STANDARD_CAN_ID_BYTES = 2; constexpr size_t EXTENDED_CAN_ID_BYTES = 4; constexpr size_t STATUS_BYTE_BYTES = 1; -// Status byte, first byte of the Forwarded Data payload: -// bit 6 : raw data — the rest of the payload is unformatted serial/Ethernet data +// Status byte, first byte of the Forwarded Data payload (UMAX142100A section 4.2): +// bit 6 : raw (serial) data — the rest of the payload is forwarded as is; clear means CAN data // bit 4 : 0 = standard 11-bit ID, 1 = extended 29-bit ID // bits 3:0 : CAN data length constexpr uint8_t STATUS_BYTE_RAW_DATA_FLAG = 0x40; constexpr uint8_t STATUS_BYTE_EXTENDED_ID_FLAG = 0x10; constexpr uint8_t STATUS_BYTE_CAN_DATA_LENGTH_MASK = 0x0F; -/// Appends every CAN frame in one Forwarded Data message body to result +/// Appends every CAN frame and raw data payload in one Forwarded Data message body to result void decodeForwardedData(const uint8_t * body, size_t body_size, protocol::DecodeResult & result) { size_t walker = 0; while (walker + STATUS_BYTE_BYTES <= body_size) { const uint8_t status_byte = body[walker]; if (0 != (status_byte & STATUS_BYTE_RAW_DATA_FLAG)) { - result.diagnostics.push_back( - "SKIP: raw data payload of " + std::to_string(body_size - walker) + " bytes at body offset " + - std::to_string(walker)); + result.raw_data.emplace_back(body + walker + STATUS_BYTE_BYTES, body + body_size); return; } const bool extended_id = 0 != (status_byte & STATUS_BYTE_EXTENDED_ID_FLAG); @@ -99,6 +99,19 @@ std::vector Ax142100a::encode(const polymath::socketcan::CanFrame & fra return protocol::encodeMessage(PROTOCOL_ID, static_cast(MessageId::ForwardedData), MESSAGE_VERSION, body); } +std::optional> Ax142100a::encodeRawData(const std::vector & data) const +{ + if (data.size() > MAX_RAW_DATA_BYTES) { + throw std::length_error( + "AX142100A raw data of " + std::to_string(data.size()) + " bytes exceeds " + std::to_string(MAX_RAW_DATA_BYTES)); + } + std::vector body; + body.reserve(STATUS_BYTE_BYTES + data.size()); + body.push_back(STATUS_BYTE_RAW_DATA_FLAG); + body.insert(body.end(), data.begin(), data.end()); + return protocol::encodeMessage(PROTOCOL_ID, static_cast(MessageId::ForwardedData), MESSAGE_VERSION, body); +} + uint16_t Ax142100a::protocolId() const { return PROTOCOL_ID; diff --git a/axiomatic_adapter/src/serial_port.cpp b/axiomatic_adapter/src/serial_port.cpp new file mode 100644 index 0000000..bb5c5f0 --- /dev/null +++ b/axiomatic_adapter/src/serial_port.cpp @@ -0,0 +1,165 @@ +// 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/serial_port.hpp" + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +namespace polymath::can +{ + +namespace +{ +constexpr size_t RECEIVE_BUFFER_SIZE = 4096; + +std::string errnoMessage(const std::string & call) +{ + return call + " failed: " + std::strerror(errno); +} + +/// Open device_path in raw mode, nonblocking +/// @throws std::system_error on failure +int openRaw(const std::string & device_path) +{ + const int fd = ::open(device_path.c_str(), O_RDWR | O_NOCTTY | O_NONBLOCK); + if (0 > fd) { + throw std::system_error(errno, std::generic_category(), "open " + device_path); + } + struct termios attributes; + if (0 != ::tcgetattr(fd, &attributes)) { + const int error = errno; + ::close(fd); + throw std::system_error(error, std::generic_category(), "tcgetattr " + device_path); + } + ::cfmakeraw(&attributes); + if (0 != ::tcsetattr(fd, TCSANOW, &attributes)) { + const int error = errno; + ::close(fd); + throw std::system_error(error, std::generic_category(), "tcsetattr " + device_path); + } + return fd; +} +} // namespace + +SerialPort::SerialPort(const std::string & device_path) +: device_path_(device_path) +, receive_callback_([](std::vector /*data*/) { /*do nothing*/ }) +, error_callback_([](error_string_t /*error*/) { /*do nothing*/ }) +, fd_(openRaw(device_path)) +{} + +SerialPort::~SerialPort() +{ + joinReceptionThread(); + ::close(fd_); +} + +bool SerialPort::setOnReceiveCallback(std::function data)> && callback_function) +{ + if (thread_running_) { + return false; + } + receive_callback_ = std::move(callback_function); + return true; +} + +bool SerialPort::setOnErrorCallback(std::function && callback_function) +{ + if (thread_running_) { + return false; + } + error_callback_ = std::move(callback_function); + return true; +} + +bool SerialPort::startReceptionThread() +{ + if (reception_thread_.joinable()) { + return false; + } + stop_thread_requested_ = false; + thread_running_ = true; + reception_thread_ = std::thread([this]() { + std::vector buffer(RECEIVE_BUFFER_SIZE); + std::optional last_error; + while (!stop_thread_requested_) { + struct pollfd poll_fd = {fd_, POLLIN, 0}; + if (0 >= ::poll(&poll_fd, 1, static_cast(RECEIVE_POLL_TIMEOUT_MS.count()))) { + continue; + } + const ssize_t bytes_read = ::read(fd_, buffer.data(), buffer.size()); + if (0 < bytes_read) { + last_error.reset(); + receive_callback_(std::vector(buffer.begin(), buffer.begin() + bytes_read)); + continue; + } + if (0 > bytes_read && (EAGAIN == errno || EINTR == errno)) { + continue; + } + const error_string_t error = 0 == bytes_read ? device_path_ + " hung up" : errnoMessage("read " + device_path_); + if (error != last_error) { + error_callback_(error); + last_error = error; + } + // A hung-up device (e.g. the other end of a virtual pair closed) polls ready forever. + std::this_thread::sleep_for(RECEIVE_POLL_TIMEOUT_MS); + } + thread_running_ = false; + }); + return true; +} + +bool SerialPort::joinReceptionThread(const std::chrono::milliseconds & timeout) +{ + stop_thread_requested_ = true; + if (!reception_thread_.joinable()) { + return false; + } + std::future join_future = std::async(std::launch::async, [this] { reception_thread_.join(); }); + return join_future.wait_for(timeout) == std::future_status::ready; +} + +std::optional SerialPort::send(const std::vector & data) +{ + size_t written = 0; + while (written < data.size()) { + const ssize_t result = ::write(fd_, data.data() + written, data.size() - written); + if (0 > result) { + if (EINTR == errno) { + continue; + } + return errnoMessage("write " + device_path_) + "; dropped " + std::to_string(data.size() - written) + " bytes"; + } + written += static_cast(result); + } + return std::nullopt; +} + +const std::string & SerialPort::devicePath() const +{ + return device_path_; +} + +} // namespace polymath::can diff --git a/axiomatic_adapter/test/axiomatic_adapter_loopback_test.cpp b/axiomatic_adapter/test/axiomatic_adapter_loopback_test.cpp new file mode 100644 index 0000000..cf5f01a --- /dev/null +++ b/axiomatic_adapter/test/axiomatic_adapter_loopback_test.cpp @@ -0,0 +1,177 @@ +// 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 +#include +#include +#include +#include +#include +#include +#include + +#include + +#if __has_include() + #include // v3 +#else + #include // v2 +#endif +#include "axiomatic_adapter/axiomatic_adapter.hpp" +#include "axiomatic_adapter/models/ax140900.hpp" +#include "axiomatic_adapter/models/ax142100a.hpp" + +using boost::asio::ip::tcp; +using polymath::can::AxiomaticAdapter; +using polymath::socketcan::CanFrame; + +namespace +{ + +constexpr std::chrono::seconds RECEIVE_WAIT{2}; +constexpr std::chrono::milliseconds SPLIT_WRITE_GAP{20}; + +// UMAX142100A Figure 3: ID 0x18F00401, 29-bit, 8 data bytes +const std::vector AX142100A_MANUAL_EXAMPLE = { + 0x41, 0x58, 0x49, 0x4F, 0x28, 0x4E, 0x01, 0x00, 0x00, 0x0D, 0x00, 0x18, + 0x01, 0x04, 0xF0, 0x18, 0x12, 0x44, 0x21, 0x55, 0x61, 0x09, 0x01, 0x02, +}; + +/// Local TCP server standing in for the converter +class FakeConverter +{ +public: + FakeConverter() + : acceptor_(io_context_, tcp::endpoint(boost::asio::ip::make_address("127.0.0.1"), 0)) + , socket_(io_context_) + {} + + std::string port() const + { + return std::to_string(acceptor_.local_endpoint().port()); + } + + /// Call after the adapter's openSocket() + void accept() + { + acceptor_.accept(socket_); + } + + void write(const std::vector & data) + { + boost::asio::write(socket_, boost::asio::buffer(data)); + } + + std::vector read(size_t size) + { + std::vector data(size); + boost::asio::read(socket_, boost::asio::buffer(data)); + return data; + } + +private: + boost::asio::io_context io_context_; + tcp::acceptor acceptor_; + tcp::socket socket_; +}; + +} // namespace + +TEST_CASE("AxiomaticAdapter delivers raw data and CAN frames split across TCP reads", "[adapter][ax142100a]") +{ + FakeConverter converter; + + std::promise frame_promise; + std::promise> raw_data_promise; + std::mutex errors_mutex; + std::vector errors; + + AxiomaticAdapter adapter( + "127.0.0.1", + converter.port(), + [&](std::unique_ptr frame) { frame_promise.set_value(*frame); }, + [&](AxiomaticAdapter::socket_error_string_t error) { + std::lock_guard guard(errors_mutex); + errors.push_back(error); + }, + AxiomaticAdapter::DEFAULT_SOCKET_RECEIVE_TIMEOUT_MS, + true, + std::make_unique()); + REQUIRE(adapter.setOnRawDataCallback([&](std::vector data) { raw_data_promise.set_value(data); })); + + REQUIRE(adapter.openSocket()); + converter.accept(); + REQUIRE(adapter.startReceptionThread()); + REQUIRE_FALSE(adapter.setOnRawDataCallback([](std::vector /*data*/) {})); + + const std::vector raw_data = {'$', 'G', 'S', ',', '1', '\r', '\n'}; + std::vector stream = *polymath::can::ax142100a::Ax142100a().encodeRawData(raw_data); + stream.insert(stream.end(), AX142100A_MANUAL_EXAMPLE.begin(), AX142100A_MANUAL_EXAMPLE.end()); + const size_t split = stream.size() - 5; + converter.write(std::vector(stream.begin(), stream.begin() + split)); + std::this_thread::sleep_for(SPLIT_WRITE_GAP); + converter.write(std::vector(stream.begin() + split, stream.end())); + + auto raw_data_future = raw_data_promise.get_future(); + REQUIRE(std::future_status::ready == raw_data_future.wait_for(RECEIVE_WAIT)); + REQUIRE(raw_data == raw_data_future.get()); + + auto frame_future = frame_promise.get_future(); + REQUIRE(std::future_status::ready == frame_future.wait_for(RECEIVE_WAIT)); + const CanFrame frame = frame_future.get(); + REQUIRE(0x18F00401 == frame.get_id()); + REQUIRE(8 == frame.get_len()); + + REQUIRE(adapter.joinReceptionThread()); + std::lock_guard guard(errors_mutex); + for (const auto & error : errors) { + INFO(error); + REQUIRE("Receive operation timed out" == error); + } +} + +TEST_CASE("AxiomaticAdapter sends raw data as one protocol message", "[adapter][ax142100a]") +{ + FakeConverter converter; + AxiomaticAdapter adapter( + "127.0.0.1", + converter.port(), + [](std::unique_ptr /*frame*/) {}, + [](AxiomaticAdapter::socket_error_string_t /*error*/) {}, + AxiomaticAdapter::DEFAULT_SOCKET_RECEIVE_TIMEOUT_MS, + true, + std::make_unique()); + REQUIRE(adapter.supportsRawData()); + REQUIRE(adapter.openSocket()); + converter.accept(); + + const std::vector raw_data = {'h', 'e', 'l', 'l', 'o'}; + const std::vector expected = *polymath::can::ax142100a::Ax142100a().encodeRawData(raw_data); + REQUIRE_FALSE(adapter.sendRawData(raw_data).has_value()); + REQUIRE(expected == converter.read(expected.size())); +} + +TEST_CASE("AxiomaticAdapter refuses raw data for a model without a raw data channel", "[adapter][ax140900]") +{ + AxiomaticAdapter adapter( + "127.0.0.1", + "4000", + [](std::unique_ptr /*frame*/) {}, + [](AxiomaticAdapter::socket_error_string_t /*error*/) {}, + AxiomaticAdapter::DEFAULT_SOCKET_RECEIVE_TIMEOUT_MS, + true, + std::make_unique()); + REQUIRE_FALSE(adapter.supportsRawData()); + REQUIRE(adapter.sendRawData({'a'}).has_value()); +} diff --git a/axiomatic_adapter/test/axiomatic_model_test.cpp b/axiomatic_adapter/test/axiomatic_model_test.cpp index 7ad96c1..28f64a0 100644 --- a/axiomatic_adapter/test/axiomatic_model_test.cpp +++ b/axiomatic_adapter/test/axiomatic_model_test.cpp @@ -72,6 +72,10 @@ const std::vector AX142100A_MANUAL_EXAMPLE = { }; const CanFrame AX142100A_MANUAL_FRAME = makeFrame(0x18F00401, true, {0x12, 0x44, 0x21, 0x55, 0x61, 0x09, 0x01, 0x02}); +// Forwarded Data, 4-byte body: raw data flag, then "abc" +const std::vector AX142100A_RAW_DATA_MESSAGE = { + 0x41, 0x58, 0x49, 0x4F, 0x28, 0x4E, 0x01, 0x00, 0x00, 0x04, 0x00, 0x40, 'a', 'b', 'c'}; + } // namespace TEST_CASE("AX142100A matches the manual example", "[model][ax142100a]") @@ -91,18 +95,58 @@ TEST_CASE("AX142100A matches the manual example", "[model][ax142100a]") } } -TEST_CASE("AX142100A skips raw data payloads", "[model][ax142100a]") +TEST_CASE("AX142100A decodes raw data payloads", "[model][ax142100a]") { - const std::vector raw = { - 0x41, 0x58, 0x49, 0x4F, 0x28, 0x4E, 0x01, 0x00, 0x00, 0x04, 0x00, 0x40, 'a', 'b', 'c'}; - const auto stream = concat(raw, AX142100A_MANUAL_EXAMPLE); + const auto stream = concat(AX142100A_RAW_DATA_MESSAGE, AX142100A_MANUAL_EXAMPLE); const auto result = polymath::can::ax142100a::Ax142100a().decode(stream.data(), stream.size()); - REQUIRE(1 == result.diagnostics.size()); + REQUIRE(result.diagnostics.empty()); + REQUIRE(stream.size() == result.consumed); + REQUIRE(std::vector>{{'a', 'b', 'c'}} == result.raw_data); REQUIRE(1 == result.frames.size()); requireSameFrame(result.frames[0], AX142100A_MANUAL_FRAME); } +TEST_CASE("AX142100A encodes raw data with the raw data flag", "[model][ax142100a]") +{ + const polymath::can::ax142100a::Ax142100a model; + REQUIRE(model.supportsRawData()); + REQUIRE(AX142100A_RAW_DATA_MESSAGE == model.encodeRawData({'a', 'b', 'c'})); + REQUIRE_THROWS_AS( + model.encodeRawData(std::vector(polymath::can::ax142100a::MAX_RAW_DATA_BYTES + 1)), std::length_error); +} + +TEST_CASE("AX140900 has no raw data channel", "[model][ax140900]") +{ + const polymath::can::ax140900::Ax140900 model; + REQUIRE_FALSE(model.supportsRawData()); + REQUIRE_FALSE(model.encodeRawData({'a'}).has_value()); +} + +TEST_CASE("Models reassemble a message split across buffers", "[model]") +{ + const auto stream = concat(AX142100A_MANUAL_EXAMPLE, AX142100A_RAW_DATA_MESSAGE); + const polymath::can::ax142100a::Ax142100a model; + + for (size_t split = 1; split < AX142100A_MANUAL_EXAMPLE.size(); ++split) { + INFO("split at " << split); + const auto partial = model.decode(stream.data(), split); + REQUIRE(partial.frames.empty()); + REQUIRE(partial.diagnostics.empty()); + REQUIRE(0 == partial.consumed); + } + + const auto first = model.decode(stream.data(), AX142100A_MANUAL_EXAMPLE.size() + 3); + REQUIRE(1 == first.frames.size()); + REQUIRE(first.raw_data.empty()); + REQUIRE(AX142100A_MANUAL_EXAMPLE.size() == first.consumed); + + const auto rest = model.decode(stream.data() + first.consumed, stream.size() - first.consumed); + REQUIRE(rest.diagnostics.empty()); + REQUIRE(stream.size() - first.consumed == rest.consumed); + REQUIRE(std::vector>{{'a', 'b', 'c'}} == rest.raw_data); +} + TEST_CASE("AX140900 encodes a CAN Stream message with all CAN_MAX_DLC data bytes", "[model][ax140900]") { const std::vector expected = { @@ -136,6 +180,24 @@ TEST_CASE("AX140900 decodes packed frames and skips non-CAN content", "[model][a REQUIRE(3 == result.diagnostics.size()); } +TEST_CASE("Models round trip raw data", "[model]") +{ + const std::vector data = {'$', 'G', 'S', ',', '1', '\r', '\n'}; + + for (const auto & name : polymath::can::modelNames()) { + INFO("model " << name); + const auto model = polymath::can::makeModel(name); + if (!model->supportsRawData()) { + continue; + } + const auto message = model->encodeRawData(data); + REQUIRE(message.has_value()); + const auto result = model->decode(message->data(), message->size()); + REQUIRE(result.frames.empty()); + REQUIRE(std::vector>{data} == result.raw_data); + } +} + TEST_CASE("Models round trip", "[model]") { const std::vector frames = { @@ -161,16 +223,18 @@ TEST_CASE("Models reject another model's Protocol ID", "[model]") const auto result = polymath::can::ax140900::Ax140900().decode(AX142100A_MANUAL_EXAMPLE.data(), AX142100A_MANUAL_EXAMPLE.size()); REQUIRE(result.frames.empty()); + REQUIRE(AX142100A_MANUAL_EXAMPLE.size() == result.consumed); REQUIRE(1 == result.diagnostics.size()); REQUIRE(std::string::npos != result.diagnostics[0].find("wrong --model")); } -TEST_CASE("Models drop buffers shorter than a header", "[model]") +TEST_CASE("Models leave buffers shorter than a header unconsumed", "[model]") { const std::vector partial(AX142100A_MANUAL_EXAMPLE.begin(), AX142100A_MANUAL_EXAMPLE.begin() + 5); const auto result = polymath::can::ax142100a::Ax142100a().decode(partial.data(), partial.size()); REQUIRE(result.frames.empty()); - REQUIRE(1 == result.diagnostics.size()); + REQUIRE(result.diagnostics.empty()); + REQUIRE(0 == result.consumed); } TEST_CASE("Model names map to their classes", "[model]") diff --git a/axiomatic_adapter/test/serial_port_test.cpp b/axiomatic_adapter/test/serial_port_test.cpp new file mode 100644 index 0000000..ee7cccd --- /dev/null +++ b/axiomatic_adapter/test/serial_port_test.cpp @@ -0,0 +1,177 @@ +// 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 +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if __has_include() + #include // v3 +#else + #include // v2 +#endif +#include "axiomatic_adapter/serial_port.hpp" + +using polymath::can::SerialPort; + +namespace +{ + +constexpr std::chrono::seconds RECEIVE_WAIT{2}; +constexpr int PEER_POLL_TIMEOUT_MS = 2000; +constexpr std::chrono::milliseconds HANG_UP_WAIT{500}; + +/// A pseudo-terminal pair: SerialPort opens device_path, the test plays the other end through peer_fd +class VirtualSerialPair +{ +public: + VirtualSerialPair() + : peer_fd_(::posix_openpt(O_RDWR | O_NOCTTY)) + { + REQUIRE(0 <= peer_fd_); + REQUIRE(0 == ::grantpt(peer_fd_)); + REQUIRE(0 == ::unlockpt(peer_fd_)); + device_path_ = ::ptsname(peer_fd_); + } + + ~VirtualSerialPair() + { + ::close(peer_fd_); + } + + const std::string & devicePath() const + { + return device_path_; + } + + int fd() const + { + return peer_fd_; + } + + void write(const std::vector & data) + { + REQUIRE(static_cast(data.size()) == ::write(peer_fd_, data.data(), data.size())); + } + + std::vector read(size_t size) + { + std::vector data; + while (data.size() < size) { + struct pollfd poll_fd = {peer_fd_, POLLIN, 0}; + REQUIRE(0 < ::poll(&poll_fd, 1, PEER_POLL_TIMEOUT_MS)); + std::vector chunk(size - data.size()); + const ssize_t bytes_read = ::read(peer_fd_, chunk.data(), chunk.size()); + REQUIRE(0 < bytes_read); + data.insert(data.end(), chunk.begin(), chunk.begin() + bytes_read); + } + return data; + } + +private: + int peer_fd_; + std::string device_path_; +}; + +} // namespace + +TEST_CASE("SerialPort closes the device on destruction", "[serial_port]") +{ + VirtualSerialPair pair; + { + SerialPort port(pair.devicePath()); + } + // The peer sees a hang-up once no one holds the device open. + struct pollfd poll_fd = {pair.fd(), POLLIN, 0}; + REQUIRE(0 < ::poll(&poll_fd, 1, PEER_POLL_TIMEOUT_MS)); + REQUIRE(0 != (poll_fd.revents & POLLHUP)); +} + +TEST_CASE("SerialPort passes bytes both ways in raw mode", "[serial_port]") +{ + VirtualSerialPair pair; + const std::vector from_peer = {'$', 'G', 'S', ',', '1', '\r', '\n'}; + std::vector received; + std::promise> read_promise; + SerialPort port(pair.devicePath()); + REQUIRE(port.setOnReceiveCallback([&](std::vector data) { + received.insert(received.end(), data.begin(), data.end()); + if (received.size() == from_peer.size()) { + read_promise.set_value(received); + } + })); + + REQUIRE(port.startReceptionThread()); + REQUIRE_FALSE(port.setOnReceiveCallback([](std::vector /*data*/) {})); + REQUIRE_FALSE(port.setOnErrorCallback([](SerialPort::error_string_t /*error*/) {})); + + pair.write(from_peer); + auto read_future = read_promise.get_future(); + REQUIRE(std::future_status::ready == read_future.wait_for(RECEIVE_WAIT)); + REQUIRE(from_peer == read_future.get()); + + const std::vector to_peer = {0x00, 0x0A, 0x0D, 0xFF, 'o', 'k'}; + REQUIRE_FALSE(port.send(to_peer).has_value()); + REQUIRE(to_peer == pair.read(to_peer.size())); + + REQUIRE(port.joinReceptionThread()); +} + +TEST_CASE("SerialPort reports a hang-up once", "[serial_port]") +{ + auto pair = std::make_unique(); + SerialPort port(pair->devicePath()); + std::mutex errors_mutex; + std::vector errors; + REQUIRE(port.setOnErrorCallback([&](SerialPort::error_string_t error) { + std::lock_guard guard(errors_mutex); + errors.push_back(error); + })); + REQUIRE(port.startReceptionThread()); + + pair.reset(); + std::this_thread::sleep_for(HANG_UP_WAIT); + + REQUIRE(port.joinReceptionThread()); + std::lock_guard guard(errors_mutex); + REQUIRE(1 == errors.size()); +} + +TEST_CASE("SerialPort throws for a missing device", "[serial_port]") +{ + REQUIRE_THROWS_AS(SerialPort("/dev/nonexistent_serial_port_test"), std::system_error); +} + +TEST_CASE("SerialPort throws for a file that is not a terminal", "[serial_port]") +{ + const auto path = std::filesystem::temp_directory_path() / + ("serial_port_test_" + std::to_string(::getpid()) + "_" + std::to_string(::rand())); + std::ofstream(path) << "not a tty"; + + REQUIRE_THROWS_AS(SerialPort(path.string()), std::system_error); + std::filesystem::remove(path); +}