From 9fa924b29f9cf70f2e22439fd6c18bf480fd5062 Mon Sep 17 00:00:00 2001 From: Ryan Bahl Date: Wed, 6 May 2026 22:12:04 +0000 Subject: [PATCH 1/6] Allow sygnal socket can interface to handle any number of MCMs --- .../sygnal_interface_socketcan.hpp | 15 +++-- .../sygnal_mcm_interface.hpp | 10 +++ .../src/sygnal_interface_socketcan.cpp | 51 ++++++--------- .../test/sygnal_interface_socketcan_test.cpp | 62 ++++++++++++++++++- .../sygnal_can_interface_node.hpp | 4 +- .../src/sygnal_can_interface_node.cpp | 25 ++++++-- .../src/sygnal_can_interface_params.yaml | 12 ++++ 7 files changed, 133 insertions(+), 46 deletions(-) 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..c07db41 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,7 @@ #include "sygnal_can_interface_lib/sygnal_interface_socketcan.hpp" +#include #include #include #include @@ -21,27 +22,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 +84,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..6bfbd62 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 @@ -159,7 +159,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 kDefaultMcmIds{ + {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, kDefaultMcmIds); std::promise frame_received_promise; std::future frame_received_future = frame_received_promise.get_future(); @@ -371,3 +378,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..7694b08 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 @@ -122,7 +122,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..32f7c33 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 @@ -69,8 +69,20 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } + // Build the flat MCM endpoint list from parallel params + if (params_.mcm_bus_addresses.size() != params_.mcm_subsystem_ids.size()) { + RCLCPP_ERROR(get_logger(), "mcm_bus_addresses and mcm_subsystem_ids must be the same length"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + std::vector mcm_ids; + mcm_ids.reserve(params_.mcm_bus_addresses.size()); + for (size_t i = 0; i < params_.mcm_bus_addresses.size(); ++i) { + mcm_ids.push_back( + {static_cast(params_.mcm_bus_addresses[i]), static_cast(params_.mcm_subsystem_ids[i])}); + } + // 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 +97,12 @@ 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_.clear(); + 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..e88b511 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,15 @@ sygnal_can_interface_ros2: description: "Command response timeout in milliseconds" validation: gt<>: [0] + mcm_bus_addresses: + type: int_array + default_value: [1, 1, 2, 2] + description: "CAN bus address for each configured MCM endpoint, parallel to mcm_subsystem_ids." + validation: + element_bounds<>: [0, 255] + mcm_subsystem_ids: + type: int_array + default_value: [0, 1, 0, 1] + description: "Subsystem ID for each configured MCM endpoint, parallel to mcm_bus_addresses." + validation: + element_bounds<>: [0, 255] From ec51a3559fd4492eea9bae32df68a79202524029 Mon Sep 17 00:00:00 2001 From: Ryan Bahl Date: Wed, 6 May 2026 22:22:11 +0000 Subject: [PATCH 2/6] fix pre-commit errors --- .../sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp | 1 + .../test/sygnal_interface_socketcan_test.cpp | 1 + .../sygnal_can_interface_ros2/src/sygnal_can_interface_node.cpp | 1 + 3 files changed, 3 insertions(+) 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 c07db41..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 @@ -15,6 +15,7 @@ #include "sygnal_can_interface_lib/sygnal_interface_socketcan.hpp" #include +#include #include #include #include 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 6bfbd62..d2731a1 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" 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 32f7c33..30f4d83 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 From 4428314564c652c37075e23c2f33f4b981b604fe Mon Sep 17 00:00:00 2001 From: Ryan Bahl Date: Thu, 7 May 2026 16:25:34 +0000 Subject: [PATCH 3/6] Change constant formatting in tests --- .../test/sygnal_interface_socketcan_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 d2731a1..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 @@ -160,14 +160,14 @@ TEST_CASE("SygnalInterfaceSocketcan vcan0 integration tests", "[vcan]") REQUIRE(interface_adapter->openSocket()); REQUIRE(simulator_adapter->openSocket()); - const std::vector kDefaultMcmIds{ + 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, kDefaultMcmIds); + std::make_unique(interface_adapter, DEFAULT_MCM_IDS); std::promise frame_received_promise; std::future frame_received_future = frame_received_promise.get_future(); From 567266a621194a2ebf8f800d1df89efcc8edbdb5 Mon Sep 17 00:00:00 2001 From: Ryan Bahl Date: Thu, 7 May 2026 16:51:14 +0000 Subject: [PATCH 4/6] have the sygnal interface node take in a string list of MCMs to define their bus and subsystem ids --- .../src/sygnal_can_interface_node.cpp | 29 +++++++++++++------ .../src/sygnal_can_interface_params.yaml | 16 ++++------ 2 files changed, 25 insertions(+), 20 deletions(-) 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 30f4d83..70d4398 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 @@ -70,16 +70,27 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } - // Build the flat MCM endpoint list from parallel params - if (params_.mcm_bus_addresses.size() != params_.mcm_subsystem_ids.size()) { - RCLCPP_ERROR(get_logger(), "mcm_bus_addresses and mcm_subsystem_ids must be the same length"); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } + // Parse ":" strings from params into a typed McmId list std::vector mcm_ids; - mcm_ids.reserve(params_.mcm_bus_addresses.size()); - for (size_t i = 0; i < params_.mcm_bus_addresses.size(); ++i) { - mcm_ids.push_back( - {static_cast(params_.mcm_bus_addresses[i]), static_cast(params_.mcm_subsystem_ids[i])}); + mcm_ids.reserve(params_.mcm_endpoints.size()); + for (const auto & endpoint : params_.mcm_endpoints) { + const auto colon = endpoint.find(':'); + if (std::string::npos == colon) { + RCLCPP_ERROR(get_logger(), "Invalid mcm_endpoints entry '%s': missing ':'", endpoint.c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + try { + const auto bus = std::stoi(endpoint.substr(0, colon)); + const auto sub = std::stoi(endpoint.substr(colon + 1)); + if (bus < 0 || bus > 255 || sub < 0 || sub > 255) { + RCLCPP_ERROR(get_logger(), "Invalid mcm_endpoints entry '%s': values must fit in uint8", endpoint.c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + mcm_ids.push_back({static_cast(bus), static_cast(sub)}); + } catch (const std::exception & e) { + RCLCPP_ERROR(get_logger(), "Invalid mcm_endpoints entry '%s': %s", endpoint.c_str(), e.what()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } } // Initialize Sygnal Interface SocketCAN controller 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 e88b511..0257f5c 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,15 +15,9 @@ sygnal_can_interface_ros2: description: "Command response timeout in milliseconds" validation: gt<>: [0] - mcm_bus_addresses: - type: int_array - default_value: [1, 1, 2, 2] - description: "CAN bus address for each configured MCM endpoint, parallel to mcm_subsystem_ids." + mcm_endpoints: + type: string_array + default_value: ["1:0", "1:1", "2:0", "2:1"] + description: "MCM endpoints, each formatted as ':'." validation: - element_bounds<>: [0, 255] - mcm_subsystem_ids: - type: int_array - default_value: [0, 1, 0, 1] - description: "Subsystem ID for each configured MCM endpoint, parallel to mcm_bus_addresses." - validation: - element_bounds<>: [0, 255] + element_pattern<>: ['^\d+:\d+$'] From 0857dfa531e6733a73f06a3915614fbd2b0455b4 Mon Sep 17 00:00:00 2001 From: Ryan Bahl Date: Thu, 7 May 2026 18:31:29 +0000 Subject: [PATCH 5/6] fix precommit errors and change : to - for string separator --- .../src/sygnal_can_interface_node.cpp | 12 ++++++------ .../src/sygnal_can_interface_params.yaml | 6 ++---- 2 files changed, 8 insertions(+), 10 deletions(-) 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 70d4398..94e77dd 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 @@ -70,18 +70,18 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } - // Parse ":" strings from params into a typed McmId list + // Parse "-" strings from params into a typed McmId list std::vector mcm_ids; mcm_ids.reserve(params_.mcm_endpoints.size()); for (const auto & endpoint : params_.mcm_endpoints) { - const auto colon = endpoint.find(':'); - if (std::string::npos == colon) { - RCLCPP_ERROR(get_logger(), "Invalid mcm_endpoints entry '%s': missing ':'", endpoint.c_str()); + const auto separator = endpoint.find('-'); + if (std::string::npos == separator) { + RCLCPP_ERROR(get_logger(), "Invalid mcm_endpoints entry '%s': missing '-'", endpoint.c_str()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } try { - const auto bus = std::stoi(endpoint.substr(0, colon)); - const auto sub = std::stoi(endpoint.substr(colon + 1)); + const auto bus = std::stoi(endpoint.substr(0, separator)); + const auto sub = std::stoi(endpoint.substr(separator + 1)); if (bus < 0 || bus > 255 || sub < 0 || sub > 255) { RCLCPP_ERROR(get_logger(), "Invalid mcm_endpoints entry '%s': values must fit in uint8", endpoint.c_str()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; 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 0257f5c..e0d9d93 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 @@ -17,7 +17,5 @@ sygnal_can_interface_ros2: gt<>: [0] mcm_endpoints: type: string_array - default_value: ["1:0", "1:1", "2:0", "2:1"] - description: "MCM endpoints, each formatted as ':'." - validation: - element_pattern<>: ['^\d+:\d+$'] + default_value: ["1-0", "1-1", "2-0", "2-1"] + description: "MCM endpoints, each formatted as '-'." From 816e85f5702482c6bb13d0903671f1feb46204e5 Mon Sep 17 00:00:00 2001 From: Ryan Bahl Date: Thu, 7 May 2026 19:07:20 +0000 Subject: [PATCH 6/6] hopefully fix precommit, move node towards RAII, and make mcm ros param read_only --- .../sygnal_can_interface_node.hpp | 3 +- .../src/sygnal_can_interface_node.cpp | 71 ++++++++++--------- .../src/sygnal_can_interface_params.yaml | 5 +- 3 files changed, 43 insertions(+), 36 deletions(-) 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 7694b08..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 @@ -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_; 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 94e77dd..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 @@ -34,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() { @@ -55,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,31 +99,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } - // Parse "-" strings from params into a typed McmId list - std::vector mcm_ids; - mcm_ids.reserve(params_.mcm_endpoints.size()); - for (const auto & endpoint : params_.mcm_endpoints) { - const auto separator = endpoint.find('-'); - if (std::string::npos == separator) { - RCLCPP_ERROR(get_logger(), "Invalid mcm_endpoints entry '%s': missing '-'", endpoint.c_str()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } - try { - const auto bus = std::stoi(endpoint.substr(0, separator)); - const auto sub = std::stoi(endpoint.substr(separator + 1)); - if (bus < 0 || bus > 255 || sub < 0 || sub > 255) { - RCLCPP_ERROR(get_logger(), "Invalid mcm_endpoints entry '%s': values must fit in uint8", endpoint.c_str()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } - mcm_ids.push_back({static_cast(bus), static_cast(sub)}); - } catch (const std::exception & e) { - RCLCPP_ERROR(get_logger(), "Invalid mcm_endpoints entry '%s': %s", endpoint.c_str(), e.what()); - return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; - } - } - // Initialize Sygnal Interface SocketCAN controller - sygnal_interface_ = std::make_unique(socketcan_adapter_, mcm_ids); + sygnal_interface_ = std::make_unique(socketcan_adapter_, mcm_ids_); // Set up callback to parse incoming messages socketcan_adapter_->setOnReceiveCallback( @@ -110,9 +116,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal diagnostics_pub_ = create_publisher("/diagnostics", rclcpp::QoS(10)); // Initialize MCM heartbeat publishers from configured endpoints - mcm_heartbeat_entries_.clear(); - mcm_heartbeat_entries_.reserve(mcm_ids.size()); - for (const auto & id : mcm_ids) { + mcm_heartbeat_entries_.reserve(mcm_ids_.size()); + for (const auto & id : mcm_ids_) { mcm_heartbeat_entries_.push_back({id.bus_id, id.subsystem_id, {}, {}}); } 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 e0d9d93..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 @@ -17,5 +17,6 @@ sygnal_can_interface_ros2: gt<>: [0] mcm_endpoints: type: string_array - default_value: ["1-0", "1-1", "2-0", "2-1"] - description: "MCM endpoints, each formatted as '-'." + default_value: [1-0, 1-1, 2-0, 2-1] + description: MCM endpoints, each formatted as '-'. + read_only: true