diff --git a/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp index f97a739..88e3ff5 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp @@ -21,6 +21,7 @@ #include #include #include +#include #include "socketcan_adapter/socketcan_adapter.hpp" #include "sygnal_can_interface_lib/sygnal_command_interface.hpp" @@ -36,11 +37,11 @@ struct SendCommandResult std::optional> response_future; }; -struct McmSystem +/// @brief Identifies one MCM endpoint by its CAN bus and subsystem ID. +struct McmId { - uint8_t bus_address; - SygnalMcmInterface mcm_0; - SygnalMcmInterface mcm_1; + uint8_t bus_id; + uint8_t subsystem_id; }; constexpr uint32_t MAX_PROMISE_QUEUE_LENGTH = 100; @@ -52,7 +53,9 @@ class SygnalInterfaceSocketcan public: /// @brief Constructor /// @param socketcan_adapter Shared pointer to socketcan adapter for CAN communication - explicit SygnalInterfaceSocketcan(std::shared_ptr socketcan_adapter); + /// @param mcm_ids Flat list of MCM endpoints to manage, each identified by bus and subsystem ID + SygnalInterfaceSocketcan( + std::shared_ptr socketcan_adapter, const std::vector & mcm_ids); /// @brief Parse incoming CAN frame for MCM heartbeat and command responses /// @param frame CAN frame to parse @@ -107,7 +110,7 @@ class SygnalInterfaceSocketcan private: std::shared_ptr socketcan_adapter_; - std::array mcm_systems_; // Assuming max 2 MCMs for now, indexed by bus address + std::vector mcms_; SygnalControlInterface control_interface_; // Promise queues for each response type diff --git a/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_mcm_interface.hpp b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_mcm_interface.hpp index 7d6d096..d0f0cb7 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_mcm_interface.hpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_mcm_interface.hpp @@ -73,6 +73,16 @@ class SygnalMcmInterface return sygnal_interface_states_; } + uint8_t get_bus_address() const + { + return bus_address_; + } + + uint8_t get_subsystem_id() const + { + return subsystem_id_; + } + SygnalSystemState get_mcm_state() const { return sygnal_mcm_state_; diff --git a/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp index 2a5ec25..a7ac2c2 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp @@ -14,6 +14,8 @@ #include "sygnal_can_interface_lib/sygnal_interface_socketcan.hpp" +#include +#include #include #include #include @@ -21,27 +23,22 @@ namespace polymath::sygnal { -SygnalInterfaceSocketcan::SygnalInterfaceSocketcan(std::shared_ptr socketcan_adapter) +SygnalInterfaceSocketcan::SygnalInterfaceSocketcan( + std::shared_ptr socketcan_adapter, const std::vector & mcm_ids) : socketcan_adapter_(socketcan_adapter) , control_interface_() { - mcm_systems_[0].bus_address = DEFAULT_MCM_BUS_ADDRESS; - mcm_systems_[0].mcm_0 = SygnalMcmInterface(mcm_systems_[0].bus_address, 0); - mcm_systems_[0].mcm_1 = SygnalMcmInterface(mcm_systems_[0].bus_address, 1); - - mcm_systems_[1].bus_address = SECONDARY_MCM_BUS_ADDRESS; - mcm_systems_[1].mcm_0 = SygnalMcmInterface(mcm_systems_[1].bus_address, 0); - mcm_systems_[1].mcm_1 = SygnalMcmInterface(mcm_systems_[1].bus_address, 1); + mcms_.reserve(mcm_ids.size()); + for (const auto & id : mcm_ids) { + mcms_.emplace_back(id.bus_id, id.subsystem_id); + } } bool SygnalInterfaceSocketcan::parse(const socketcan::CanFrame & frame) { - // Try parsing as MCM heartbeat (both interfaces will check subsystem_id) - for (auto & mcm_system : mcm_systems_) { - if (mcm_system.mcm_0.parseMcmHeartbeatFrame(frame)) { - return true; - } - if (mcm_system.mcm_1.parseMcmHeartbeatFrame(frame)) { + // Try parsing as MCM heartbeat (each interface checks its own bus/subsystem_id) + for (auto & mcm : mcms_) { + if (mcm.parseMcmHeartbeatFrame(frame)) { return true; } } @@ -88,30 +85,23 @@ bool SygnalInterfaceSocketcan::parse(const socketcan::CanFrame & frame) std::optional> SygnalInterfaceSocketcan::get_interface_state( const uint8_t bus_address, const uint8_t subsystem_id) const { - for (const auto & mcm_system : mcm_systems_) { - if (mcm_system.bus_address == bus_address) { - if (subsystem_id == 0) { - return mcm_system.mcm_0.get_interface_states(); - } else if (subsystem_id == 1) { - return mcm_system.mcm_1.get_interface_states(); - } - } + auto it = std::find_if(mcms_.begin(), mcms_.end(), [bus_address, subsystem_id](const SygnalMcmInterface & m) { + return m.get_bus_address() == bus_address && m.get_subsystem_id() == subsystem_id; + }); + if (it != mcms_.end()) { + return it->get_interface_states(); } - return std::nullopt; } std::optional SygnalInterfaceSocketcan::get_sygnal_mcm_state( const uint8_t bus_address, const uint8_t subsystem_id) const { - for (const auto & mcm_system : mcm_systems_) { - if (mcm_system.bus_address == bus_address) { - if (subsystem_id == 0) { - return mcm_system.mcm_0.get_mcm_state(); - } else if (subsystem_id == 1) { - return mcm_system.mcm_1.get_mcm_state(); - } - } + auto it = std::find_if(mcms_.begin(), mcms_.end(), [bus_address, subsystem_id](const SygnalMcmInterface & m) { + return m.get_bus_address() == bus_address && m.get_subsystem_id() == subsystem_id; + }); + if (it != mcms_.end()) { + return it->get_mcm_state(); } return std::nullopt; } diff --git a/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_test.cpp b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_test.cpp index b1bea82..97522b0 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_test.cpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_test.cpp @@ -30,6 +30,7 @@ using Catch::Approx; #include #include #include +#include #include "socketcan_adapter/socketcan_adapter.hpp" #include "sygnal_can_interface_lib/crc8.hpp" @@ -159,7 +160,14 @@ TEST_CASE("SygnalInterfaceSocketcan vcan0 integration tests", "[vcan]") REQUIRE(interface_adapter->openSocket()); REQUIRE(simulator_adapter->openSocket()); - auto sygnal_interface = std::make_unique(interface_adapter); + const std::vector DEFAULT_MCM_IDS{ + {DEFAULT_MCM_BUS_ADDRESS, 0}, + {DEFAULT_MCM_BUS_ADDRESS, 1}, + {SECONDARY_MCM_BUS_ADDRESS, 0}, + {SECONDARY_MCM_BUS_ADDRESS, 1}, + }; + auto sygnal_interface = + std::make_unique(interface_adapter, DEFAULT_MCM_IDS); std::promise frame_received_promise; std::future frame_received_future = frame_received_promise.get_future(); @@ -371,3 +379,56 @@ TEST_CASE("SygnalInterfaceSocketcan vcan0 integration tests", "[vcan]") interface_adapter->closeSocket(); simulator_adapter->closeSocket(); } + +TEST_CASE("SygnalInterfaceSocketcan arbitrary MCM IDs", "[vcan]") +{ + auto interface_adapter = std::make_shared(VCAN_INTERFACE); + auto simulator_adapter = std::make_shared(VCAN_INTERFACE); + + REQUIRE(interface_adapter->openSocket()); + REQUIRE(simulator_adapter->openSocket()); + + // Single bus with non-contiguous subsystem IDs + const std::vector kCustomMcmIds{ + {1, 0}, + {1, 2}, + {1, 5}, + }; + auto sygnal_interface = + std::make_unique(interface_adapter, kCustomMcmIds); + + std::promise frame_received_promise; + std::future frame_received_future = frame_received_promise.get_future(); + + REQUIRE(interface_adapter->setOnReceiveCallback( + [&sygnal_interface, &frame_received_promise](std::unique_ptr frame) { + frame_received_promise.set_value(); + sygnal_interface->parse(*frame); + })); + + REQUIRE(interface_adapter->startReceptionThread()); + + SECTION("Heartbeat for configured subsystem ID 5 is parsed") + { + constexpr uint8_t bus_id = 1; + constexpr uint8_t subsystem_id = 5; + constexpr uint8_t system_state = static_cast(polymath::sygnal::SygnalSystemState::MCM_CONTROL); + std::array interface_states = {1, 1, 1, 1, 1}; + + auto heartbeat_frame = createHeartbeatFrame(bus_id, subsystem_id, system_state, interface_states); + REQUIRE_FALSE(simulator_adapter->send(heartbeat_frame).has_value()); + + frame_received_future.wait_for(std::chrono::seconds(2)); + + auto mcm_state_opt = sygnal_interface->get_sygnal_mcm_state(bus_id, subsystem_id); + REQUIRE(mcm_state_opt.has_value()); + REQUIRE(mcm_state_opt.value() == polymath::sygnal::SygnalSystemState::MCM_CONTROL); + + // Non-configured subsystem on same bus returns nullopt + REQUIRE_FALSE(sygnal_interface->get_sygnal_mcm_state(bus_id, 1).has_value()); + } + + interface_adapter->joinReceptionThread(); + interface_adapter->closeSocket(); + simulator_adapter->closeSocket(); +} diff --git a/sygnal_can_interface/sygnal_can_interface_ros2/include/sygnal_can_interface_ros2/sygnal_can_interface_node.hpp b/sygnal_can_interface/sygnal_can_interface_ros2/include/sygnal_can_interface_ros2/sygnal_can_interface_node.hpp index c0b06c8..81c12ff 100644 --- a/sygnal_can_interface/sygnal_can_interface_ros2/include/sygnal_can_interface_ros2/sygnal_can_interface_node.hpp +++ b/sygnal_can_interface/sygnal_can_interface_ros2/include/sygnal_can_interface_ros2/sygnal_can_interface_node.hpp @@ -15,12 +15,12 @@ #ifndef SYGNAL_CAN_INTERFACE_NODE__SYGNAL_CAN_INTERFACE_NODE_HPP_ #define SYGNAL_CAN_INTERFACE_NODE__SYGNAL_CAN_INTERFACE_NODE_HPP_ -#include #include #include #include #include #include +#include #include @@ -98,8 +98,9 @@ class SygnalCanInterfaceNode : public rclcpp_lifecycle::LifecycleNode diagnostic_msgs::msg::DiagnosticArray createDiagnosticsMessage(); // Parameters - std::shared_ptr param_listener_; + sygnal_can_interface_ros2::ParamListener param_listener_; sygnal_can_interface_ros2::Params params_; + const std::vector mcm_ids_; // SocketCAN and Sygnal components std::shared_ptr socketcan_adapter_; @@ -122,7 +123,7 @@ class SygnalCanInterfaceNode : public rclcpp_lifecycle::LifecycleNode // Publishers rclcpp_lifecycle::LifecyclePublisher::SharedPtr diagnostics_pub_; - std::array mcm_heartbeat_entries_; + std::vector mcm_heartbeat_entries_; rclcpp::Subscription::SharedPtr control_command_sub_; }; diff --git a/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_node.cpp b/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_node.cpp index e3d20e0..c190d57 100644 --- a/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_node.cpp +++ b/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_node.cpp @@ -21,6 +21,7 @@ #include #include #include +#include #if __has_include() #include @@ -33,15 +34,45 @@ #include "sygnal_can_interface_lib/sygnal_command_interface.hpp" #include "sygnal_can_interface_lib/sygnal_mcm_interface.hpp" +namespace +{ + +template +static std::vector iife_vector(const std::vector & input, F transform) +{ + std::vector result; + result.reserve(input.size()); + for (const auto & item : input) { + result.push_back(transform(item)); + } + return result; +} + +static polymath::sygnal::McmId parse_mcm_id(const std::string & endpoint) +{ + const auto sep = endpoint.find('-'); + if (std::string::npos == sep) { + throw std::invalid_argument("Invalid mcm_endpoints entry '" + endpoint + "': missing '-'"); + } + const int bus = std::stoi(endpoint.substr(0, sep)); + const int sub = std::stoi(endpoint.substr(sep + 1)); + if (0 > bus || bus > 255 || 0 > sub || sub > 255) { + throw std::invalid_argument("Invalid mcm_endpoints entry '" + endpoint + "': values must fit in uint8"); + } + return {static_cast(bus), static_cast(sub)}; +} + +} // namespace + namespace polymath::sygnal { SygnalCanInterfaceNode::SygnalCanInterfaceNode(const rclcpp::NodeOptions & options) : rclcpp_lifecycle::LifecycleNode("sygnal_can_interface_node", "", options) -{ - param_listener_ = std::make_shared(get_node_parameters_interface()); - params_ = param_listener_->get_params(); -} +, param_listener_(get_node_parameters_interface()) +, params_(param_listener_.get_params()) +, mcm_ids_(iife_vector(params_.mcm_endpoints, parse_mcm_id)) +{} SygnalCanInterfaceNode::~SygnalCanInterfaceNode() { @@ -54,8 +85,7 @@ SygnalCanInterfaceNode::~SygnalCanInterfaceNode() rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SygnalCanInterfaceNode::on_configure( const rclcpp_lifecycle::State &) { - // Get parameters (refresh in case they changed) - params_ = param_listener_->get_params(); + params_ = param_listener_.get_params(); RCLCPP_INFO( get_logger(), "Configuring Sygnal CAN Interface node with CAN interface: %s", params_.can_interface.c_str()); @@ -70,7 +100,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal } // Initialize Sygnal Interface SocketCAN controller - sygnal_interface_ = std::make_unique(socketcan_adapter_); + sygnal_interface_ = std::make_unique(socketcan_adapter_, mcm_ids_); // Set up callback to parse incoming messages socketcan_adapter_->setOnReceiveCallback( @@ -85,11 +115,11 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal // Create publishers diagnostics_pub_ = create_publisher("/diagnostics", rclcpp::QoS(10)); - // Initialize MCM heartbeat publishers for all 4 devices - mcm_heartbeat_entries_[0] = {DEFAULT_MCM_BUS_ADDRESS, 0, {}, {}}; - mcm_heartbeat_entries_[1] = {DEFAULT_MCM_BUS_ADDRESS, 1, {}, {}}; - mcm_heartbeat_entries_[2] = {SECONDARY_MCM_BUS_ADDRESS, 0, {}, {}}; - mcm_heartbeat_entries_[3] = {SECONDARY_MCM_BUS_ADDRESS, 1, {}, {}}; + // Initialize MCM heartbeat publishers from configured endpoints + mcm_heartbeat_entries_.reserve(mcm_ids_.size()); + for (const auto & id : mcm_ids_) { + mcm_heartbeat_entries_.push_back({id.bus_id, id.subsystem_id, {}, {}}); + } for (auto & entry : mcm_heartbeat_entries_) { std::string topic = diff --git a/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_params.yaml b/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_params.yaml index f6c4c3a..170be3f 100644 --- a/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_params.yaml +++ b/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_params.yaml @@ -15,3 +15,8 @@ sygnal_can_interface_ros2: description: "Command response timeout in milliseconds" validation: gt<>: [0] + mcm_endpoints: + type: string_array + default_value: [1-0, 1-1, 2-0, 2-1] + description: MCM endpoints, each formatted as '-'. + read_only: true