diff --git a/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_command_interface.hpp b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_command_interface.hpp index ef5b924..c97377b 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_command_interface.hpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_command_interface.hpp @@ -46,7 +46,7 @@ enum class SygnalControlCommandResponseType : uint8_t }; constexpr uint32_t MAX_SYGNAL_INTERFACES = 6; -constexpr uint32_t MAX_SYGNAL_SUBSYSTEMS = 1; +constexpr uint32_t MAX_SYGNAL_SUBSYSTEMS = 2; struct SygnalControlCommandResponse { 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 b435892..f97a739 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 @@ -36,6 +36,13 @@ struct SendCommandResult std::optional> response_future; }; +struct McmSystem +{ + uint8_t bus_address; + SygnalMcmInterface mcm_0; + SygnalMcmInterface mcm_1; +}; + constexpr uint32_t MAX_PROMISE_QUEUE_LENGTH = 100; /// @brief Combined Sygnal MCM and Control interface with SocketCAN communication @@ -52,16 +59,11 @@ class SygnalInterfaceSocketcan bool parse(const socketcan::CanFrame & frame); /// @brief Get interface states array from MCM 0 - std::array get_interface_states_0() const; - - /// @brief Get interface states array from MCM 1 - std::array get_interface_states_1() const; + std::optional> get_interface_state( + const uint8_t bus_address, const uint8_t subsystem_id) const; /// @brief Get MCM 0 system state - SygnalSystemState get_sygnal_mcm0_state() const; - - /// @brief Get MCM 1 system state - SygnalSystemState get_sygnal_mcm1_state() const; + std::optional get_sygnal_mcm_state(const uint8_t bus_address, const uint8_t subsystem_id) const; /// @brief Send control state (enable/disable) command /// @param bus_id Bus address @@ -105,8 +107,7 @@ class SygnalInterfaceSocketcan private: std::shared_ptr socketcan_adapter_; - SygnalMcmInterface mcm_interface_0_; - SygnalMcmInterface mcm_interface_1_; + std::array mcm_systems_; // Assuming max 2 MCMs for now, indexed by bus address 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 ae20d44..7d6d096 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 @@ -24,6 +24,9 @@ namespace polymath::sygnal { +constexpr uint8_t DEFAULT_MCM_BUS_ADDRESS = 1; +constexpr uint8_t SECONDARY_MCM_BUS_ADDRESS = 2; + /// @brief Enum representing the overall state of the system enum class SygnalSystemState : uint8_t { @@ -61,7 +64,8 @@ inline std::string sygnalSystemStateToString(SygnalSystemState state) class SygnalMcmInterface { public: - explicit SygnalMcmInterface(uint8_t subsystem_id); + SygnalMcmInterface(); + explicit SygnalMcmInterface(const uint8_t bus_address, const uint8_t subsystem_id); ~SygnalMcmInterface() = default; std::array get_interface_states() const @@ -83,6 +87,7 @@ class SygnalMcmInterface private: uint8_t subsystem_id_; + uint8_t bus_address_; SygnalSystemState sygnal_mcm_state_; std::array sygnal_interface_states_; std::chrono::system_clock::time_point last_heartbeat_timestamp_; diff --git a/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_command_interface.cpp b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_command_interface.cpp index b84069e..4896ea7 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_command_interface.cpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_command_interface.cpp @@ -53,6 +53,7 @@ std::optional SygnalControlInterface::createContr } mcm_unpacked_control_enable_t.bus_address = bus_id; + mcm_unpacked_control_enable_t.sub_system_id = subsystem_id; mcm_unpacked_control_enable_t.interface_id = interface_id; mcm_unpacked_control_enable_t.enable = static_cast(control_state); mcm_unpacked_control_enable_t.crc = 0; @@ -112,6 +113,7 @@ std::optional SygnalControlInterface::createContr } mcm_unpacked_control_command_t.bus_address = bus_id; + mcm_unpacked_control_command_t.sub_system_id = subsystem_id; mcm_unpacked_control_command_t.interface_id = interface_id; mcm_unpacked_control_command_t.count8 = mcm_control_control_command_count8_encode(0.0); mcm_unpacked_control_command_t.value = mcm_control_control_command_value_encode(value); 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 24c7cab..558cd81 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 @@ -22,22 +22,29 @@ namespace polymath::sygnal { SygnalInterfaceSocketcan::SygnalInterfaceSocketcan(std::shared_ptr socketcan_adapter) -: socketcan_adapter_(std::move(socketcan_adapter)) -, mcm_interface_0_(0) -, mcm_interface_1_(1) +: 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); +} bool SygnalInterfaceSocketcan::parse(const socketcan::CanFrame & frame) { // Try parsing as MCM heartbeat (both interfaces will check subsystem_id) - if (mcm_interface_0_.parseMcmHeartbeatFrame(frame)) { - return true; - }; - - if (mcm_interface_1_.parseMcmHeartbeatFrame(frame)) { - return true; - }; + for (auto & mcm_system : mcm_systems_) { + if (mcm_system.mcm_0.parseMcmHeartbeatFrame(frame)) { + return true; + } + if (mcm_system.mcm_1.parseMcmHeartbeatFrame(frame)) { + return true; + } + } // Try parsing as command response auto response = control_interface_.parseCommandResponseFrame(frame); @@ -78,24 +85,35 @@ bool SygnalInterfaceSocketcan::parse(const socketcan::CanFrame & frame) return true; } -std::array SygnalInterfaceSocketcan::get_interface_states_0() const -{ - return mcm_interface_0_.get_interface_states(); -} - -std::array SygnalInterfaceSocketcan::get_interface_states_1() const +std::optional> SygnalInterfaceSocketcan::get_interface_state( + const uint8_t bus_address, const uint8_t subsystem_id) const { - return mcm_interface_1_.get_interface_states(); -} + 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(); + } + } + } -SygnalSystemState SygnalInterfaceSocketcan::get_sygnal_mcm0_state() const -{ - return mcm_interface_0_.get_mcm_state(); + return std::nullopt; } -SygnalSystemState SygnalInterfaceSocketcan::get_sygnal_mcm1_state() const +std::optional SygnalInterfaceSocketcan::get_sygnal_mcm_state( + const uint8_t bus_address, const uint8_t subsystem_id) const { - return mcm_interface_1_.get_mcm_state(); + 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(); + } + } + } + return std::nullopt; } SendCommandResult SygnalInterfaceSocketcan::sendControlStateCommand( @@ -113,6 +131,8 @@ SendCommandResult SygnalInterfaceSocketcan::sendControlStateCommand( return {false, std::nullopt}; } + auto data = frame_opt->get_data(); + std::optional> future_opt; if (expect_reply) { std::promise promise; diff --git a/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_mcm_interface.cpp b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_mcm_interface.cpp index 0677392..70692c9 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_mcm_interface.cpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_mcm_interface.cpp @@ -22,8 +22,17 @@ namespace polymath::sygnal { -SygnalMcmInterface::SygnalMcmInterface(uint8_t subsystem_id) +SygnalMcmInterface::SygnalMcmInterface() +: subsystem_id_(0) +, bus_address_(0) +, sygnal_mcm_state_(SygnalSystemState::FAIL_HARD) +{ + sygnal_interface_states_.fill(SygnalSystemState::FAIL_HARD); +} + +SygnalMcmInterface::SygnalMcmInterface(const uint8_t bus_address, const uint8_t subsystem_id) : subsystem_id_(subsystem_id) +, bus_address_(bus_address) , sygnal_mcm_state_(SygnalSystemState::FAIL_HARD) { sygnal_interface_states_.fill(SygnalSystemState::FAIL_HARD); @@ -55,6 +64,10 @@ bool SygnalMcmInterface::parseMcmHeartbeatFrame(const socketcan::CanFrame & fram return false; } + if (unpacked_heartbeat_t.bus_address != bus_address_) { + return false; + } + last_heartbeat_timestamp_ = std::chrono::system_clock::now(); // Set the MCM state 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 79a3a1e..ab277e2 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 @@ -270,10 +270,13 @@ TEST_CASE("SygnalInterfaceSocketcan vcan0 integration tests", "[vcan]") frame_received_future.wait_for(std::chrono::seconds(2)); - auto mcm0_state = sygnal_interface->get_sygnal_mcm0_state(); - REQUIRE(mcm0_state == polymath::sygnal::SygnalSystemState::MCM_CONTROL); + auto mcm0_state_opt = sygnal_interface->get_sygnal_mcm_state(bus_id, 0); + REQUIRE(mcm0_state_opt.has_value()); + REQUIRE(mcm0_state_opt.value() == polymath::sygnal::SygnalSystemState::MCM_CONTROL); - auto interface_states_result = sygnal_interface->get_interface_states_0(); + auto interface_states_opt = sygnal_interface->get_interface_state(bus_id, 0); + REQUIRE(interface_states_opt.has_value()); + auto interface_states_result = interface_states_opt.value(); REQUIRE(interface_states_result[0] == polymath::sygnal::SygnalSystemState::MCM_CONTROL); REQUIRE(interface_states_result[1] == polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); REQUIRE(interface_states_result[2] == polymath::sygnal::SygnalSystemState::MCM_CONTROL); @@ -294,10 +297,13 @@ TEST_CASE("SygnalInterfaceSocketcan vcan0 integration tests", "[vcan]") frame_received_future.wait_for(std::chrono::seconds(2)); - auto mcm1_state = sygnal_interface->get_sygnal_mcm1_state(); - REQUIRE(mcm1_state == polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); + auto mcm1_state_opt = sygnal_interface->get_sygnal_mcm_state(bus_id, 1); + REQUIRE(mcm1_state_opt.has_value()); + REQUIRE(mcm1_state_opt.value() == polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); - auto interface_states_result = sygnal_interface->get_interface_states_1(); + auto interface_states_opt = sygnal_interface->get_interface_state(bus_id, 1); + REQUIRE(interface_states_opt.has_value()); + auto interface_states_result = interface_states_opt.value(); REQUIRE(interface_states_result[0] == polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); REQUIRE(interface_states_result[1] == polymath::sygnal::SygnalSystemState::MCM_CONTROL); REQUIRE(interface_states_result[2] == polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); diff --git a/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_mcm_interface_test.cpp b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_mcm_interface_test.cpp index 427ecc8..db71ca6 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_mcm_interface_test.cpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_mcm_interface_test.cpp @@ -40,7 +40,7 @@ static polymath::socketcan::CanFrame createTestFrame(uint32_t can_id, const std: TEST_CASE("SygnalMcmInterface constructor initializes to FAIL_HARD for subsystem 0", "[sygnal_mcm_interface]") { - polymath::sygnal::SygnalMcmInterface interface(0); + polymath::sygnal::SygnalMcmInterface interface(polymath::sygnal::DEFAULT_MCM_BUS_ADDRESS, 0); REQUIRE(interface.get_mcm_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); @@ -52,7 +52,7 @@ TEST_CASE("SygnalMcmInterface constructor initializes to FAIL_HARD for subsystem TEST_CASE("SygnalMcmInterface constructor initializes to FAIL_HARD for subsystem 1", "[sygnal_mcm_interface]") { - polymath::sygnal::SygnalMcmInterface interface(1); + polymath::sygnal::SygnalMcmInterface interface(polymath::sygnal::DEFAULT_MCM_BUS_ADDRESS, 1); REQUIRE(interface.get_mcm_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); @@ -64,7 +64,7 @@ TEST_CASE("SygnalMcmInterface constructor initializes to FAIL_HARD for subsystem TEST_CASE("SygnalMcmInterface parses MCM_CONTROL heartbeat", "[sygnal_mcm_interface]") { - polymath::sygnal::SygnalMcmInterface interface(0); + polymath::sygnal::SygnalMcmInterface interface(polymath::sygnal::DEFAULT_MCM_BUS_ADDRESS, 0); // Generated from DBC: MCM_CONTROL state, subsystem 0, all interfaces MCM_CONTROL std::vector data = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0x98}; @@ -81,7 +81,7 @@ TEST_CASE("SygnalMcmInterface parses MCM_CONTROL heartbeat", "[sygnal_mcm_interf TEST_CASE("SygnalMcmInterface parses HUMAN_CONTROL heartbeat", "[sygnal_mcm_interface]") { - polymath::sygnal::SygnalMcmInterface interface(1); + polymath::sygnal::SygnalMcmInterface interface(polymath::sygnal::DEFAULT_MCM_BUS_ADDRESS, 1); // Generated from DBC: HUMAN_CONTROL state, subsystem 1 std::vector data = {0x81, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x86}; @@ -98,7 +98,7 @@ TEST_CASE("SygnalMcmInterface parses HUMAN_CONTROL heartbeat", "[sygnal_mcm_inte TEST_CASE("SygnalMcmInterface rejects wrong frame ID", "[sygnal_mcm_interface]") { - polymath::sygnal::SygnalMcmInterface interface(0); + polymath::sygnal::SygnalMcmInterface interface(polymath::sygnal::DEFAULT_MCM_BUS_ADDRESS, 0); std::vector data = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0x98}; auto frame = createTestFrame(0x999, data); // Wrong ID @@ -110,7 +110,7 @@ TEST_CASE("SygnalMcmInterface rejects wrong frame ID", "[sygnal_mcm_interface]") TEST_CASE("SygnalMcmInterface rejects bad CRC", "[sygnal_mcm_interface]") { - polymath::sygnal::SygnalMcmInterface interface(0); + polymath::sygnal::SygnalMcmInterface interface(polymath::sygnal::DEFAULT_MCM_BUS_ADDRESS, 0); // Valid data but corrupted CRC (last byte) std::vector data = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0xFF}; @@ -123,7 +123,7 @@ TEST_CASE("SygnalMcmInterface rejects bad CRC", "[sygnal_mcm_interface]") TEST_CASE("SygnalMcmInterface only parses matching subsystem_id", "[sygnal_mcm_interface]") { // Interface configured for subsystem 0 should ignore subsystem 1 frames - polymath::sygnal::SygnalMcmInterface interface_0(0); + polymath::sygnal::SygnalMcmInterface interface_0(polymath::sygnal::DEFAULT_MCM_BUS_ADDRESS, 0); // Heartbeat frame for subsystem 1 std::vector data_sub1 = {0x81, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x86}; @@ -134,7 +134,7 @@ TEST_CASE("SygnalMcmInterface only parses matching subsystem_id", "[sygnal_mcm_i REQUIRE(interface_0.get_mcm_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); // Interface configured for subsystem 1 should ignore subsystem 0 frames - polymath::sygnal::SygnalMcmInterface interface_1(1); + polymath::sygnal::SygnalMcmInterface interface_1(polymath::sygnal::DEFAULT_MCM_BUS_ADDRESS, 1); // Heartbeat frame for subsystem 0 std::vector data_sub0 = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0x98}; 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 32d801c..c0b06c8 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,6 +15,7 @@ #ifndef SYGNAL_CAN_INTERFACE_NODE__SYGNAL_CAN_INTERFACE_NODE_HPP_ #define SYGNAL_CAN_INTERFACE_NODE__SYGNAL_CAN_INTERFACE_NODE_HPP_ +#include #include #include #include @@ -110,16 +111,20 @@ class SygnalCanInterfaceNode : public rclcpp_lifecycle::LifecycleNode rclcpp::Service::SharedPtr send_control_command_service_; rclcpp::Service::SharedPtr send_relay_command_service_; + // MCM heartbeat publisher entry + struct McmHeartbeatEntry + { + uint8_t bus_id; + uint8_t subsystem_id; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr publisher; + std::optional current_state; + }; + // Publishers rclcpp_lifecycle::LifecyclePublisher::SharedPtr diagnostics_pub_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr mcm0_heartbeat_pub_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr mcm1_heartbeat_pub_; + std::array mcm_heartbeat_entries_; rclcpp::Subscription::SharedPtr control_command_sub_; - - // Current MCM state storage - std::optional current_mcm0_state_; - std::optional current_mcm1_state_; }; } // namespace polymath::sygnal 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 06b23e0..c442202 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 @@ -78,8 +78,18 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal // Create publishers diagnostics_pub_ = create_publisher("/diagnostics", rclcpp::QoS(10)); - mcm0_heartbeat_pub_ = create_publisher("~/mcm0_heartbeat", rclcpp::QoS(10)); - mcm1_heartbeat_pub_ = create_publisher("~/mcm1_heartbeat", 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, {}, {}}; + + for (auto & entry : mcm_heartbeat_entries_) { + std::string topic = + "~/mcm_heartbeat_bus" + std::to_string(entry.bus_id) + "_sub" + std::to_string(entry.subsystem_id); + entry.publisher = create_publisher(topic, rclcpp::QoS(10)); + } // Create services set_control_state_service_ = create_service( @@ -114,8 +124,9 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal { // Activate publishers diagnostics_pub_->on_activate(); - mcm0_heartbeat_pub_->on_activate(); - mcm1_heartbeat_pub_->on_activate(); + for (auto & entry : mcm_heartbeat_entries_) { + entry.publisher->on_activate(); + } create_subscription( "~/control_command", @@ -131,8 +142,9 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal { // Deactivate publishers diagnostics_pub_->on_deactivate(); - mcm0_heartbeat_pub_->on_deactivate(); - mcm1_heartbeat_pub_->on_deactivate(); + for (auto & entry : mcm_heartbeat_entries_) { + entry.publisher->on_deactivate(); + } RCLCPP_INFO(get_logger(), "Sygnal CAN Interface node deactivated"); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; @@ -147,8 +159,10 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal send_control_command_service_.reset(); send_relay_command_service_.reset(); diagnostics_pub_.reset(); - mcm0_heartbeat_pub_.reset(); - mcm1_heartbeat_pub_.reset(); + for (auto & entry : mcm_heartbeat_entries_) { + entry.publisher.reset(); + entry.current_state.reset(); + } if (socketcan_adapter_) { socketcan_adapter_->joinReceptionThread(); @@ -164,47 +178,31 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal void SygnalCanInterfaceNode::timerCallback() { - // Get MCM states from sygnal_interface_ - auto mcm0_state = sygnal_interface_->get_sygnal_mcm0_state(); - auto mcm1_state = sygnal_interface_->get_sygnal_mcm1_state(); - auto interface_states_0 = sygnal_interface_->get_interface_states_0(); - auto interface_states_1 = sygnal_interface_->get_interface_states_1(); - - // Create and populate MCM 0 heartbeat message - sygnal_can_msgs::msg::McmHeartbeat mcm0_msg; - mcm0_msg.header.stamp = now(); - mcm0_msg.state = static_cast(mcm0_state); - - // Convert interface states array to vector for MCM 0 - mcm0_msg.interface_states.clear(); - for (const auto & state : interface_states_0) { - mcm0_msg.interface_states.push_back(static_cast(state)); - } + for (auto & entry : mcm_heartbeat_entries_) { + auto mcm_state_opt = sygnal_interface_->get_sygnal_mcm_state(entry.bus_id, entry.subsystem_id); + auto interface_states_opt = sygnal_interface_->get_interface_state(entry.bus_id, entry.subsystem_id); - // Store current state and publish MCM 0 - current_mcm0_state_ = mcm0_msg; - mcm0_heartbeat_pub_->publish(mcm0_msg); + if (!mcm_state_opt || !interface_states_opt) { + continue; + } - // Create and populate MCM 1 heartbeat message - sygnal_can_msgs::msg::McmHeartbeat mcm1_msg; - mcm1_msg.header.stamp = now(); - mcm1_msg.state = static_cast(mcm1_state); + sygnal_can_msgs::msg::McmHeartbeat msg; + msg.header.stamp = now(); + msg.state = static_cast(mcm_state_opt.value()); - // Convert interface states array to vector for MCM 1 - mcm1_msg.interface_states.clear(); - for (const auto & state : interface_states_1) { - mcm1_msg.interface_states.push_back(static_cast(state)); - } + for (const auto & state : interface_states_opt.value()) { + msg.interface_states.push_back(static_cast(state)); + } - // Store current state and publish MCM 1 - current_mcm1_state_ = mcm1_msg; - mcm1_heartbeat_pub_->publish(mcm1_msg); + entry.current_state = msg; + entry.publisher->publish(msg); + } // Create and publish diagnostics auto diagnostics = createDiagnosticsMessage(); diagnostics_pub_->publish(diagnostics); - RCLCPP_DEBUG(get_logger(), "Published MCM 0 and MCM 1 heartbeats and diagnostics"); + RCLCPP_DEBUG(get_logger(), "Published MCM heartbeats and diagnostics"); } void SygnalCanInterfaceNode::setControlStateCallback( @@ -405,116 +403,69 @@ diagnostic_msgs::msg::DiagnosticArray SygnalCanInterfaceNode::createDiagnosticsM array_msg.header.stamp = get_clock()->now(); array_msg.header.frame_id = ""; - // Get current MCM states - auto mcm0_state = sygnal_interface_->get_sygnal_mcm0_state(); - auto mcm1_state = sygnal_interface_->get_sygnal_mcm1_state(); - auto interface_states_0 = sygnal_interface_->get_interface_states_0(); - auto interface_states_1 = sygnal_interface_->get_interface_states_1(); - - // Create MCM 0 diagnostics - { - diagnostic_msgs::msg::DiagnosticStatus mcm0_diag; - mcm0_diag.name = "sygnal_mcm0"; - mcm0_diag.hardware_id = "sygnal_can_interface"; - - // Determine diagnostic level based on state - if (mcm0_state == SygnalSystemState::MCM_CONTROL || mcm0_state == SygnalSystemState::HUMAN_CONTROL) { - mcm0_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - } else if ( - mcm0_state == SygnalSystemState::FAIL_OPERATIONAL_1 || mcm0_state == SygnalSystemState::FAIL_OPERATIONAL_2) - { - mcm0_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - } else { - mcm0_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - } + for (const auto & entry : mcm_heartbeat_entries_) { + auto mcm_state_opt = sygnal_interface_->get_sygnal_mcm_state(entry.bus_id, entry.subsystem_id); + auto interface_states_opt = sygnal_interface_->get_interface_state(entry.bus_id, entry.subsystem_id); - mcm0_diag.message = "MCM0 state: " + sygnalSystemStateToString(mcm0_state); + if (!mcm_state_opt) { + continue; + } - diagnostic_msgs::msg::KeyValue kv; - kv.key = "state"; - kv.value = sygnalSystemStateToString(mcm0_state); - mcm0_diag.values.push_back(kv); + auto mcm_state = mcm_state_opt.value(); + std::string mcm_name = + "sygnal_mcm_bus" + std::to_string(entry.bus_id) + "_sub" + std::to_string(entry.subsystem_id); - array_msg.status.push_back(mcm0_diag); - } + // Create MCM diagnostic + diagnostic_msgs::msg::DiagnosticStatus mcm_diag; + mcm_diag.name = mcm_name; + mcm_diag.hardware_id = "sygnal_can_interface"; - // Create MCM 1 diagnostics - { - diagnostic_msgs::msg::DiagnosticStatus mcm1_diag; - mcm1_diag.name = "sygnal_mcm1"; - mcm1_diag.hardware_id = "sygnal_can_interface"; - - // Determine diagnostic level based on state - if (mcm1_state == SygnalSystemState::MCM_CONTROL || mcm1_state == SygnalSystemState::HUMAN_CONTROL) { - mcm1_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - } else if ( - mcm1_state == SygnalSystemState::FAIL_OPERATIONAL_1 || mcm1_state == SygnalSystemState::FAIL_OPERATIONAL_2) + if (mcm_state == SygnalSystemState::MCM_CONTROL || mcm_state == SygnalSystemState::HUMAN_CONTROL) { + mcm_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + } else if (mcm_state == SygnalSystemState::FAIL_OPERATIONAL_1 || mcm_state == SygnalSystemState::FAIL_OPERATIONAL_2) { - mcm1_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + mcm_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; } else { - mcm1_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mcm_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; } - mcm1_diag.message = "MCM1 state: " + sygnalSystemStateToString(mcm1_state); + mcm_diag.message = mcm_name + " state: " + sygnalSystemStateToString(mcm_state); diagnostic_msgs::msg::KeyValue kv; kv.key = "state"; - kv.value = sygnalSystemStateToString(mcm1_state); - mcm1_diag.values.push_back(kv); - - array_msg.status.push_back(mcm1_diag); - } - - // Create diagnostics for each interface (0-4) from both MCM subsystems - for (size_t i = 0; i < interface_states_0.size(); ++i) { - auto state_0 = interface_states_0[i]; - auto state_1 = interface_states_1[i]; - - // Create diagnostic for this interface - diagnostic_msgs::msg::DiagnosticStatus interface_diag; - interface_diag.name = "sygnal_interface_" + std::to_string(i); - interface_diag.hardware_id = "sygnal_can_interface"; - - // Determine worst-case diagnostic level from both MCMs - auto worst_state = state_0; - if (static_cast(state_1) > static_cast(state_0)) { - worst_state = state_1; + kv.value = sygnalSystemStateToString(mcm_state); + mcm_diag.values.push_back(kv); + + array_msg.status.push_back(mcm_diag); + + // Create per-interface diagnostics if available + if (interface_states_opt) { + const auto & interface_states = interface_states_opt.value(); + for (size_t i = 0; i < interface_states.size(); ++i) { + auto state = interface_states[i]; + + diagnostic_msgs::msg::DiagnosticStatus interface_diag; + interface_diag.name = mcm_name + "_interface_" + std::to_string(i); + interface_diag.hardware_id = "sygnal_can_interface"; + + if (state == SygnalSystemState::MCM_CONTROL || state == SygnalSystemState::HUMAN_CONTROL) { + interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + } else if (state == SygnalSystemState::FAIL_OPERATIONAL_1 || state == SygnalSystemState::FAIL_OPERATIONAL_2) { + interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } else { + interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + + interface_diag.message = "Interface " + std::to_string(i) + " state: " + sygnalSystemStateToString(state); + + diagnostic_msgs::msg::KeyValue interface_kv; + interface_kv.key = "state"; + interface_kv.value = sygnalSystemStateToString(state); + interface_diag.values.push_back(interface_kv); + + array_msg.status.push_back(interface_diag); + } } - - if (worst_state == SygnalSystemState::MCM_CONTROL || worst_state == SygnalSystemState::HUMAN_CONTROL) { - interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - } else if ( - worst_state == SygnalSystemState::FAIL_OPERATIONAL_1 || worst_state == SygnalSystemState::FAIL_OPERATIONAL_2) - { - interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - } else { - interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - } - - // Build message showing states from both MCMs - interface_diag.message = "Interface " + std::to_string(i) + " - MCM0: " + sygnalSystemStateToString(state_0) + - ", MCM1: " + sygnalSystemStateToString(state_1); - - // Add state values from both MCMs - diagnostic_msgs::msg::KeyValue kv_mcm0; - kv_mcm0.key = "mcm0_state"; - kv_mcm0.value = sygnalSystemStateToString(state_0); - interface_diag.values.push_back(kv_mcm0); - - diagnostic_msgs::msg::KeyValue kv_mcm1; - kv_mcm1.key = "mcm1_state"; - kv_mcm1.value = sygnalSystemStateToString(state_1); - interface_diag.values.push_back(kv_mcm1); - - // Flag if states differ - if (state_0 != state_1) { - diagnostic_msgs::msg::KeyValue kv_mismatch; - kv_mismatch.key = "state_mismatch"; - kv_mismatch.value = "true"; - interface_diag.values.push_back(kv_mismatch); - } - - array_msg.status.push_back(interface_diag); } return array_msg;