Add HPO support in SygnalInterfaceSocketcan via parse-ordering arbitration

sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt

@@ -125,6 +125,22 @@ if(BUILD_TESTING)
     WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
   )
 
+  add_executable(sygnal_interface_socketcan_routing_tests
+    test/sygnal_interface_socketcan_routing_test.cpp
+  )
+  target_link_libraries(sygnal_interface_socketcan_routing_tests
+    PRIVATE ${PROJECT_NAME} sygnal_dbc::sygnal_dbc Catch2::Catch2WithMain
+  )
+  ament_add_test(
+    sygnal_interface_socketcan_routing_tests
+    GENERATE_RESULT_FOR_RETURN_CODE_ZERO
+    COMMAND "$<TARGET_FILE:sygnal_interface_socketcan_routing_tests>"
+      -r junit -s
+      -o test_results/${PROJECT_NAME}/sygnal_interface_socketcan_routing_tests_output.xml
+    ENV CATCH_CONFIG_CONSOLE_WIDTH=120
+    WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
+  )
+
   if(DEFINED ENV{CAN_AVAILABLE})
     add_executable(sygnal_interface_socketcan_tests
       test/sygnal_interface_socketcan_test.cpp

sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_hpo_interface.hpp

@@ -25,7 +25,10 @@
 namespace polymath::sygnal
 {
 
-constexpr uint8_t HPO_NUM_INTERFACES = 7;
+// HPO hardware exposes 5 interfaces (0-4). The DBC defines 7 signals (0-6) but the
+// remaining two are unused / always zero on real boards; we ignore them, mirroring the
+// same DBC-vs-hardware mismatch already handled by the MCM heartbeat parser.
+constexpr uint8_t HPO_NUM_INTERFACES = 5;
 
 /// @brief Parsed HPO ControlEnableResponse / ControlCommandResponse.
 ///        is_enable_response disambiguates which payload is meaningful:

sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp

@@ -25,6 +25,7 @@
 
 #include "socketcan_adapter/socketcan_adapter.hpp"
 #include "sygnal_can_interface_lib/sygnal_command_interface.hpp"
+#include "sygnal_can_interface_lib/sygnal_hpo_interface.hpp"
 #include "sygnal_can_interface_lib/sygnal_mcm_interface.hpp"
 
 namespace polymath::sygnal
@@ -44,6 +45,20 @@ struct McmId
   uint8_t subsystem_id;
 };
 
+/// @brief Identifies one HPO endpoint by its CAN bus address.
+struct HpoId
+{
+  uint8_t bus_id;
+};
+
+/// @brief Result of an HPO send operation. Mirrors SendCommandResult but is typed for HpoControlResponse
+///        because the HPO response carries a `message_id` instead of MCM's interface_id/subsystem_id.
+struct SendHpoCommandResult
+{
+  bool success;
+  std::optional<std::future<HpoControlResponse>> response_future;
+};
+
 /// @brief Represents a single control interface in Sygnal's System.
 ///        Interfaces can either take floats(default) or ints as inputs.
 struct InterfaceEndpoint
@@ -76,8 +91,15 @@ class SygnalInterfaceSocketcan
   /// @brief Constructor
   /// @param socketcan_adapter Shared pointer to socketcan adapter for CAN communication
   /// @param mcm_ids Flat list of MCM endpoints to manage, each identified by bus and subsystem ID
+  /// @param hpo_ids Flat list of HPO endpoints to manage, each identified by bus ID. Empty by
+  ///                default; an empty list disables all HPO behavior. The constructor throws
+  ///                std::invalid_argument if any HPO bus_id collides with an MCM bus_id, since
+  ///                disjoint bus addresses are required to route CAN-ID-overloaded frames correctly
+  ///                (see parse() comments).
   SygnalInterfaceSocketcan(
-    std::shared_ptr<socketcan::SocketcanAdapter> socketcan_adapter, const std::vector<McmId> & mcm_ids);
+    std::shared_ptr<socketcan::SocketcanAdapter> socketcan_adapter,
+    const std::vector<McmId> & mcm_ids,
+    const std::vector<HpoId> & hpo_ids = {});
 
   /// @brief Parse incoming CAN frame for MCM heartbeat and command responses
   /// @param frame CAN frame to parse
@@ -157,15 +179,46 @@ class SygnalInterfaceSocketcan
   SendCommandResult sendRelayCommand(
     InterfaceEndpoint interface, bool relay_state, bool expect_reply, std::string & error_message);
 
+  /// @brief Send an HPO ControlEnable command.
+  /// @param bus_id Bus address of the target HPO. Must match one of the hpo_ids passed at construction.
+  /// @param message_id HPO MessageID (8-bit identifier of the specific signal/interface on the HPO).
+  /// @param enable true to grant HPO control of the signal, false to release back to human control.
+  /// @param expect_reply If true, returns a future for the ControlEnableResponse; fire-and-forget otherwise.
+  /// @param[out] error_message Populated on failure.
+  /// @return Result with success flag and optional response future.
+  SendHpoCommandResult sendHpoControlEnable(
+    uint8_t bus_id, uint8_t message_id, bool enable, bool expect_reply, std::string & error_message);
+
+  /// @brief Send an HPO ControlCommand with a float value.
+  /// @param bus_id Bus address of the target HPO. Must match one of the hpo_ids passed at construction.
+  /// @param message_id HPO MessageID (8-bit identifier of the specific signal/interface on the HPO).
+  /// @param value Control value (encoded per DBC SIG_VALTYPE float).
+  /// @param expect_reply If true, returns a future for the ControlCommandResponse; fire-and-forget otherwise.
+  /// @param[out] error_message Populated on failure.
+  /// @return Result with success flag and optional response future.
+  SendHpoCommandResult sendHpoControlCommand(
+    uint8_t bus_id, uint8_t message_id, double value, bool expect_reply, std::string & error_message);
+
+  /// @brief Read the cached interface bits from the named HPO's latest heartbeat.
+  /// @return std::nullopt if no HPO with this bus_address has been registered.
+  std::optional<std::array<bool, HPO_NUM_INTERFACES>> get_hpo_interface_states(uint8_t bus_address) const;
+
+  /// @brief Read the cached overall interface bit from the named HPO's latest heartbeat.
+  /// @return std::nullopt if no HPO with this bus_address has been registered.
+  std::optional<bool> get_hpo_overall_interface_state(uint8_t bus_address) const;
+
 private:
   std::shared_ptr<socketcan::SocketcanAdapter> socketcan_adapter_;
   std::vector<SygnalMcmInterface> mcms_;
+  std::vector<SygnalHpoInterface> hpos_;
   SygnalControlInterface control_interface_;
 
   // Promise queues for each response type
   std::queue<std::promise<SygnalControlCommandResponse>> enable_response_promises_;
   std::queue<std::promise<SygnalControlCommandResponse>> control_response_promises_;
   std::queue<std::promise<SygnalControlCommandResponse>> relay_response_promises_;
+  std::queue<std::promise<HpoControlResponse>> hpo_enable_response_promises_;
+  std::queue<std::promise<HpoControlResponse>> hpo_command_response_promises_;
 
   std::mutex promises_mutex_;
 };

sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_hpo_interface.cpp

@@ -72,8 +72,7 @@ bool SygnalHpoInterface::parseHeartbeatFrame(const socketcan::CanFrame & frame)
   hpo_interface_states_[2] = (0 != unpacked.interface2_state);
   hpo_interface_states_[3] = (0 != unpacked.interface3_state);
   hpo_interface_states_[4] = (0 != unpacked.interface4_state);
-  hpo_interface_states_[5] = (0 != unpacked.interface5_state);
-  hpo_interface_states_[6] = (0 != unpacked.interface6_state);
+  // Signals interface5_state / interface6_state are present in the DBC but unused on real HPO hardware.
   hpo_overall_interface_state_ = (0 != unpacked.overall_interface_state);
 
   return true;

sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp

@@ -16,6 +16,7 @@
 
 #include <algorithm>
 #include <memory>
+#include <stdexcept>
 #include <string>
 #include <utility>
 #include <vector>
@@ -24,26 +25,89 @@ namespace polymath::sygnal
 {
 
 SygnalInterfaceSocketcan::SygnalInterfaceSocketcan(
-  std::shared_ptr<socketcan::SocketcanAdapter> socketcan_adapter, const std::vector<McmId> & mcm_ids)
+  std::shared_ptr<socketcan::SocketcanAdapter> socketcan_adapter,
+  const std::vector<McmId> & mcm_ids,
+  const std::vector<HpoId> & hpo_ids)
 : socketcan_adapter_(socketcan_adapter)
 , control_interface_()
 {
+  // Sygnal overloads CAN IDs across MCM/HPO/IO devices: a single frame ID (e.g. 0x161 ControlCommandResponse)
+  // carries different byte layouts depending on which device type produced it. The only field with stable
+  // semantics across layouts is the 7-bit BusAddress. Per-board parsers filter by bus_address to claim only
+  // their own frames; the MCM control-response parser (SygnalControlInterface) does not filter, so it would
+  // misinterpret an HPO response if it ever saw one. We sidestep this by (a) running HPO parsers first in
+  // parse() so HPO frames never reach the MCM parser, and (b) requiring disjoint bus addresses so the
+  // ordering can correctly route by elimination. The second invariant is enforced here.
+  for (const auto & hpo : hpo_ids) {
+    for (const auto & mcm : mcm_ids) {
+      if (hpo.bus_id == mcm.bus_id) {
+        throw std::invalid_argument(
+          "Bus address " + std::to_string(hpo.bus_id) +
+          " is assigned to both an MCM and an HPO; bus addresses must be disjoint across device types "
+          "until Sygnal CAN-ID overlap is resolved structurally.");
+      }
+    }
+  }
+
   mcms_.reserve(mcm_ids.size());
   for (const auto & id : mcm_ids) {
     mcms_.emplace_back(id.bus_id, id.subsystem_id);
   }
+
+  hpos_.reserve(hpo_ids.size());
+  for (const auto & id : hpo_ids) {
+    hpos_.emplace_back(id.bus_id);
+  }
 }
 
 bool SygnalInterfaceSocketcan::parse(const socketcan::CanFrame & frame)
 {
-  // Try parsing as MCM heartbeat (each interface checks its own bus/subsystem_id)
+  // DO NOT REORDER without re-reading the rationale in the constructor comment:
+  // HPO parsers MUST run before SygnalControlInterface::parseCommandResponseFrame because the latter does
+  // not filter by bus_address, and HPO control responses share CAN IDs (0x61 / 0x161) with MCM responses.
+  // Heartbeats and HPO error frames already filter by bus_address inside each per-board parser, so the
+  // order between MCM heartbeats and HPO heartbeats is arbitrary, but we keep the HPO block contiguous
+  // for clarity.
+
+  // --- HPO first ---
+  for (auto & hpo : hpos_) {
+    if (hpo.parseHeartbeatFrame(frame)) {
+      return true;
+    }
+  }
+
+  for (auto & hpo : hpos_) {
+    auto hpo_response = hpo.parseControlResponse(frame);
+    if (!hpo_response.has_value()) {
+      continue;
+    }
+    std::lock_guard<std::mutex> lock(promises_mutex_);
+    auto & queue = hpo_response->is_enable_response ? hpo_enable_response_promises_ : hpo_command_response_promises_;
+    if (!queue.empty()) {
+      auto promise = std::move(queue.front());
+      queue.pop();
+      promise.set_value(*hpo_response);
+    }
+    return true;
+  }
+
+  for (auto & hpo : hpos_) {
+    // Error frames are parsed (and CRC-checked) but not yet surfaced to callers; see design doc open
+    // question #2. Claim the frame so the MCM error path (if/when added) doesn't double-handle it.
+    if (hpo.parseErrorFrame(frame).has_value()) {
+      return true;
+    }
+  }
+
+  // --- MCM ---
   for (auto & mcm : mcms_) {
     if (mcm.parseMcmHeartbeatFrame(frame)) {
       return true;
     }
   }
 
-  // Try parsing as command response
+  // Try parsing as a (legacy unified) MCM command response. Safe to run last: every HPO-addressed frame
+  // has already been claimed above, so anything reaching this line is either an MCM frame or unrelated.
   auto response = control_interface_.parseCommandResponseFrame(frame);
   if (!response.has_value()) {
     return false;
@@ -229,4 +293,99 @@ SendCommandResult SygnalInterfaceSocketcan::sendRelayCommand(
   return sendRelayCommand(interface.bus_id, interface.subsystem_id, relay_state, expect_reply, error_message);
 }
 
+SendHpoCommandResult SygnalInterfaceSocketcan::sendHpoControlEnable(
+  uint8_t bus_id, uint8_t message_id, bool enable, bool expect_reply, std::string & error_message)
+{
+  auto it = std::find_if(
+    hpos_.begin(), hpos_.end(), [bus_id](const SygnalHpoInterface & h) { return h.get_bus_address() == bus_id; });
+  if (it == hpos_.end()) {
+    error_message += "No HPO registered at bus address " + std::to_string(bus_id) + "\n";
+    return {false, std::nullopt};
+  }
+
+  auto frame_opt = it->createControlEnableFrame(message_id, enable, error_message);
+  if (!frame_opt.has_value()) {
+    return {false, std::nullopt};
+  }
+
+  std::optional<std::future<HpoControlResponse>> future_opt;
+  if (expect_reply) {
+    std::promise<HpoControlResponse> promise;
+    future_opt = promise.get_future();
+    std::lock_guard<std::mutex> lock(promises_mutex_);
+    if (hpo_enable_response_promises_.size() >= MAX_PROMISE_QUEUE_LENGTH) {
+      hpo_enable_response_promises_.pop();
+    }
+    hpo_enable_response_promises_.push(std::move(promise));
+  }
+
+  auto err = socketcan_adapter_->send(*frame_opt);
+  if (err.has_value()) {
+    error_message += "Failed to send HPO control enable: " + err.value() + "\n";
+    // The promise was already pushed before the send attempt; surface the future regardless so the caller
+    // can choose to wait on it (or discard) instead of leaving a dangling future the harness can't observe.
+    return {false, std::move(future_opt)};
+  }
+
+  return {true, std::move(future_opt)};
+}
+
+SendHpoCommandResult SygnalInterfaceSocketcan::sendHpoControlCommand(
+  uint8_t bus_id, uint8_t message_id, double value, bool expect_reply, std::string & error_message)
+{
+  auto it = std::find_if(
+    hpos_.begin(), hpos_.end(), [bus_id](const SygnalHpoInterface & h) { return h.get_bus_address() == bus_id; });
+  if (it == hpos_.end()) {
+    error_message += "No HPO registered at bus address " + std::to_string(bus_id) + "\n";
+    return {false, std::nullopt};
+  }
+
+  auto frame_opt = it->createControlCommandFrame(message_id, value, error_message);
+  if (!frame_opt.has_value()) {
+    return {false, std::nullopt};
+  }
+
+  std::optional<std::future<HpoControlResponse>> future_opt;
+  if (expect_reply) {
+    std::promise<HpoControlResponse> promise;
+    future_opt = promise.get_future();
+    std::lock_guard<std::mutex> lock(promises_mutex_);
+    if (hpo_command_response_promises_.size() >= MAX_PROMISE_QUEUE_LENGTH) {
+      hpo_command_response_promises_.pop();
+    }
+    hpo_command_response_promises_.push(std::move(promise));
+  }
+
+  auto err = socketcan_adapter_->send(*frame_opt);
+  if (err.has_value()) {
+    error_message += "Failed to send HPO control command: " + err.value() + "\n";
+    return {false, std::move(future_opt)};
+  }
+
+  return {true, std::move(future_opt)};
+}
+
+std::optional<std::array<bool, HPO_NUM_INTERFACES>> SygnalInterfaceSocketcan::get_hpo_interface_states(
+  uint8_t bus_address) const
+{
+  auto it = std::find_if(hpos_.begin(), hpos_.end(), [bus_address](const SygnalHpoInterface & h) {
+    return h.get_bus_address() == bus_address;
+  });
+  if (it == hpos_.end()) {
+    return std::nullopt;
+  }
+  return it->get_interface_states();
+}
+
+std::optional<bool> SygnalInterfaceSocketcan::get_hpo_overall_interface_state(uint8_t bus_address) const
+{
+  auto it = std::find_if(hpos_.begin(), hpos_.end(), [bus_address](const SygnalHpoInterface & h) {
+    return h.get_bus_address() == bus_address;
+  });
+  if (it == hpos_.end()) {
+    return std::nullopt;
+  }
+  return it->get_overall_interface_state();
+}
+
 }  // namespace polymath::sygnal

sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_hpo_interface_test.cpp

@@ -71,8 +71,8 @@ polymath::socketcan::CanFrame buildHpoHeartbeat(
   msg.interface2_state = interface_states[2] ? 1 : 0;
   msg.interface3_state = interface_states[3] ? 1 : 0;
   msg.interface4_state = interface_states[4] ? 1 : 0;
-  msg.interface5_state = interface_states[5] ? 1 : 0;
-  msg.interface6_state = interface_states[6] ? 1 : 0;
+  msg.interface5_state = 0;
+  msg.interface6_state = 0;
   msg.overall_interface_state = overall_state ? 1 : 0;
   msg.count16 = 0;
   msg.crc = 0;
@@ -167,7 +167,7 @@ TEST_CASE("SygnalHpoInterface explicit constructor sets bus address", "[sygnal_h
 TEST_CASE("SygnalHpoInterface parses heartbeat with all interfaces under HPO control", "[sygnal_hpo_interface]")
 {
   SygnalHpoInterface hpo(TEST_BUS_ADDRESS);
-  std::array<bool, HPO_NUM_INTERFACES> interfaces{true, true, true, true, true, true, true};
+  std::array<bool, HPO_NUM_INTERFACES> interfaces{true, true, true, true, true};
   auto frame = buildHpoHeartbeat(TEST_BUS_ADDRESS, interfaces, true);
 
   REQUIRE(hpo.parseHeartbeatFrame(frame));
@@ -180,7 +180,7 @@ TEST_CASE("SygnalHpoInterface parses heartbeat with all interfaces under HPO con
 TEST_CASE("SygnalHpoInterface parses heartbeat with mixed interface bits", "[sygnal_hpo_interface]")
 {
   SygnalHpoInterface hpo(TEST_BUS_ADDRESS);
-  std::array<bool, HPO_NUM_INTERFACES> interfaces{true, false, true, false, true, false, true};
+  std::array<bool, HPO_NUM_INTERFACES> interfaces{true, false, true, false, true};
   auto frame = buildHpoHeartbeat(TEST_BUS_ADDRESS, interfaces, false);
 
   REQUIRE(hpo.parseHeartbeatFrame(frame));
@@ -194,7 +194,7 @@ TEST_CASE("SygnalHpoInterface parses heartbeat with mixed interface bits", "[syg
 TEST_CASE("SygnalHpoInterface rejects heartbeat with wrong frame ID", "[sygnal_hpo_interface]")
 {
   SygnalHpoInterface hpo(TEST_BUS_ADDRESS);
-  std::array<bool, HPO_NUM_INTERFACES> interfaces{true, true, true, true, true, true, true};
+  std::array<bool, HPO_NUM_INTERFACES> interfaces{true, true, true, true, true};
   auto frame = buildHpoHeartbeat(TEST_BUS_ADDRESS, interfaces, true);
   frame.set_can_id(0x999);
 
@@ -205,7 +205,7 @@ TEST_CASE("SygnalHpoInterface rejects heartbeat with wrong frame ID", "[sygnal_h
 TEST_CASE("SygnalHpoInterface rejects heartbeat with bad CRC", "[sygnal_hpo_interface]")
 {
   SygnalHpoInterface hpo(TEST_BUS_ADDRESS);
-  std::array<bool, HPO_NUM_INTERFACES> interfaces{true, true, true, true, true, true, true};
+  std::array<bool, HPO_NUM_INTERFACES> interfaces{true, true, true, true, true};
   auto frame = buildHpoHeartbeat(TEST_BUS_ADDRESS, interfaces, true);
 
   // Corrupt the CRC byte.
@@ -219,7 +219,7 @@ TEST_CASE("SygnalHpoInterface rejects heartbeat with bad CRC", "[sygnal_hpo_inte
 TEST_CASE("SygnalHpoInterface rejects heartbeat addressed to a different bus", "[sygnal_hpo_interface]")
 {
   SygnalHpoInterface hpo(TEST_BUS_ADDRESS);
-  std::array<bool, HPO_NUM_INTERFACES> interfaces{true, true, true, true, true, true, true};
+  std::array<bool, HPO_NUM_INTERFACES> interfaces{true, true, true, true, true};
   auto frame = buildHpoHeartbeat(OTHER_BUS_ADDRESS, interfaces, true);
 
   REQUIRE_FALSE(hpo.parseHeartbeatFrame(frame));