From c017737526a7adde9b052835c1c92030509d1edf Mon Sep 17 00:00:00 2001 From: David Tarazi Date: Tue, 21 Apr 2026 13:55:47 -0700 Subject: [PATCH 1/2] adding axiomatic adapter package for Axiomatic CAN-ETH functionality --- CHANGELOG.md | 7 + README.md | 6 + axiomatic_adapter/CHANGELOG.rst | 3 + axiomatic_adapter/CMakeLists.txt | 123 +++++ axiomatic_adapter/README.md | 55 +++ .../axiomatic_adapter/axiomatic_adapter.hpp | 123 +++++ .../axiomatic_socketcan_bridge.hpp | 74 +++ axiomatic_adapter/package.xml | 24 + axiomatic_adapter/src/axiomatic_adapter.cpp | 444 ++++++++++++++++++ .../src/axiomatic_socketcan_bridge.cpp | 137 ++++++ .../src/axiomatic_socketcan_bridge_node.cpp | 137 ++++++ .../test/axiomatic_adapter_test.cpp | 220 +++++++++ 12 files changed, 1353 insertions(+) create mode 100644 axiomatic_adapter/CHANGELOG.rst create mode 100644 axiomatic_adapter/CMakeLists.txt create mode 100644 axiomatic_adapter/README.md create mode 100644 axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp create mode 100644 axiomatic_adapter/include/axiomatic_adapter/axiomatic_socketcan_bridge.hpp create mode 100644 axiomatic_adapter/package.xml create mode 100644 axiomatic_adapter/src/axiomatic_adapter.cpp create mode 100644 axiomatic_adapter/src/axiomatic_socketcan_bridge.cpp create mode 100644 axiomatic_adapter/src/axiomatic_socketcan_bridge_node.cpp create mode 100644 axiomatic_adapter/test/axiomatic_adapter_test.cpp diff --git a/CHANGELOG.md b/CHANGELOG.md index 8c79851..5dfa9fd 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -10,3 +10,10 @@ __INITIAL RELEASE__ - Ability to send and receive CanFrame types - Built in Threading to receive data - SocketcanBridgeNode to run a ros2 passthrough node + +## v0.2.1 + +### Features +- Now with `axiomatic_adapter` package! +- Introduces an additional interface between Axiomatic's CAN-ETH converter and CAN messages +- Has a wrapper to put ETH CAN traffic onto a socketcan interface using `SocketcanAdapter` diff --git a/README.md b/README.md index 1c82b3e..4d1e56d 100644 --- a/README.md +++ b/README.md @@ -21,6 +21,12 @@ ROS2 wrapper providing nodes and launch files. - Launch files with parameter support - Depends on the core `socketcan_adapter` library +### [axiomatic_adapter](./axiomatic_adapter/README.md) +C++ library to connect CAN data to Axiomatic CAN-ETH adapter +- Core Axiomatic ETH-CAN bridge functionality +- Modeled after the socketcan_adapter +- Has an additional bridge to leverage the `socketcan_adapter` library to put eth traffic on socketcan + ## Quick Start ### Build Both Packages diff --git a/axiomatic_adapter/CHANGELOG.rst b/axiomatic_adapter/CHANGELOG.rst new file mode 100644 index 0000000..5d40fe4 --- /dev/null +++ b/axiomatic_adapter/CHANGELOG.rst @@ -0,0 +1,3 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package axiomatic_adapter +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ diff --git a/axiomatic_adapter/CMakeLists.txt b/axiomatic_adapter/CMakeLists.txt new file mode 100644 index 0000000..0b89491 --- /dev/null +++ b/axiomatic_adapter/CMakeLists.txt @@ -0,0 +1,123 @@ +# 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. + +cmake_minimum_required(VERSION 3.8) +project(axiomatic_adapter) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) + add_link_options("-Wl,--no-undefined") +endif() + +if(NOT DEFINED BUILD_JAMMY) + set(BUILD_JAMMY OFF) + if(EXISTS "/etc/os-release") + file(READ "/etc/os-release" _OS_RELEASE) + string(REGEX MATCH "VERSION_CODENAME=([^\n\r]+)" _match "${_OS_RELEASE}") + if(CMAKE_MATCH_1) + string(TOLOWER "${CMAKE_MATCH_1}" _UBUNTU_CODENAME) + if(_UBUNTU_CODENAME STREQUAL "jammy") + set(BUILD_JAMMY ON) + endif() + endif() + endif() +endif() + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) +find_package(Boost REQUIRED COMPONENTS system) +find_package(CLI11 REQUIRED) +find_package(polymath_corelib REQUIRED) +find_package(polymath_cpputils REQUIRED) +find_package(socketcan_adapter REQUIRED) + +# Axiomatic Adapter Library +add_library(axiomatic_adapter SHARED src/axiomatic_adapter.cpp) + +target_compile_features(axiomatic_adapter PUBLIC c_std_99 cxx_std_17) +target_include_directories(axiomatic_adapter PUBLIC + $ + $ +) +set_target_properties(axiomatic_adapter PROPERTIES LINK_FLAGS "-Wl,--no-undefined") +target_link_libraries(axiomatic_adapter + PUBLIC socketcan_adapter::socketcan_adapter + PRIVATE + Boost::system + polymath_corelib::polymath_corelib + polymath_cpputils::threading +) + +install(DIRECTORY include/ DESTINATION include) + +install(TARGETS axiomatic_adapter + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Axiomatic SocketCAN Bridge +add_executable(axiomatic_socketcan_bridge + src/axiomatic_socketcan_bridge.cpp + src/axiomatic_socketcan_bridge_node.cpp +) + +target_include_directories(axiomatic_socketcan_bridge PUBLIC + $ + $ +) +set_target_properties(axiomatic_socketcan_bridge PROPERTIES LINK_FLAGS "-Wl,--no-undefined") +target_link_libraries(axiomatic_socketcan_bridge + axiomatic_adapter + CLI11::CLI11 + socketcan_adapter::socketcan_adapter +) + +# Install the bridge executable +install(TARGETS axiomatic_socketcan_bridge + DESTINATION lib/${PROJECT_NAME} +) + +if(BUILD_TESTING) + if(BUILD_JAMMY) + find_package(Catch2 2 REQUIRED) + else() + find_package(Catch2 3 REQUIRED) + endif() + include(Catch OPTIONAL) + + # Don't run tests in CI (requires hardware: Axiomatic device + CAN interface) + if(NOT DEFINED ENV{CI}) + add_executable(test_axiomatic_adapter test/axiomatic_adapter_test.cpp) + target_link_libraries(test_axiomatic_adapter PRIVATE + axiomatic_adapter + socketcan_adapter::socketcan_adapter + Catch2::Catch2WithMain + ) + if(COMMAND catch_discover_tests) + catch_discover_tests(test_axiomatic_adapter) + else() + add_test(NAME test_axiomatic_adapter COMMAND test_axiomatic_adapter) + endif() + endif() +endif() + +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + +ament_package() diff --git a/axiomatic_adapter/README.md b/axiomatic_adapter/README.md new file mode 100644 index 0000000..e8f9b50 --- /dev/null +++ b/axiomatic_adapter/README.md @@ -0,0 +1,55 @@ +# Axiomatic Adapter +Library and Adapter for the Axiomatic CAN-ETH converter. + +For more information on the decoding/endcoding and product, see the links below: + +https://www.notion.so/polymathrobotics/Axiomatic-CAN-to-Ethernet-Converter-08e078d8914f40d6b7cd99ebf39fe1b0 + +https://products.axiomatic.com/viewitems/connectivity/ethernet-can-converters + +## Usage +### Socketcan-Axiomatic Bridge + +```bash +ros2 run axiomatic_adapter axiomatic_socketcan_bridge [CAN_INTERFACE_NAME] [IP_ADDRESS] [PORT] [OPTIONAL]--retry-connection[-r] [OPTIONAL]--max-retry-attempts [OPTIONAL]--verbose[-v] + +# Examples +# generic example to bridge vcan0 with axiomatic using 192.168.50.34:4000 +ros2 run axiomatic_adapter axiomatic_socketcan_bridge vcan0 192.168.50.34 4000 + +# 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 + +# this will attempt to reconnect a max number of 100 times before failing +ros2 run axiomatic_adapter axiomatic_socketcan_bridge vcan0 192.168.50.34 4000 -r --max-retry-attempts 100 +``` + +### Library + +```cpp +// construct the adapter +std::string ip_address = "192.168.0.34"; +std::string port = "4000"; +std::chrono::milliseconds receive_timeout_ms(100); + +// the two functions passed in are the receive and error callback functions, receive timeout has a default +polymath::can::AxiomaticAdapter adapter( + ip_address, + port, + [](std::unique_ptr /*frame*/) { /* No-op */ }, + [](polymath::can::AxiomaticAdapter::socket_error_string_t /*error*/) { /*do nothing*/ }, + receive_timeout_ms +); + +// open the socket +adapter.openSocket(); + +// start the reception thread. At this point, it's running +adapter.startReceptionThread(); + +// on shutdown/destruction, thread will join and socket will close +``` + +## KNOWN ISSUES +1. There seems to be an issue with massive amounts of CAN data causing timing inconsistencies through the adapter library + 1. This has not been fully diagnosed diff --git a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp new file mode 100644 index 0000000..61a48e6 --- /dev/null +++ b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp @@ -0,0 +1,123 @@ +// 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_ADAPTER_HPP_ +#define AXIOMATIC_ADAPTER__AXIOMATIC_ADAPTER_HPP_ + +#include +#include + +#include +#include +#include +#include +#include + +#include "socketcan_adapter/can_frame.hpp" + +namespace polymath +{ +namespace can +{ + +/// @brief State of TCP socket, error, open or closed +enum class TCPSocketState +{ + ERROR = -1, + OPEN = 0, + CLOSED = 1, +}; + +/// @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 +{ +public: + /// @brief Mapped to std lib, but should be remapped to Polymath Safety compatible versions + using socket_error_string_t = std::string; + + static constexpr std::chrono::milliseconds DEFAULT_SOCKET_RECEIVE_TIMEOUT_MS{100}; + static constexpr std::chrono::milliseconds JOIN_RECEPTION_TIMEOUT_MS{100}; + + /// @brief AxiomaticAdapter Class Init + /// @param ip_address Axiomatic Device IP address to connect + /// @param port Axiomatic Device Port to connect to + /// @param receive_timeout_ms receive timeout in milliseconds + AxiomaticAdapter( + const std::string & ip_address, + const std::string & port, + const std::function frame)> && receive_callback_function = + [](std::unique_ptr /*frame*/) { /*do nothing*/ }, + const std::function && error_callback_function = + [](socket_error_string_t /*error*/) { /*do nothing*/ }, + const std::chrono::milliseconds & receive_timeout_ms = AxiomaticAdapter::DEFAULT_SOCKET_RECEIVE_TIMEOUT_MS); + + /// @brief Destructor for AxiomaticAdapter + virtual ~AxiomaticAdapter(); + + /// @brief Open TCP Socket + /// @return bool success for opening socket + bool openSocket(); + + /// @brief Close TCP Socket + /// @return bool success for closing socket + bool closeSocket(); + + /// @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); + + /// @brief Receive returns the received CanFrame + /// @return optional CanFrame + /// nullopt is returned if no frame received, acts like null + std::optional receive(); + + /// @brief Start a reception thread (calls callback) + /// @return success on started + bool startReceptionThread(); + + /// @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::milliseconds & timeout_s = AxiomaticAdapter::JOIN_RECEPTION_TIMEOUT_MS); + + /// @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); + + /// @brief Transmit a can frame via socket + /// @param frame Linux CAN frame to send + /// @return optional error string filled with an error message if any + std::optional send(const can_frame & frame); + + /// @brief Get state of socket + /// @return TCPSocketState data type detailing OPEN or CLOSED + TCPSocketState get_socket_state(); + + /// @brief Checks if the receive thread is running + /// @return True if the thread is running, false otherwise + bool is_thread_running(); + +private: + /// @brief use Implemention (pimpl) to avoid including boost/asio.hpp in header + linking in CMake + class AxiomaticAdapterImpl; + std::unique_ptr pimpl_; +}; +} // namespace can +} // namespace polymath + +#endif // AXIOMATIC_ADAPTER__AXIOMATIC_ADAPTER_HPP_ diff --git a/axiomatic_adapter/include/axiomatic_adapter/axiomatic_socketcan_bridge.hpp b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_socketcan_bridge.hpp new file mode 100644 index 0000000..5456b24 --- /dev/null +++ b/axiomatic_adapter/include/axiomatic_adapter/axiomatic_socketcan_bridge.hpp @@ -0,0 +1,74 @@ +// 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_SOCKETCAN_BRIDGE_NODE_HPP_ +#define AXIOMATIC_ADAPTER__AXIOMATIC_SOCKETCAN_BRIDGE_NODE_HPP_ + +#include + +#include "axiomatic_adapter/axiomatic_adapter.hpp" +#include "socketcan_adapter/can_frame.hpp" +#include "socketcan_adapter/socketcan_adapter.hpp" + +namespace polymath +{ +namespace can +{ + +/// @class polymath::can::AxiomaticSocketcanBridge +/// @brief This class will bridge a socketcan connection and read/write to the can-eth device tcp connection +class AxiomaticSocketcanBridge +{ +public: + /// @brief Constructor + /// @param can_interface_name name of the can interface for socketcan + /// @param ip IP address of the ethernet can device + /// @param port port for the ethernet can device + /// @param verbose enables printing of debug logs if true. Defaults to false + AxiomaticSocketcanBridge( + const std::string & can_interface_name, const std::string & ip, const std::string & port, bool verbose = false); + + /// @brief Destruct axiomatic socketcan bridge + ~AxiomaticSocketcanBridge(); + + /// @brief configures and opens the TCP and CAN sockets for the bridge + /// @return success for opening both sockets + bool on_configure(); + + /// @brief activates and starts the reception threads for the socketcan and axiomatic adapters + /// @return success for starting both threads + bool on_activate(); + + /// @brief joins the reception threads to the main thread on deactivate + /// @return success for joining bpth threads + bool on_deactivate(); + + /// @brief Shuts down and closes the TCP and CAN sockets + /// @return success for closing both sockets + bool on_shutdown(); + +private: + polymath::socketcan::SocketcanAdapter socketcan_adapter_; + polymath::can::AxiomaticAdapter axiomatic_adapter_; + + bool verbose_; + + void socketcanReceiveCallback(std::unique_ptr frame); + void ethcanReceiveCallback(std::unique_ptr frame); +}; + +} // namespace can +} // namespace polymath + +#endif // AXIOMATIC_ADAPTER__AXIOMATIC_SOCKETCAN_BRIDGE_NODE_HPP_ diff --git a/axiomatic_adapter/package.xml b/axiomatic_adapter/package.xml new file mode 100644 index 0000000..07a0cd6 --- /dev/null +++ b/axiomatic_adapter/package.xml @@ -0,0 +1,24 @@ + + + + axiomatic_adapter + 0.0.1 + An Adapter Library and socketcan bridge for Axiomatic eth-can interface + Polymath Robotics + Apache-2.0 + David Tarazi + + ament_cmake_ros + + boost + cli11 + polymath_corelib + polymath_cpputils + socketcan_adapter + + catch2 + + + ament_cmake + + diff --git a/axiomatic_adapter/src/axiomatic_adapter.cpp b/axiomatic_adapter/src/axiomatic_adapter.cpp new file mode 100644 index 0000000..df425c0 --- /dev/null +++ b/axiomatic_adapter/src/axiomatic_adapter.cpp @@ -0,0 +1,444 @@ +// 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_adapter.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "polymath_cpputils/mutex_protected.hpp" + +namespace polymath +{ +namespace can +{ + +class AxiomaticAdapter::AxiomaticAdapterImpl +{ +public: + AxiomaticAdapterImpl( + const std::string & ip_address, + const std::string & port, + const std::function frame)> && receive_callback_function, + const std::function && error_callback_function, + const std::chrono::milliseconds & receive_timeout_ms) + : tcp_io_context_() + , tcp_socket_(tcp_io_context_) + , ip_address_(ip_address) + , port_(port) + , receive_callback_(receive_callback_function) + , error_callback_(error_callback_function) + , receive_timeout_ms_(receive_timeout_ms) + {} + + ~AxiomaticAdapterImpl() + { + joinReceptionThread(); + closeSocket(); + } + + bool openSocket() + { + try { + boost::asio::ip::tcp::resolver resolver(tcp_io_context_); + auto endpoints = resolver.resolve(ip_address_, port_); + + boost::asio::steady_timer timer(tcp_io_context_); + timer.expires_after(TCP_IP_CONNECTION_TIMEOUT_MS); + + polymath::core::utils::MutexProtected connection_state{ + {false, boost::asio::error::would_block}}; + + // Asynchronously attempt to connect + boost::asio::async_connect( + tcp_socket_, endpoints, [&](const boost::system::error_code & error, const boost::asio::ip::tcp::endpoint &) { + auto guard = connection_state.lock(); + guard->error_code = error; + guard->connected = !error; + // Cancel timeout if connected successfully + timer.cancel(); + }); + + // Set up a timer to cancel the operation if it exceeds the timeout + timer.async_wait([&](const boost::system::error_code & error) { + if (!error) { + auto guard = connection_state.lock(); + if (!guard->connected) { + guard->error_code = boost::asio::error::timed_out; + tcp_socket_.cancel(); + } + } + }); + + // Run the I/O context to handle events + tcp_io_context_.restart(); + tcp_io_context_.run(); + + // capture the error message and connection state + boost::system::error_code captured_error; + bool is_connected; + { + auto guard = connection_state.lock(); + captured_error = guard->error_code; + is_connected = guard->connected; + } + + if (captured_error || !is_connected) { + std::cerr << "Connection failed: " << captured_error.message() << std::endl; + socket_state_ = TCPSocketState::ERROR; + return false; + } + + socket_state_ = TCPSocketState::OPEN; + return true; + } catch (std::exception & e) { + std::cerr << "Connection failed (exception): " << e.what() << std::endl; + socket_state_ = TCPSocketState::ERROR; + return false; + } + } + + bool closeSocket() + { + if (socket_state_ != TCPSocketState::CLOSED) { + boost::system::error_code error_code; + tcp_socket_.close(error_code); + + if (error_code) { + std::cerr << "[ERROR] Failed to close TCP socket: " << error_code.message() << std::endl; + return false; + } else { + socket_state_ = TCPSocketState::CLOSED; + return true; + } + } + return true; + } + + bool startReceptionThread() + { + if (socket_state_ == TCPSocketState::CLOSED) { + return false; + } + + stop_thread_requested_ = false; + thread_running_ = true; + + 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 { + error_callback_(*error); + } + } + + thread_running_ = false; + }); + + return true; + } + + bool joinReceptionThread(const std::chrono::milliseconds & timeout_s = AxiomaticAdapter::JOIN_RECEPTION_TIMEOUT_MS) + { + stop_thread_requested_ = true; + + if (tcp_receive_thread_.joinable()) { + // Use std::async to wait asynchronously for the thread to stop + std::future join_future = std::async(std::launch::async, [this] { tcp_receive_thread_.join(); }); + + // Wait for the thread to stop within the timeout period + return join_future.wait_for(timeout_s) == std::future_status::ready; + } + + return false; + } + + std::optional receive(polymath::socketcan::CanFrame & can_frame) + { + std::vector data(1024, 0); + std::atomic data_received(false); + boost::system::error_code error_code; + + // Set up the timer for timeout + boost::asio::steady_timer timer(tcp_io_context_); + timer.expires_after(receive_timeout_ms_); + + // Start async receive operation + tcp_socket_.async_receive( + boost::asio::buffer(data), [&](const boost::system::error_code & error, std::size_t bytes_transferred) { + error_code = error; + if (!error) { + data.resize(bytes_transferred); + data_received = true; + } + timer.cancel(); + }); + + // Set up the timer to handle timeout cancellation + timer.async_wait([&](const boost::system::error_code & error) { + if (!error && !data_received.load()) { + error_code = boost::asio::error::timed_out; + // Cancel the ongoing async receive operation on timeout (does not close socket) + tcp_socket_.cancel(); + } + }); + + // Run the I/O operations concurrently (this allows for new async operations in the future) + tcp_io_context_.restart(); + tcp_io_context_.run(); + + // Check for timeout or other errors + if (error_code == boost::asio::error::timed_out) { + return std::optional("Receive operation timed out"); + } else if (error_code) { + return std::optional( + "Receive operation failed: " + error_code.message()); + } + + // --- Process the received data --- + + // Check size, header info and message type + if (data.size() < AXIOMATIC_CAN_MESSAGE_HEADER.size() + 5) { + return std::make_optional("Data too short for header and control byte."); + } + if (!std::equal(AXIOMATIC_CAN_MESSAGE_HEADER.begin(), AXIOMATIC_CAN_MESSAGE_HEADER.end(), data.begin())) { + return std::make_optional("Not a valid CAN message."); + } + + uint8_t control_byte = data[11]; + // Extract timestamp size (bits 6 & 5) + size_t timestamp_size = (control_byte & 0x60) >> 5; + // Check if the frame is extended (bit 4) + bool is_can_extended = (control_byte & 0x10) >> 4; + // Extract CAN frame length (lower 4 bits) + size_t can_length = control_byte & 0x0F; + + // Determine where the CAN ID starts (after timestamp bytes) + size_t can_id_start = 12 + timestamp_size; + uint32_t can_id = 0; + size_t can_data_start = 0; + + // Ensure data is large enough for CAN ID extraction + size_t min_id_size = is_can_extended ? 4 : 2; + if (data.size() < can_id_start + min_id_size) { + return std::make_optional("Data too short for CAN ID."); + } + + // Extract CAN ID (little-endian) + if (!is_can_extended) { + can_id = static_cast(data[can_id_start] | (data[can_id_start + 1] << 8)); + can_data_start = can_id_start + 2; + } else { + can_id = static_cast( + data[can_id_start] | (data[can_id_start + 1] << 8) | (data[can_id_start + 2] << 16) | + (data[can_id_start + 3] << 24)); + can_data_start = can_id_start + 4; + can_frame.set_id_as_extended(); + } + + // Ensure data is large enough for CAN payload + if (data.size() < can_data_start + can_length) { + return std::make_optional("Data too short for CAN payload."); + } + + // Extract CAN data (zero-padded to 8 bytes) + std::array can_data = {0}; + std::copy_n(data.begin() + can_data_start, can_length, can_data.begin()); + + // Set CAN frame properties + can_frame.set_can_id(can_id); + can_frame.set_len(can_length); + can_frame.set_data(can_data); + + 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) + { + auto frame_data = frame.get_data(); + auto frame_data_length = frame.get_len(); + size_t control_timestamp_byte_length = 3; + + // Determine the CAN frame ID length (extended or standard) + size_t frame_id_byte_length; + bool is_extended = false; + if (frame.get_id_type() == polymath::socketcan::IdType::EXTENDED) { + frame_id_byte_length = 4; + is_extended = true; + } else { + frame_id_byte_length = 2; + } + + size_t message_length = frame_data_length + control_timestamp_byte_length + frame_id_byte_length; + unsigned char control_byte = (1 << 6); + control_byte |= (is_extended ? (1 << 4) : 0); + control_byte |= (frame_data_length & 0x0F); + + // initialize the full message with the header, control bytes, timestamp bytes + std::vector full_message; + full_message.insert(full_message.end(), AXIOMATIC_CAN_MESSAGE_HEADER.begin(), AXIOMATIC_CAN_MESSAGE_HEADER.end()); + full_message.push_back(0x00); + full_message.push_back(0x00); + full_message.push_back(static_cast(message_length & 0xFF)); + full_message.push_back(static_cast((message_length >> 8) & 0xFF)); + full_message.push_back(control_byte); + full_message.push_back(192); + full_message.push_back(70); + + // insert the can frame id + auto can_id = frame.get_id(); + for (size_t i = 0; i < frame_id_byte_length; ++i) { + full_message.push_back(static_cast((can_id >> (i * 8)) & 0xFF)); + } + // insert the can frame data + full_message.insert(full_message.end(), frame_data.begin(), frame_data.end()); + + try { + boost::asio::write(tcp_socket_, boost::asio::buffer(full_message.data(), full_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::array AXIOMATIC_CAN_MESSAGE_HEADER = {'A', 'X', 'I', 'O', 0xBA, 0x36, 0x01}; + static constexpr std::chrono::milliseconds TCP_IP_CONNECTION_TIMEOUT_MS{3000}; + + /// @brief Socket connection state as a struct for the mutex during TCP Open Socket to update the variables together + struct TCPSocketConnectionState + { + bool connected{false}; + boost::system::error_code error_code{boost::asio::error::would_block}; + }; + + boost::asio::io_context tcp_io_context_; + boost::asio::ip::tcp::socket tcp_socket_; + TCPSocketState socket_state_{TCPSocketState::CLOSED}; + + std::thread tcp_receive_thread_; + std::atomic thread_running_; + std::atomic stop_thread_requested_; + + // from construction + std::string ip_address_; + std::string port_; + std::function frame)> receive_callback_; + std::function error_callback_; + std::chrono::milliseconds receive_timeout_ms_; +}; + +AxiomaticAdapter::AxiomaticAdapter( + const std::string & ip_address, + const std::string & port, + const std::function frame)> && receive_callback_function, + const std::function && error_callback_function, + const std::chrono::milliseconds & receive_timeout_ms) +: pimpl_(std::make_unique( + ip_address, port, std::move(receive_callback_function), std::move(error_callback_function), receive_timeout_ms)) +{} + +AxiomaticAdapter::~AxiomaticAdapter() +{ + closeSocket(); +} + +bool AxiomaticAdapter::openSocket() +{ + return pimpl_->openSocket(); +} + +bool AxiomaticAdapter::closeSocket() +{ + return pimpl_->closeSocket(); +} + +bool AxiomaticAdapter::startReceptionThread() +{ + return pimpl_->startReceptionThread(); +} + +bool AxiomaticAdapter::joinReceptionThread(const std::chrono::milliseconds & timeout_s) +{ + return pimpl_->joinReceptionThread(timeout_s); +} + +std::optional AxiomaticAdapter::receive( + polymath::socketcan::CanFrame & can_frame) +{ + return pimpl_->receive(can_frame); +} + +std::optional AxiomaticAdapter::receive() +{ + return pimpl_->receive(); +} + +std::optional AxiomaticAdapter::send( + const polymath::socketcan::CanFrame & frame) +{ + return pimpl_->send(frame); +} + +std::optional AxiomaticAdapter::send(const can_frame & frame) +{ + return send(polymath::socketcan::CanFrame(frame)); +} + +TCPSocketState AxiomaticAdapter::get_socket_state() +{ + return pimpl_->get_socket_state(); +} + +bool AxiomaticAdapter::is_thread_running() +{ + return pimpl_->is_thread_running(); +} + +} // namespace can +} // namespace polymath diff --git a/axiomatic_adapter/src/axiomatic_socketcan_bridge.cpp b/axiomatic_adapter/src/axiomatic_socketcan_bridge.cpp new file mode 100644 index 0000000..141a304 --- /dev/null +++ b/axiomatic_adapter/src/axiomatic_socketcan_bridge.cpp @@ -0,0 +1,137 @@ +// 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_socketcan_bridge.hpp" + +#include +#include + +namespace polymath +{ +namespace can +{ + +AxiomaticSocketcanBridge::AxiomaticSocketcanBridge( + const std::string & can_interface_name, const std::string & ip, const std::string & port, bool verbose) +: socketcan_adapter_(can_interface_name) +, axiomatic_adapter_(ip, port, std::bind(&AxiomaticSocketcanBridge::ethcanReceiveCallback, this, std::placeholders::_1)) +, verbose_(verbose) +{ + socketcan_adapter_.setOnReceiveCallback( + std::bind(&AxiomaticSocketcanBridge::socketcanReceiveCallback, this, std::placeholders::_1)); +} + +AxiomaticSocketcanBridge::~AxiomaticSocketcanBridge() +{ + on_deactivate(); + on_shutdown(); +} + +bool AxiomaticSocketcanBridge::on_configure() +{ + // open sockets + if (!socketcan_adapter_.openSocket()) { + std::cout << "Socketcan Adapter can't open socket..." << std::endl; + return false; + } + if (!axiomatic_adapter_.openSocket()) { + std::cout << "Axiomatic Adapter can't open socket..." << std::endl; + return false; + } + return true; +} + +bool AxiomaticSocketcanBridge::on_activate() +{ + if (!socketcan_adapter_.startReceptionThread()) { + return false; + } + if (!axiomatic_adapter_.startReceptionThread()) { + return false; + } + return true; +} + +bool AxiomaticSocketcanBridge::on_deactivate() +{ + bool success = true; + if (!socketcan_adapter_.joinReceptionThread()) { + success = false; + } + if (!axiomatic_adapter_.joinReceptionThread()) { + success = false; + } + return success; +} + +bool AxiomaticSocketcanBridge::on_shutdown() +{ + bool success = true; + if (!socketcan_adapter_.closeSocket()) { + success = false; + } + if (!axiomatic_adapter_.closeSocket()) { + success = false; + } + return success; +} + +void AxiomaticSocketcanBridge::socketcanReceiveCallback(std::unique_ptr frame) +{ + if (!frame) { + return; + } + auto frame_copy = polymath::socketcan::CanFrame(*frame); + + if (verbose_) { + std::cout << "[SocketCAN RX] Received CAN Frame: ID = " << std::hex << frame_copy.get_id() << ", Data = ["; + + auto data = frame_copy.get_data(); + for (size_t i = 0; i < data.size(); ++i) { + std::cout << " " << std::hex << static_cast(data[i]); + } + std::cout << " ]" << std::endl; + } + + auto result = axiomatic_adapter_.send(frame_copy); + if (!result) { + std::cerr << "[SocketCAN RX] Failed to send CAN frame with message: " << *result << std::endl; + } +} + +void AxiomaticSocketcanBridge::ethcanReceiveCallback(std::unique_ptr frame) +{ + if (!frame) { + return; + } + auto frame_copy = polymath::socketcan::CanFrame(*frame); + + if (verbose_) { + std::cout << "[EthCAN RX] Received CAN Frame: ID = " << std::hex << frame_copy.get_id() << ", Data = ["; + + auto data = frame_copy.get_data(); + for (size_t i = 0; i < data.size(); ++i) { + std::cout << " " << std::hex << static_cast(data[i]); + } + std::cout << " ]" << std::endl; + } + + auto result = socketcan_adapter_.send(frame_copy); + if (!result) { + std::cerr << "[EthCAN RX] Failed to send CAN frame with message: " << *result << std::endl; + } +} + +} // namespace can +} // namespace polymath diff --git a/axiomatic_adapter/src/axiomatic_socketcan_bridge_node.cpp b/axiomatic_adapter/src/axiomatic_socketcan_bridge_node.cpp new file mode 100644 index 0000000..b2536f4 --- /dev/null +++ b/axiomatic_adapter/src/axiomatic_socketcan_bridge_node.cpp @@ -0,0 +1,137 @@ +// 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 "axiomatic_adapter/axiomatic_socketcan_bridge.hpp" + +bool verbose(false); +bool retry_connection(false); +int max_retry_attempts(-1); +int retry_counter(0); +std::atomic shutdown_requested(false); +std::condition_variable shutdown_conditional; + +// Signal handler +void signalHandler(int signal) +{ + if (signal == SIGINT || signal == SIGTERM) { + std::cout << "Signal received, shutting down..." << std::endl; + shutdown_requested.store(true); + shutdown_conditional.notify_all(); + } +} + +// CLI argument parser +void configureArguments( + CLI::App & app, + std::string & can_interface, + std::string & ip, + std::string & port, + bool & verbose, + bool & retry_connection, + int & max_retry_attempts) +{ + 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"); + app.add_option("port", port, "Port number of the bridge (default: 4000)")->default_val("4000"); + app.add_flag("-v,--verbose", verbose, "Enable verbose logging"); + app.add_flag("-r,--retry-connection", retry_connection, "Retry configure/activate if it fails"); + app + .add_option("--max-retry-attempts", max_retry_attempts, "Maximum number of retry attempts (default: -1 = infinite)") + ->default_val(-1); +} + +int main(int argc, char * argv[]) +{ + std::signal(SIGINT, signalHandler); + std::signal(SIGTERM, signalHandler); + std::string can_interface, ip, port; + CLI::App app{"Axiomatic SocketCAN Bridge"}; + configureArguments(app, can_interface, ip, port, verbose, retry_connection, max_retry_attempts); + CLI11_PARSE(app, argc, argv); + + polymath::can::AxiomaticSocketcanBridge bridge(can_interface, ip, port, verbose); + + std::cout << "Axiomatic Socketcan Bridge configuring..." << std::endl; + + while (!shutdown_requested.load() && !bridge.on_configure()) { + std::cerr << "Configuration failed."; + if (!retry_connection) { + std::cerr << " Exiting..." << std::endl; + bridge.on_shutdown(); + return 1; + } + if (max_retry_attempts >= 0 && retry_counter >= max_retry_attempts) { + std::cerr << " (max retry attempts reached). Exiting." << std::endl; + bridge.on_shutdown(); + return 1; + } + retry_counter += 1; + std::cerr << " Retrying in 3 seconds..." << std::endl; + std::this_thread::sleep_for(std::chrono::seconds(3)); + } + if (shutdown_requested.load()) { + std::cerr << "Shutdown requested during configuration. Exiting early." << std::endl; + bridge.on_shutdown(); + return 0; + } + + std::cerr << "Configuration succeeded." << std::endl; + std::cout << "Axiomatic Socketcan Bridge activating..." << std::endl; + + while (!shutdown_requested.load() && !bridge.on_activate()) { + std::cerr << "Activation failed."; + if (!retry_connection) { + std::cerr << " Exiting..." << std::endl; + bridge.on_shutdown(); + return 1; + } + if (max_retry_attempts >= 0 && retry_counter >= max_retry_attempts) { + std::cerr << " (max retry attempts reached). Exiting." << std::endl; + bridge.on_shutdown(); + return 1; + } + retry_counter += 1; + std::cerr << " Retrying in 3 seconds..." << std::endl; + std::this_thread::sleep_for(std::chrono::seconds(3)); + } + + if (shutdown_requested.load()) { + std::cerr << "Shutdown requested during activation. Exiting early." << std::endl; + bridge.on_shutdown(); + return 0; + } + std::cerr << "Activation succeeded." << std::endl; + + std::mutex shutdown_mutex; + { + std::unique_lock lock(shutdown_mutex); + shutdown_conditional.wait(lock, [] { return shutdown_requested.load(); }); + } + + std::cout << "Axiomatic Socketcan Bridge deactivating..." << std::endl; + bridge.on_deactivate(); + std::cout << "Axiomatic Socketcan Bridge shutting down..." << std::endl; + bridge.on_shutdown(); + + return 0; +} diff --git a/axiomatic_adapter/test/axiomatic_adapter_test.cpp b/axiomatic_adapter/test/axiomatic_adapter_test.cpp new file mode 100644 index 0000000..cf82ee3 --- /dev/null +++ b/axiomatic_adapter/test/axiomatic_adapter_test.cpp @@ -0,0 +1,220 @@ +// 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_adapter.hpp" + +#include +#include +#include +#include + +#if __has_include() + #include // v3 +#else + #include // v2 +#endif +#include "socketcan_adapter/socketcan_adapter.hpp" + +using polymath::can::AxiomaticAdapter; +using polymath::can::TCPSocketState; +using polymath::socketcan::CanFrame; + +// NOTE: These tests were written to run on hq0-robot04 with the in office CAN setup +// NOTE: these tests leverage the socketcan adapter to act as a "secondary device" transmitting messages +// NOTE: the first run may fail. This is due to an issue with the Axiomatic adapter that causes just the first message +// read to fail. +TEST_CASE("AxiomaticAdapter tests", "[AxiomaticAdapter]") +{ + SECTION("Constructor and destructor") + { + std::string ip_address = "192.168.0.34"; + std::string port = "4000"; + AxiomaticAdapter adapter(ip_address, port); + REQUIRE(adapter.get_socket_state() == TCPSocketState::CLOSED); + } + + SECTION("Open and close socket") + { + std::string ip_address = "192.168.0.34"; + std::string port = "4000"; + AxiomaticAdapter adapter(ip_address, port); + REQUIRE(adapter.openSocket()); + REQUIRE(adapter.get_socket_state() == TCPSocketState::OPEN); + REQUIRE(adapter.closeSocket()); + REQUIRE(adapter.get_socket_state() == TCPSocketState::CLOSED); + } + + SECTION("Receive CanFrame") + { + std::string socketcan_send_inteface = "can0"; + std::string ip_address = "192.168.0.34"; + std::string port = "4000"; + + polymath::socketcan::SocketcanAdapter send_adapter(socketcan_send_inteface); + REQUIRE(send_adapter.openSocket()); + + AxiomaticAdapter receipt_adapter(ip_address, port); + REQUIRE(receipt_adapter.openSocket()); + + CanFrame frame; + frame.set_can_id(0x123); + REQUIRE(frame.get_id() == 0x123); + frame.set_len(4); + std::array data = {0x01, 0x02, 0x03, 0x04}; + frame.set_data(data); + + auto send_result = send_adapter.send(frame); + REQUIRE(!send_result.has_value()); + + std::optional maybe_frame; + // try to receive 20 times + for (int i = 0; i < 20; i++) { + maybe_frame = receipt_adapter.receive(); + if (maybe_frame.has_value()) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + REQUIRE(maybe_frame.has_value()); + + CanFrame received_frame = *maybe_frame; + REQUIRE(received_frame.get_id() == 0x123); + REQUIRE(received_frame.get_len() == 4); + REQUIRE(received_frame.get_data() == data); + + REQUIRE(send_adapter.closeSocket()); + REQUIRE(receipt_adapter.closeSocket()); + } + + SECTION("Send CanFrame") + { + std::string socketcan_inteface = "can0"; + std::string ip_address = "192.168.0.34"; + std::string port = "4000"; + + polymath::socketcan::SocketcanAdapter receipt_adapter(socketcan_inteface); + REQUIRE(receipt_adapter.openSocket()); + + AxiomaticAdapter send_adapter(ip_address, port); + REQUIRE(send_adapter.openSocket()); + + CanFrame frame; + frame.set_can_id(0x123); + REQUIRE(frame.get_id() == 0x123); + frame.set_len(4); + std::array data = {0x01, 0x02, 0x03, 0x04}; + frame.set_data(data); + + auto send_result = send_adapter.send(frame); + REQUIRE(!send_result.has_value()); + + std::optional maybe_frame; + // try to receive 20 times + for (int i = 0; i < 20; i++) { + maybe_frame = receipt_adapter.receive(); + if (maybe_frame.has_value()) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + REQUIRE(maybe_frame.has_value()); + + CanFrame received_frame = *maybe_frame; + REQUIRE(received_frame.get_id() == 0x123); + REQUIRE(received_frame.get_len() == 4); + REQUIRE(received_frame.get_data() == data); + + REQUIRE(send_adapter.closeSocket()); + REQUIRE(receipt_adapter.closeSocket()); + } + + SECTION("Reception thread") + { + std::string socketcan_send_inteface = "can0"; + std::string ip_address = "192.168.0.34"; + std::string port = "4000"; + + polymath::socketcan::SocketcanAdapter send_adapter(socketcan_send_inteface); + REQUIRE(send_adapter.openSocket()); + + bool callback_called = false; + AxiomaticAdapter receipt_adapter( + ip_address, port, [&callback_called](std::unique_ptr /*frame*/) { callback_called = true; }); + + REQUIRE(receipt_adapter.openSocket()); + REQUIRE(receipt_adapter.startReceptionThread()); + + CanFrame frame; + frame.set_can_id(0x123); + frame.set_len(4); + std::array data = {0x01, 0x02, 0x03, 0x04}; + frame.set_data(data); + + auto send_result = send_adapter.send(frame); + REQUIRE(!send_result.has_value()); + + // Allow some time for the reception thread + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + REQUIRE(callback_called); + + REQUIRE(receipt_adapter.joinReceptionThread()); + REQUIRE(receipt_adapter.closeSocket()); + REQUIRE(send_adapter.closeSocket()); + } + + SECTION("Error handling") + { + std::string ip_address = "192.168.0.34"; + std::string port = "4000"; + + int32_t num_error_callbacks_called = 0; + std::string error_message = ""; + AxiomaticAdapter adapter( + ip_address, + port, + [](std::unique_ptr /*frame*/) { /*do nothing*/ }, + [&num_error_callbacks_called, &error_message](std::string error) { + num_error_callbacks_called++; + error_message = error; + }); + + adapter.openSocket(); + adapter.startReceptionThread(); + + REQUIRE(adapter.closeSocket()); + + // Allow for poll to fail + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + + adapter.joinReceptionThread(); + + REQUIRE(num_error_callbacks_called > 0); + REQUIRE(!error_message.empty()); + } + + SECTION("Check thread running state") + { + std::string ip_address = "192.168.0.34"; + std::string port = "4000"; + AxiomaticAdapter adapter(ip_address, port, [](std::unique_ptr /*frame*/) { /* No-op */ }); + REQUIRE(!adapter.is_thread_running()); + REQUIRE(adapter.openSocket()); + + REQUIRE(adapter.startReceptionThread()); + REQUIRE(adapter.is_thread_running()); + REQUIRE(adapter.joinReceptionThread()); + REQUIRE(!adapter.is_thread_running()); + REQUIRE(adapter.closeSocket()); + } +} From 1d97a8d75d1b6b85599982631d93d6ce41d25de8 Mon Sep 17 00:00:00 2001 From: David Tarazi Date: Tue, 21 Apr 2026 15:59:18 -0700 Subject: [PATCH 2/2] remove last polymath-specific packages and tooling --- axiomatic_adapter/CMakeLists.txt | 9 +++----- axiomatic_adapter/package.xml | 2 -- axiomatic_adapter/src/axiomatic_adapter.cpp | 25 ++++++++++----------- 3 files changed, 15 insertions(+), 21 deletions(-) diff --git a/axiomatic_adapter/CMakeLists.txt b/axiomatic_adapter/CMakeLists.txt index 0b89491..fe6b375 100644 --- a/axiomatic_adapter/CMakeLists.txt +++ b/axiomatic_adapter/CMakeLists.txt @@ -42,8 +42,6 @@ find_package(ament_cmake REQUIRED) find_package(ament_cmake_ros REQUIRED) find_package(Boost REQUIRED COMPONENTS system) find_package(CLI11 REQUIRED) -find_package(polymath_corelib REQUIRED) -find_package(polymath_cpputils REQUIRED) find_package(socketcan_adapter REQUIRED) # Axiomatic Adapter Library @@ -59,8 +57,6 @@ target_link_libraries(axiomatic_adapter PUBLIC socketcan_adapter::socketcan_adapter PRIVATE Boost::system - polymath_corelib::polymath_corelib - polymath_cpputils::threading ) install(DIRECTORY include/ DESTINATION include) @@ -102,8 +98,9 @@ if(BUILD_TESTING) endif() include(Catch OPTIONAL) - # Don't run tests in CI (requires hardware: Axiomatic device + CAN interface) - if(NOT DEFINED ENV{CI}) + option(HARDWARE_CONNECTED "Enable tests that require a connected Axiomatic device and CAN interface" OFF) + + if(HARDWARE_CONNECTED) add_executable(test_axiomatic_adapter test/axiomatic_adapter_test.cpp) target_link_libraries(test_axiomatic_adapter PRIVATE axiomatic_adapter diff --git a/axiomatic_adapter/package.xml b/axiomatic_adapter/package.xml index 07a0cd6..fb3278e 100644 --- a/axiomatic_adapter/package.xml +++ b/axiomatic_adapter/package.xml @@ -12,8 +12,6 @@ boost cli11 - polymath_corelib - polymath_cpputils socketcan_adapter catch2 diff --git a/axiomatic_adapter/src/axiomatic_adapter.cpp b/axiomatic_adapter/src/axiomatic_adapter.cpp index df425c0..8fb1491 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 @@ -30,8 +31,6 @@ #include #include -#include "polymath_cpputils/mutex_protected.hpp" - namespace polymath { namespace can @@ -70,15 +69,15 @@ class AxiomaticAdapter::AxiomaticAdapterImpl boost::asio::steady_timer timer(tcp_io_context_); timer.expires_after(TCP_IP_CONNECTION_TIMEOUT_MS); - polymath::core::utils::MutexProtected connection_state{ - {false, boost::asio::error::would_block}}; + TCPSocketConnectionState connection_state{false, boost::asio::error::would_block}; + std::mutex connection_state_mutex; // Asynchronously attempt to connect boost::asio::async_connect( tcp_socket_, endpoints, [&](const boost::system::error_code & error, const boost::asio::ip::tcp::endpoint &) { - auto guard = connection_state.lock(); - guard->error_code = error; - guard->connected = !error; + std::lock_guard guard(connection_state_mutex); + connection_state.error_code = error; + connection_state.connected = !error; // Cancel timeout if connected successfully timer.cancel(); }); @@ -86,9 +85,9 @@ class AxiomaticAdapter::AxiomaticAdapterImpl // Set up a timer to cancel the operation if it exceeds the timeout timer.async_wait([&](const boost::system::error_code & error) { if (!error) { - auto guard = connection_state.lock(); - if (!guard->connected) { - guard->error_code = boost::asio::error::timed_out; + std::lock_guard guard(connection_state_mutex); + if (!connection_state.connected) { + connection_state.error_code = boost::asio::error::timed_out; tcp_socket_.cancel(); } } @@ -102,9 +101,9 @@ class AxiomaticAdapter::AxiomaticAdapterImpl boost::system::error_code captured_error; bool is_connected; { - auto guard = connection_state.lock(); - captured_error = guard->error_code; - is_connected = guard->connected; + std::lock_guard guard(connection_state_mutex); + captured_error = connection_state.error_code; + is_connected = connection_state.connected; } if (captured_error || !is_connected) {