Skip to content
Draft
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
76 changes: 54 additions & 22 deletions axiomatic_adapter/include/axiomatic_adapter/axiomatic_adapter.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,28 +25,36 @@
#include <string>

#include "socketcan_adapter/can_frame.hpp"
#include "socketcan_adapter/i_can_backend.hpp"
#include "socketcan_adapter/socketcan_adapter.hpp" // for polymath::socketcan::SocketState

namespace polymath
{
namespace can
{

/// @brief State of TCP socket, error, open or closed
enum class TCPSocketState
{
ERROR = -1,
OPEN = 0,
CLOSED = 1,
};
/// @brief State of TCP socket — kept as an alias of polymath::socketcan::SocketState
/// since the legacy enum's values matched the interface enum exactly. The alias
/// preserves source compatibility for existing callers (e.g. the bridge node
/// using `TCPSocketState::OPEN`) while letting AxiomaticAdapter implement the
/// ICanBackend::get_socket_state() contract.
using TCPSocketState = polymath::socketcan::SocketState;

/// @class polymath::can::AxiomaticAdapter
/// @brief Creates and manages a tcp connection and simplifies the interface.
/// Generally does not throw, but returns booleans to tell you success
class AxiomaticAdapter : public std::enable_shared_from_this<AxiomaticAdapter>
/// Generally does not throw, but returns booleans to tell you success.
///
/// As of 2026-05 AxiomaticAdapter implements polymath::socketcan::ICanBackend
/// so that consumers can hold a non-owning ICanBackend* and select the
/// transport (kernel SocketCAN, this Axiomatic ETH-CAN bridge) at construction
/// time. The retrofit is purely additive: the existing constructor that takes
/// receive + error callbacks remains the canonical setup path, and runtime
/// callback setters were added to satisfy the interface — set them BEFORE
/// startReceptionThread() to avoid a benign race with the rx thread.
class AxiomaticAdapter : public polymath::socketcan::ICanBackend, public std::enable_shared_from_this<AxiomaticAdapter>
{
public:
/// @brief Mapped to std lib, but should be remapped to Polymath Safety compatible versions
using socket_error_string_t = std::string;
using socket_error_string_t = polymath::socketcan::ICanBackend::socket_error_string_t;

static constexpr std::chrono::milliseconds DEFAULT_SOCKET_RECEIVE_TIMEOUT_MS{100};
static constexpr std::chrono::milliseconds JOIN_RECEPTION_TIMEOUT_MS{100};
Expand All @@ -65,20 +73,20 @@ class AxiomaticAdapter : public std::enable_shared_from_this<AxiomaticAdapter>
const std::chrono::milliseconds & receive_timeout_ms = AxiomaticAdapter::DEFAULT_SOCKET_RECEIVE_TIMEOUT_MS);

/// @brief Destructor for AxiomaticAdapter
virtual ~AxiomaticAdapter();
~AxiomaticAdapter() override;

/// @brief Open TCP Socket
/// @return bool success for opening socket
bool openSocket();
bool openSocket() override;

/// @brief Close TCP Socket
/// @return bool success for closing socket
bool closeSocket();
bool closeSocket() override;

/// @brief Receive with a reference to a CanFrame to fill
/// @param frame OUTPUT CanFrame to fill
/// @return optional error string filled with an error message if any
std::optional<socket_error_string_t> receive(polymath::socketcan::CanFrame & can_frame);
std::optional<socket_error_string_t> receive(polymath::socketcan::CanFrame & can_frame) override;

/// @brief Receive returns the received CanFrame
/// @return optional CanFrame
Expand All @@ -87,30 +95,54 @@ class AxiomaticAdapter : public std::enable_shared_from_this<AxiomaticAdapter>

/// @brief Start a reception thread (calls callback)
/// @return success on started
bool startReceptionThread();
bool startReceptionThread() override;

/// @brief Stop and join reception thread
/// @param timeout_s INPUT timeout in seconds, <=0 means no timeout
/// @brief Stop and join reception thread (legacy ms-based overload).
/// @param timeout_s INPUT timeout in milliseconds, <=0 means no timeout
/// @return success on closed and joined thread
bool joinReceptionThread(const std::chrono::milliseconds & timeout_s = AxiomaticAdapter::JOIN_RECEPTION_TIMEOUT_MS);

/// @brief Stop and join reception thread (ICanBackend interface signature).
/// @param timeout_s INPUT timeout as a chrono::duration<float>; cast to ms internally.
/// @return success on closed and joined thread
bool joinReceptionThread(const std::chrono::duration<float> & timeout_s) override;

/// @brief Transmit a can frame via socket
/// @param frame INPUT const reference to the frame
/// @return optional error string filled with an error message if any
std::optional<socket_error_string_t> send(const polymath::socketcan::CanFrame & frame);
std::optional<socket_error_string_t> send(const polymath::socketcan::CanFrame & frame) override;

/// @brief Transmit a can frame via socket
/// @param frame INPUT shared_ptr to a frame to send (ICanBackend overload).
/// @return optional error string filled with an error message if any
std::optional<socket_error_string_t> send(const std::shared_ptr<const polymath::socketcan::CanFrame> frame) override;

/// @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<socket_error_string_t> send(const can_frame & frame);

/// @brief Get state of socket
/// @return TCPSocketState data type detailing OPEN or CLOSED
TCPSocketState get_socket_state();
/// @return SocketState data type detailing OPEN or CLOSED
///
/// (TCPSocketState is now an alias of polymath::socketcan::SocketState, so
/// callers that hold the result as TCPSocketState continue to work.)
polymath::socketcan::SocketState get_socket_state() override;

/// @brief Checks if the receive thread is running
/// @return True if the thread is running, false otherwise
bool is_thread_running();
bool is_thread_running() override;

/// @brief Set the receive callback after construction (ICanBackend interface).
/// Set BEFORE calling startReceptionThread() to avoid racing the rx thread.
/// @return true on success.
bool setOnReceiveCallback(
std::function<void(std::unique_ptr<const polymath::socketcan::CanFrame> frame)> && callback_function) override;

/// @brief Set the error callback after construction (ICanBackend interface).
/// Set BEFORE calling startReceptionThread() to avoid racing the rx thread.
/// @return true on success.
bool setOnErrorCallback(std::function<void(socket_error_string_t error)> && callback_function) override;

private:
/// @brief use Implemention (pimpl) to avoid including boost/asio.hpp in header + linking in CMake
Expand Down
48 changes: 47 additions & 1 deletion axiomatic_adapter/src/axiomatic_adapter.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,7 @@
#include <atomic>
#include <future>
#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
Expand Down Expand Up @@ -345,6 +346,22 @@ class AxiomaticAdapter::AxiomaticAdapterImpl
return thread_running_;
}

// Runtime callback setters — added 2026-05 for the ICanBackend retrofit.
// Mirrors SocketcanAdapter's behavior: plain assignment, no synchronization.
// The convention is "set before startReceptionThread()" to avoid racing the
// rx thread.
bool setOnReceiveCallback(std::function<void(std::unique_ptr<const polymath::socketcan::CanFrame> frame)> && cb)
{
receive_callback_ = std::move(cb);
return true;
}

bool setOnErrorCallback(std::function<void(AxiomaticAdapter::socket_error_string_t error)> && cb)
{
error_callback_ = std::move(cb);
return true;
}

private:
static constexpr std::array<uint8_t, 7> AXIOMATIC_CAN_MESSAGE_HEADER = {'A', 'X', 'I', 'O', 0xBA, 0x36, 0x01};
static constexpr std::chrono::milliseconds TCP_IP_CONNECTION_TIMEOUT_MS{3000};
Expand Down Expand Up @@ -429,7 +446,7 @@ std::optional<AxiomaticAdapter::socket_error_string_t> AxiomaticAdapter::send(co
return send(polymath::socketcan::CanFrame(frame));
}

TCPSocketState AxiomaticAdapter::get_socket_state()
polymath::socketcan::SocketState AxiomaticAdapter::get_socket_state()
{
return pimpl_->get_socket_state();
}
Expand All @@ -439,5 +456,34 @@ bool AxiomaticAdapter::is_thread_running()
return pimpl_->is_thread_running();
}

// ---------------------------------------------------------------------------
// ICanBackend bridge methods — added 2026-05 for the AxiomaticAdapter retrofit.
// ---------------------------------------------------------------------------

bool AxiomaticAdapter::joinReceptionThread(const std::chrono::duration<float> & timeout_s)
{
return pimpl_->joinReceptionThread(std::chrono::duration_cast<std::chrono::milliseconds>(timeout_s));
}

std::optional<AxiomaticAdapter::socket_error_string_t> AxiomaticAdapter::send(
const std::shared_ptr<const polymath::socketcan::CanFrame> frame)
{
if (!frame) {
return std::optional<socket_error_string_t>{"send: null frame"};
}
return pimpl_->send(*frame);
}

bool AxiomaticAdapter::setOnReceiveCallback(
std::function<void(std::unique_ptr<const polymath::socketcan::CanFrame> frame)> && callback_function)
{
return pimpl_->setOnReceiveCallback(std::move(callback_function));
}

bool AxiomaticAdapter::setOnErrorCallback(std::function<void(socket_error_string_t error)> && callback_function)
{
return pimpl_->setOnErrorCallback(std::move(callback_function));
}

} // namespace can
} // namespace polymath
110 changes: 110 additions & 0 deletions socketcan_adapter/include/socketcan_adapter/i_can_backend.hpp
Original file line number Diff line number Diff line change
@@ -0,0 +1,110 @@
// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef SOCKETCAN_ADAPTER__I_CAN_BACKEND_HPP_
#define SOCKETCAN_ADAPTER__I_CAN_BACKEND_HPP_

#include <chrono>
#include <functional>
#include <memory>
#include <optional>
#include <string>

#include "socketcan_adapter/can_frame.hpp"

namespace polymath::socketcan
{

/// @brief State of the underlying transport (socket, TCP/UDP connection, etc).
/// SocketState was previously the SocketcanAdapter-only enum; lifting it here
/// so any backend (Linux SocketCAN, Axiomatic Ethernet/CAN, future variants)
/// reports its open/closed/error state through one type.
enum class SocketState; // forward — defined in socketcan_adapter.hpp

/// @brief FilterMode for how to add filters
enum class FilterMode; // forward — defined in socketcan_adapter.hpp

/// @class polymath::socketcan::ICanBackend
/// @brief Cross-cutting interface implemented by every CAN transport backend.
///
/// Carved out of SocketcanAdapter's public surface in 2026-05 to allow
/// non-SocketCAN backends (e.g. the Axiomatic AX140970 Dual CAN FD over
/// Ethernet converter) to plug into the same ROS2 bridge node without forking
/// the runtime path. Linux-specific knobs (CAN_RAW_FILTER vector, ERR mask,
/// JOIN flag) stay on the concrete SocketcanAdapter — they have no analog in
/// the Axiomatic protocol and forcing every backend to no-op them muddies
/// the contract.
///
/// All implementations must:
/// * be safe to construct without opening any sockets (open happens in
/// openSocket()),
/// * tolerate openSocket()/closeSocket() being called multiple times,
/// * deliver received frames to the on-receive callback set by
/// setOnReceiveCallback() once startReceptionThread() has been called.
class ICanBackend
{
public:
/// @brief Mapped to std lib, but should be remapped to Polymath Safety compatible versions
using socket_error_string_t = std::string;

virtual ~ICanBackend() = default;

// ---------------------------------------------------------------------------
// Lifecycle
// ---------------------------------------------------------------------------

/// @brief Open the underlying transport. Returns true on success.
virtual bool openSocket() = 0;

/// @brief Close the underlying transport. Returns true on success.
virtual bool closeSocket() = 0;

/// @brief Get the current transport state.
virtual SocketState get_socket_state() = 0;

// ---------------------------------------------------------------------------
// Send / receive
// ---------------------------------------------------------------------------

/// @brief Transmit a frame.
/// @param frame INPUT shared_ptr to a frame to send.
/// @return optional error string if any.
virtual std::optional<socket_error_string_t> send(const std::shared_ptr<const CanFrame> frame) = 0;

/// @brief Transmit a frame.
virtual std::optional<socket_error_string_t> send(const CanFrame & frame) = 0;

/// @brief Block-receive a frame into `frame`. Subject to the receive timeout
/// configured via the constructor / set_receive_timeout().
/// @return optional error string if any.
virtual std::optional<socket_error_string_t> receive(CanFrame & frame) = 0;

// ---------------------------------------------------------------------------
// Reception thread + callbacks
// ---------------------------------------------------------------------------

virtual bool startReceptionThread() = 0;

virtual bool joinReceptionThread(const std::chrono::duration<float> & timeout_s) = 0;

virtual bool is_thread_running() = 0;

virtual bool setOnReceiveCallback(std::function<void(std::unique_ptr<const CanFrame> frame)> && callback) = 0;

virtual bool setOnErrorCallback(std::function<void(socket_error_string_t error)> && callback) = 0;
};

} // namespace polymath::socketcan

#endif // SOCKETCAN_ADAPTER__I_CAN_BACKEND_HPP_
Loading
Loading