Skip to content

First pass at adding additional interfaces and simplifying the layout… #13

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
zeerekahmad merged 4 commits into from
Feb 23, 2026
Merged

First pass at adding additional interfaces and simplifying the layout… #13

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
8d7823f
First pass at adding additional interfaces and simplifying the layout…
zeerekahmad Feb 11, 2026
96d2ddb
increase max allowed subsystems to 2
Feb 11, 2026
d59ec8c
debugging messages
Feb 11, 2026
b4cb14c
remove debug message, leav in fix for subsystem id in fix for actuall…
Feb 11, 2026
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion ...ce/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_command_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Copy link
Contributor Author

@zeerekahmad zeerekahmad Feb 11, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Why would MAX_SYGNAL_SUBSYSTEMS be 2? It's a boolean, it's 0 or 1?

Copy link

@Ryanbahl9 Ryanbahl9 Feb 13, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

The usage was comparing like this:

  if (subsystem_id >= MAX_SYGNAL_SUBSYSTEMS) {
    error_message = error_message + "Subsystem ID exceeds maximum allowed subsystems. \n";
    return std::nullopt;
  }

The Use of >= made me assume it was the max number of subsystems, not the value of the highest subsystem id.

Copy link

@Ryanbahl9 Ryanbahl9 Feb 13, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

So when MAX_SYGNAL_SUBSYSTEMS = 1, this would error out when I tried to send a command to subsystem 1.

Copy link
Contributor Author

@zeerekahmad zeerekahmad Feb 23, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is fine, subsystem ID and # of subsystems are off by 1. This is good enough.


struct SygnalControlCommandResponse
{
Expand Down
21 changes: 11 additions & 10 deletions .../sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,13 @@ struct SendCommandResult
std::optional<std::future<SygnalControlCommandResponse>> 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
Expand All @@ -52,16 +59,11 @@ class SygnalInterfaceSocketcan
bool parse(const socketcan::CanFrame & frame);

/// @brief Get interface states array from MCM 0
std::array<SygnalSystemState, 5> get_interface_states_0() const;

/// @brief Get interface states array from MCM 1
std::array<SygnalSystemState, 5> get_interface_states_1() const;
std::optional<std::array<SygnalSystemState, 5>> 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<SygnalSystemState> 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
Expand Down Expand Up @@ -105,8 +107,7 @@ class SygnalInterfaceSocketcan

private:
std::shared_ptr<socketcan::SocketcanAdapter> socketcan_adapter_;
SygnalMcmInterface mcm_interface_0_;
SygnalMcmInterface mcm_interface_1_;
std::array<McmSystem, 2> mcm_systems_; // Assuming max 2 MCMs for now, indexed by bus address
SygnalControlInterface control_interface_;

// Promise queues for each response type
Expand Down
7 changes: 6 additions & 1 deletion ...erface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_mcm_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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<SygnalSystemState, 5> get_interface_states() const
Expand All @@ -83,6 +87,7 @@ class SygnalMcmInterface

private:
uint8_t subsystem_id_;
uint8_t bus_address_;
SygnalSystemState sygnal_mcm_state_;
std::array<SygnalSystemState, 5> sygnal_interface_states_;
std::chrono::system_clock::time_point last_heartbeat_timestamp_;
Expand Down
2 changes: 2 additions & 0 deletions sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_command_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,6 +53,7 @@ std::optional<polymath::socketcan::CanFrame> 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<uint8_t>(control_state);
mcm_unpacked_control_enable_t.crc = 0;
Expand Down Expand Up @@ -112,6 +113,7 @@ std::optional<polymath::socketcan::CanFrame> 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);
Expand Down
68 changes: 44 additions & 24 deletions sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,22 +22,29 @@ namespace polymath::sygnal
{

SygnalInterfaceSocketcan::SygnalInterfaceSocketcan(std::shared_ptr<socketcan::SocketcanAdapter> 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);
Expand Down Expand Up @@ -78,24 +85,35 @@ bool SygnalInterfaceSocketcan::parse(const socketcan::CanFrame & frame)
return true;
}

std::array<SygnalSystemState, 5> SygnalInterfaceSocketcan::get_interface_states_0() const
{
return mcm_interface_0_.get_interface_states();
}

std::array<SygnalSystemState, 5> SygnalInterfaceSocketcan::get_interface_states_1() const
std::optional<std::array<SygnalSystemState, 5>> 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<SygnalSystemState> 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(
Expand All @@ -113,6 +131,8 @@ SendCommandResult SygnalInterfaceSocketcan::sendControlStateCommand(
return {false, std::nullopt};
}

auto data = frame_opt->get_data();

std::optional<std::future<SygnalControlCommandResponse>> future_opt;
if (expect_reply) {
std::promise<SygnalControlCommandResponse> promise;
Expand Down
15 changes: 14 additions & 1 deletion sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_mcm_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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
Expand Down
18 changes: 12 additions & 6 deletions sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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);
Expand Down
16 changes: 8 additions & 8 deletions sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_mcm_interface_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);

Expand All @@ -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);

Expand All @@ -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<uint8_t> data = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0x98};
Expand All @@ -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<uint8_t> data = {0x81, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x86};
Expand All @@ -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<uint8_t> data = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0x98};
auto frame = createTestFrame(0x999, data); // Wrong ID
Expand All @@ -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<uint8_t> data = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0xFF};
Expand All @@ -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<uint8_t> data_sub1 = {0x81, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x86};
Expand All @@ -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<uint8_t> data_sub0 = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0x98};
Expand Down
17 changes: 11 additions & 6 deletions ...sygnal_can_interface_ros2/include/sygnal_can_interface_ros2/sygnal_can_interface_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 <array>
#include <chrono>
#include <memory>
#include <optional>
Expand Down Expand Up @@ -110,16 +111,20 @@ class SygnalCanInterfaceNode : public rclcpp_lifecycle::LifecycleNode
rclcpp::Service<sygnal_can_msgs::srv::SendControlCommand>::SharedPtr send_control_command_service_;
rclcpp::Service<sygnal_can_msgs::srv::SendRelayCommand>::SharedPtr send_relay_command_service_;

// MCM heartbeat publisher entry
struct McmHeartbeatEntry
{
uint8_t bus_id;
uint8_t subsystem_id;
rclcpp_lifecycle::LifecyclePublisher<sygnal_can_msgs::msg::McmHeartbeat>::SharedPtr publisher;
std::optional<sygnal_can_msgs::msg::McmHeartbeat> current_state;
};

// Publishers
rclcpp_lifecycle::LifecyclePublisher<diagnostic_msgs::msg::DiagnosticArray>::SharedPtr diagnostics_pub_;
rclcpp_lifecycle::LifecyclePublisher<sygnal_can_msgs::msg::McmHeartbeat>::SharedPtr mcm0_heartbeat_pub_;
rclcpp_lifecycle::LifecyclePublisher<sygnal_can_msgs::msg::McmHeartbeat>::SharedPtr mcm1_heartbeat_pub_;
std::array<McmHeartbeatEntry, 4> mcm_heartbeat_entries_;

rclcpp::Subscription<sygnal_can_msgs::msg::ControlCommand>::SharedPtr control_command_sub_;

// Current MCM state storage
std::optional<sygnal_can_msgs::msg::McmHeartbeat> current_mcm0_state_;
std::optional<sygnal_can_msgs::msg::McmHeartbeat> current_mcm1_state_;
};

} // namespace polymath::sygnal
Expand Down
Loading