Allow sygnal socket can interface to handle any number of MCMs

sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp

@@ -21,6 +21,7 @@
 #include <optional>
 #include <queue>
 #include <string>
+#include <vector>
 
 #include "socketcan_adapter/socketcan_adapter.hpp"
 #include "sygnal_can_interface_lib/sygnal_command_interface.hpp"
@@ -36,11 +37,11 @@ struct SendCommandResult
   std::optional<std::future<SygnalControlCommandResponse>> 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::SocketcanAdapter> socketcan_adapter);
+  /// @param mcm_ids Flat list of MCM endpoints to manage, each identified by bus and subsystem ID
+  SygnalInterfaceSocketcan(
+    std::shared_ptr<socketcan::SocketcanAdapter> socketcan_adapter, const std::vector<McmId> & 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::SocketcanAdapter> socketcan_adapter_;
-  std::array<McmSystem, 2> mcm_systems_;  // Assuming max 2 MCMs for now, indexed by bus address
+  std::vector<SygnalMcmInterface> mcms_;
   SygnalControlInterface control_interface_;
 
   // Promise queues for each response type

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_;

sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp

@@ -14,34 +14,31 @@
 
 #include "sygnal_can_interface_lib/sygnal_interface_socketcan.hpp"
 
+#include <algorithm>
+#include <memory>
 #include <string>
 #include <utility>
 #include <vector>
 
 namespace polymath::sygnal
 {
 
-SygnalInterfaceSocketcan::SygnalInterfaceSocketcan(std::shared_ptr<socketcan::SocketcanAdapter> socketcan_adapter)
+SygnalInterfaceSocketcan::SygnalInterfaceSocketcan(
+  std::shared_ptr<socketcan::SocketcanAdapter> socketcan_adapter, const std::vector<McmId> & 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<std::array<SygnalSystemState, 5>> 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<SygnalSystemState> 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;
 }

sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_test.cpp

@@ -30,6 +30,7 @@ using Catch::Approx;
 #include <algorithm>
 #include <future>
 #include <string>
+#include <vector>
 
 #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<polymath::sygnal::SygnalInterfaceSocketcan>(interface_adapter);
+  const std::vector<polymath::sygnal::McmId> 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<polymath::sygnal::SygnalInterfaceSocketcan>(interface_adapter, DEFAULT_MCM_IDS);
 
   std::promise<void> frame_received_promise;
   std::future<void> 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<polymath::socketcan::SocketcanAdapter>(VCAN_INTERFACE);
+  auto simulator_adapter = std::make_shared<polymath::socketcan::SocketcanAdapter>(VCAN_INTERFACE);
+
+  REQUIRE(interface_adapter->openSocket());
+  REQUIRE(simulator_adapter->openSocket());
+
+  // Single bus with non-contiguous subsystem IDs
+  const std::vector<polymath::sygnal::McmId> kCustomMcmIds{
+    {1, 0},
+    {1, 2},
+    {1, 5},
+  };
+  auto sygnal_interface =
+    std::make_unique<polymath::sygnal::SygnalInterfaceSocketcan>(interface_adapter, kCustomMcmIds);
+
+  std::promise<void> frame_received_promise;
+  std::future<void> frame_received_future = frame_received_promise.get_future();
+
+  REQUIRE(interface_adapter->setOnReceiveCallback(
+    [&sygnal_interface, &frame_received_promise](std::unique_ptr<const polymath::socketcan::CanFrame> 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<uint8_t>(polymath::sygnal::SygnalSystemState::MCM_CONTROL);
+    std::array<uint8_t, 5> 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();
+}

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 <array>
 #include <chrono>
 #include <memory>
 #include <optional>
 #include <string>
 #include <thread>
+#include <vector>
 
 #include <sygnal_can_interface_ros2/sygnal_can_interface_params.hpp>
 
@@ -98,8 +98,9 @@ class SygnalCanInterfaceNode : public rclcpp_lifecycle::LifecycleNode
   diagnostic_msgs::msg::DiagnosticArray createDiagnosticsMessage();
 
   // Parameters
-  std::shared_ptr<sygnal_can_interface_ros2::ParamListener> param_listener_;
+  sygnal_can_interface_ros2::ParamListener param_listener_;
   sygnal_can_interface_ros2::Params params_;
+  const std::vector<polymath::sygnal::McmId> mcm_ids_;
 
   // SocketCAN and Sygnal components
   std::shared_ptr<polymath::socketcan::SocketcanAdapter> socketcan_adapter_;
@@ -122,7 +123,7 @@ class SygnalCanInterfaceNode : public rclcpp_lifecycle::LifecycleNode
 
   // Publishers
   rclcpp_lifecycle::LifecyclePublisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_;
-  std::array<McmHeartbeatEntry, 4> mcm_heartbeat_entries_;
+  std::vector<McmHeartbeatEntry> mcm_heartbeat_entries_;
 
   rclcpp::Subscription<sygnal_can_msgs::msg::ControlCommand>::SharedPtr control_command_sub_;
 };

sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_node.cpp

@@ -21,6 +21,7 @@
 #include <optional>
 #include <string>
 #include <utility>
+#include <vector>
 
 #if __has_include(<magic_enum/magic_enum.hpp>)
   #include <magic_enum/magic_enum.hpp>
@@ -33,15 +34,45 @@
 #include "sygnal_can_interface_lib/sygnal_command_interface.hpp"
 #include "sygnal_can_interface_lib/sygnal_mcm_interface.hpp"
 
+namespace
+{
+
+template <typename T, typename U, typename F>
+static std::vector<T> iife_vector(const std::vector<U> & input, F transform)
+{
+  std::vector<T> 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<uint8_t>(bus), static_cast<uint8_t>(sub)};
+}
+
+}  // namespace
+
 namespace polymath::sygnal
 {
 
 SygnalCanInterfaceNode::SygnalCanInterfaceNode(const rclcpp::NodeOptions & options)
 : rclcpp_lifecycle::LifecycleNode("sygnal_can_interface_node", "", options)
-{
-  param_listener_ = std::make_shared<sygnal_can_interface_ros2::ParamListener>(get_node_parameters_interface());
-  params_ = param_listener_->get_params();
-}
+, param_listener_(get_node_parameters_interface())
+, params_(param_listener_.get_params())
+, mcm_ids_(iife_vector<McmId>(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<polymath::sygnal::SygnalInterfaceSocketcan>(socketcan_adapter_);
+    sygnal_interface_ = std::make_unique<polymath::sygnal::SygnalInterfaceSocketcan>(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<diagnostic_msgs::msg::DiagnosticArray>("/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 =

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 '<bus_id>-<subsystem_id>'.
+    read_only: true