From fb26ac28dad638e6dbc97e9d32dbc34a4123a6cb Mon Sep 17 00:00:00 2001 From: Zeerek Date: Wed, 21 Jan 2026 01:51:39 +0000 Subject: [PATCH 01/15] Adding in interface base from a customer repo --- .../sygnal_can_interface_lib/CMakeLists.txt | 75 +++++ .../sygnal_can_interface_lib/README.md | 2 + .../sygnal_command_interface.hpp | 101 +++++++ .../sygnal_mcm_interface.hpp | 95 +++++++ .../sygnal_can_interface_lib/package.xml | 18 ++ .../src/sygnal_command_interface.cpp | 263 ++++++++++++++++++ .../src/sygnal_mcm_interface.cpp | 104 +++++++ 7 files changed, 658 insertions(+) create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/README.md create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_command_interface.hpp create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_mcm_interface.hpp create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/package.xml create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_command_interface.cpp create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_mcm_interface.cpp diff --git a/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt b/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt new file mode 100644 index 0000000..943a14d --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt @@ -0,0 +1,75 @@ +# Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +# Proprietary. Any unauthorized copying, distribution, or modification of this software is strictly prohibited. +cmake_minimum_required(VERSION 3.8) +project(sygnal_can_interface_lib) + +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic -Werror=switch) + add_link_options("-Wl,--no-undefined") +endif() + +find_package(ament_cmake REQUIRED) +find_package(socketcan_adapter REQUIRED) +find_package(sygnal_dbc REQUIRED) + +add_library( + ${PROJECT_NAME} SHARED + src/sygnal_mcm_interface.cpp + src/sygnal_command_interface.cpp +) + +target_include_directories(${PROJECT_NAME} PUBLIC + "$" + "$" +) + +target_link_libraries(${PROJECT_NAME} + PUBLIC socketcan_adapter::socketcan_adapter +) + +ament_target_dependencies(${PROJECT_NAME} PUBLIC sygnal_dbc) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}_TARGETS + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin +) + +install( + EXPORT ${PROJECT_NAME}_TARGETS + FILE ${PROJECT_NAME}Targets.cmake + NAMESPACE ${PROJECT_NAME}:: + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_targets(${PROJECT_NAME}_TARGETS HAS_LIBRARY_TARGET) +ament_export_dependencies( + socketcan_adapter + sygnal_dbc +) + +ament_package() diff --git a/sygnal_can_interface/sygnal_can_interface_lib/README.md b/sygnal_can_interface/sygnal_can_interface_lib/README.md new file mode 100644 index 0000000..3774f9d --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/README.md @@ -0,0 +1,2 @@ + +TO BE FILLED OUT 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 new file mode 100644 index 0000000..4594e06 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_command_interface.hpp @@ -0,0 +1,101 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// Proprietary. Any unauthorized copying, distribution, or modification of this software is strictly prohibited. + +#ifndef SYGNAL_CAN_INTERFACE_LIB__SYGNAL_COMMAND_INTERFACE_HPP_ +#define SYGNAL_CAN_INTERFACE_LIB__SYGNAL_COMMAND_INTERFACE_HPP_ + +#include +#include + +#include "socketcan_adapter/can_frame.hpp" +#include "socketcan_adapter/socketcan_adapter.hpp" + +namespace polymath::sygnal +{ + +/// @brief Enum representing whether the system is under MCM or human control +enum class SygnalControlState : uint8_t +{ + MCM_CONTROL = 1, + HUMAN_CONTROL = 0 +}; + +/// @brief Enum representing whether the relay is enabled or disabled +enum class SygnalRelayState : uint8_t +{ + ENABLE = 1, + DISABLE = 0 +}; + +enum class SygnalControlCommandResponseType : uint8_t +{ + ENABLE = 0, + CONTROL = 1, + RELAY = 2 +}; + +struct SygnalControlCommandResponse +{ + SygnalControlCommandResponseType response_type; + uint8_t interface_id; + uint8_t bus_id; + // Value can represent different things based on response type + double value; +}; + +class SygnalControlInterface +{ +public: + SygnalControlInterface(); + ~SygnalControlInterface() = default; + + /// @brief Set the control state (enable/disable MCM control) + /// @param interface_id Sygnal Interface to enable/disable + /// @param control_state Sygnal Control State (Human or MCM control) + /// @param[out] error_message Error message in case of failure + /// @return can frame if succesful + std::optional createControlStateCommandFrame( + const uint8_t bus_id, + const uint8_t interface_id, + const SygnalControlState control_state, + std::string & error_message); + + /// @brief Generate a control command frame to send to the vehicle + /// @param interface_id Sygnal Interface ID to send command to + /// @param value Value to set interface control command to + /// @param error_message Error in case of failure + /// @return can frame if succesful + std::optional createControlCommandFrame( + const uint8_t bus_id, const uint8_t interface_id, const double value, std::string & error_message); + + /// @brief Set the relay state for engine control or gears + /// @param subsystem_id Sygnal Subsystem Relay to enable/disable + /// @param relay_state Sygnal Relay state (Enable or Disable) + /// @param[out] error_message Error message in case of failure + /// @return can frame if succesful + std::optional createRelayCommandFrame( + const uint8_t bus_id, const uint8_t subsystem_id, const bool relay_state, std::string & error_message); + + /// @brief Parse a command response frame if it is covered by this interface + /// @param frame Raw cran frame to check and parse + /// @return Optional, if the frame is a valid response, return the response + std::optional parseCommandResponseFrame(const socketcan::CanFrame & frame); +}; + +} // namespace polymath::sygnal + +#endif // SYGNAL_CAN_INTERFACE_LIB__SYGNAL_COMMAND_INTERFACE_HPP_ 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 new file mode 100644 index 0000000..861fa09 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_mcm_interface.hpp @@ -0,0 +1,95 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// Proprietary. Any unauthorized copying, distribution, or modification of this software is strictly prohibited. + +#ifndef SYGNAL_CAN_INTERFACE_LIB__SYGNAL_MCM_INTERFACE_HPP_ +#define SYGNAL_CAN_INTERFACE_LIB__SYGNAL_MCM_INTERFACE_HPP_ + +#include +#include + +#include "socketcan_adapter/can_frame.hpp" +#include "socketcan_adapter/socketcan_adapter.hpp" + +namespace polymath::sygnal +{ + +/// @brief Enum representing the overall state of the system +enum class SygnalSystemState : uint8_t +{ + FAIL_HARD = 254, + HUMAN_OVERRIDE = 253, + FAIL_OPERATIONAL_2 = 242, + FAIL_OPERATIONAL_1 = 241, + MCM_CONTROL = 1, + HUMAN_CONTROL = 0 +}; + +/// @brief Get the string of the Sygnal System State +/// @param state input SygnalSystemState +/// @return string of the state value +inline std::string sygnalSystemStateToString(SygnalSystemState state) +{ + switch (state) { + case SygnalSystemState::FAIL_HARD: + return "FAIL_HARD"; + case SygnalSystemState::HUMAN_OVERRIDE: + return "HUMAN_OVERRIDE"; + case SygnalSystemState::FAIL_OPERATIONAL_2: + return "FAIL_OPERATIONAL_2"; + case SygnalSystemState::FAIL_OPERATIONAL_1: + return "FAIL_OPERATIONAL_1"; + case SygnalSystemState::MCM_CONTROL: + return "MCM_CONTROL"; + case SygnalSystemState::HUMAN_CONTROL: + return "HUMAN_CONTROL"; + default: + return "UNKNOWN_STATE"; + } +} + +class SygnalMcmInterface +{ +public: + SygnalMcmInterface(); + ~SygnalMcmInterface() = default; + + std::array get_interface_states() const + { + return sygnal_interface_states_; + } + + SygnalSystemState get_sygnal_mcm0_state() const + { + return sygnal_mcm_0_state_; + } + + SygnalSystemState get_sygnal_mcm1_state() const + { + return sygnal_mcm_1_state_; + } + + bool parseMcmHeartbeatFrame(const socketcan::CanFrame & frame); + +private: + SygnalSystemState sygnal_mcm_0_state_; + SygnalSystemState sygnal_mcm_1_state_; + std::array sygnal_interface_states_; +}; + +} // namespace polymath::sygnal + +#endif // SYGNAL_CAN_INTERFACE_LIB__SYGNAL_MCM_INTERFACE_HPP_ diff --git a/sygnal_can_interface/sygnal_can_interface_lib/package.xml b/sygnal_can_interface/sygnal_can_interface_lib/package.xml new file mode 100644 index 0000000..14c9b72 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/package.xml @@ -0,0 +1,18 @@ + + + + sygnal_can_interface_lib + 0.0.0 + Library for interfacing with Sygnal MCM over CAN using Socketcan Adapter + Polymath Engineering + Apache-2.0 + Zeerek Ahmad + + ament_cmake + socketcan_adapter + sygnal_dbc + + + ament_cmake + + 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 new file mode 100644 index 0000000..1699c52 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_command_interface.cpp @@ -0,0 +1,263 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// Proprietary. Any unauthorized copying, distribution, or modification of this software is strictly prohibited. + +#include "sygnal_can_interface_lib/sygnal_command_interface.hpp" + +#include + +#include "sygnal_dbc/mcm_control.h" +#include "sygnal_dbc/mcm_relay.h" + +namespace polymath::sygnal +{ + +namespace +{ + +/// @brief Generate the crc8 checksum assuming 8 byte data with crc at data[7] +/// @param data Data buffer to calculate the checksum for +/// @return crc8 checksum +uint8_t generate_crc8(uint8_t * data) +{ + uint8_t crc = 0x00; + size_t i, j; + // Assumes length 8 data with last bit being checksum + for (i = 0; i < 7; i++) { + crc ^= data[i]; + for (j = 0; j < 8; j++) { + if ((crc & 0x80) != 0) { + crc = (uint8_t)((crc << 1) ^ 0x07); + } else { + crc <<= 1; + } + } + } + return crc; +} + +/// @brief Check the crc8 checksum of incoming data assuming 8 byte data with crc at data[7] +/// @param data Data buffer to check +/// @return true if checksum matches, false otherwise +bool check_crc8(uint8_t * data) +{ + return data[7] == generate_crc8(data); +} + +} // namespace + +SygnalControlInterface::SygnalControlInterface() +{} + +std::optional SygnalControlInterface::createControlStateCommandFrame( + const uint8_t bus_id, const uint8_t interface_id, const SygnalControlState control_state, std::string & error_message) +{ + polymath::socketcan::CanFrame frame; + uint8_t control_enable_buffer[CAN_MAX_DLC]; + mcm_control_control_enable_t mcm_unpacked_control_enable_t; + + if (mcm_control_control_enable_init(&mcm_unpacked_control_enable_t) != 0) { + error_message = error_message + "Unable to initialize mcm_unpacked_control_enable_t. \n"; + return std::nullopt; + } + + mcm_unpacked_control_enable_t.bus_address = bus_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; + + if (!mcm_control_control_enable_pack( + control_enable_buffer, &mcm_unpacked_control_enable_t, sizeof(control_enable_buffer))) + { + error_message = error_message + "Could not pack mcm_unpacked_control_enable_t. \n"; + return std::nullopt; + } + + // Get checksum and pack it again + mcm_unpacked_control_enable_t.crc = generate_crc8(control_enable_buffer); + if (!mcm_control_control_enable_pack( + control_enable_buffer, &mcm_unpacked_control_enable_t, sizeof(control_enable_buffer))) + { + error_message = error_message + "Could not pack mcm_unpacked_control_enable_t. \n"; + return std::nullopt; + } + + std::array control_enable_packed_data; + for (size_t i = 0; i < control_enable_packed_data.size(); ++i) { + control_enable_packed_data[i] = control_enable_buffer[i]; + } + + frame.set_can_id(MCM_CONTROL_CONTROL_ENABLE_FRAME_ID); + frame.set_len(MCM_CONTROL_CONTROL_ENABLE_LENGTH); + frame.set_data(control_enable_packed_data); + + return frame; +} + +std::optional SygnalControlInterface::createControlCommandFrame( + const uint8_t bus_id, const uint8_t interface_id, const double value, std::string & error_message) +{ + polymath::socketcan::CanFrame frame; + uint8_t control_command_buffer[CAN_MAX_DLC]; + mcm_control_control_command_t mcm_unpacked_control_command_t; + + if (mcm_control_control_command_init(&mcm_unpacked_control_command_t) != 0) { + error_message = error_message + "Unable to initialize mcm_unpacked_control_command_t. \n"; + return std::nullopt; + } + + mcm_unpacked_control_command_t.bus_address = bus_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); + mcm_unpacked_control_command_t.crc = 0; + + if (!mcm_control_control_command_pack( + control_command_buffer, &mcm_unpacked_control_command_t, sizeof(control_command_buffer))) + { + error_message = error_message + "Could not pack mcm_unpacked_control_command_t. \n"; + return std::nullopt; + } + + // Get checksum and pack it again + mcm_unpacked_control_command_t.crc = generate_crc8(control_command_buffer); + if (!mcm_control_control_command_pack( + control_command_buffer, &mcm_unpacked_control_command_t, sizeof(control_command_buffer))) + { + error_message = error_message + "Could not pack mcm_unpacked_control_command_t. \n"; + return std::nullopt; + } + + std::array control_command_packed_data; + for (size_t i = 0; i < control_command_packed_data.size(); ++i) { + control_command_packed_data[i] = control_command_buffer[i]; + } + + frame.set_can_id(MCM_CONTROL_CONTROL_COMMAND_FRAME_ID); + frame.set_len(MCM_CONTROL_CONTROL_COMMAND_LENGTH); + frame.set_data(control_command_packed_data); + + return frame; +} + +std::optional SygnalControlInterface::createRelayCommandFrame( + const uint8_t bus_id, const uint8_t subsystem_id, const bool relay_state, std::string & error_message) +{ + polymath::socketcan::CanFrame frame; + uint8_t relay_command_buffer[CAN_MAX_DLC]; + mcm_relay_relay_command_t mcm_unpacked_relay_command_t; + + if (mcm_relay_relay_command_init(&mcm_unpacked_relay_command_t) != 0) { + error_message = error_message + "Unable to initialize mcm_unpacked_relay_command_t. \n"; + return std::nullopt; + } + + mcm_unpacked_relay_command_t.bus_address = bus_id; + mcm_unpacked_relay_command_t.subsystem_id = subsystem_id; + mcm_unpacked_relay_command_t.enable = static_cast(relay_state); + mcm_unpacked_relay_command_t.crc = 0; + + if (!mcm_relay_relay_command_pack(relay_command_buffer, &mcm_unpacked_relay_command_t, sizeof(relay_command_buffer))) + { + error_message = error_message + "Could not pack mcm_unpacked_relay_command_t. \n"; + return std::nullopt; + } + + // Get checksum and pack it again + mcm_unpacked_relay_command_t.crc = generate_crc8(relay_command_buffer); + if (!mcm_relay_relay_command_pack(relay_command_buffer, &mcm_unpacked_relay_command_t, sizeof(relay_command_buffer))) + { + error_message = error_message + "Could not pack mcm_unpacked_relay_command_t. \n"; + return std::nullopt; + } + + std::array mcm_relay_command_packed_data; + for (size_t i = 0; i < mcm_relay_command_packed_data.size(); ++i) { + mcm_relay_command_packed_data[i] = relay_command_buffer[i]; + } + + frame.set_can_id(MCM_RELAY_RELAY_COMMAND_FRAME_ID); + frame.set_len(MCM_RELAY_RELAY_COMMAND_LENGTH); + frame.set_data(mcm_relay_command_packed_data); + + return frame; +} + +std::optional SygnalControlInterface::parseCommandResponseFrame( + const socketcan::CanFrame & frame) +{ + auto frame_copy = frame.get_frame(); + uint32_t frame_id = frame.get_id(); + + // Check CRC first for all supported frame types + if (!check_crc8(reinterpret_cast(frame_copy.data))) { + return std::nullopt; + } + + SygnalControlCommandResponse response; + + switch (frame_id) { + case MCM_CONTROL_CONTROL_ENABLE_RESPONSE_FRAME_ID: { + mcm_control_control_enable_response_t unpacked; + if (mcm_control_control_enable_response_init(&unpacked) != 0) { + return std::nullopt; + } + if (mcm_control_control_enable_response_unpack(&unpacked, frame_copy.data, frame_copy.len) != 0) { + return std::nullopt; + } + response.response_type = SygnalControlCommandResponseType::ENABLE; + response.bus_id = unpacked.bus_address; + response.interface_id = unpacked.interface_id; + response.value = static_cast(unpacked.enable); + return response; + } + + case MCM_CONTROL_CONTROL_COMMAND_RESPONSE_FRAME_ID: { + mcm_control_control_command_response_t unpacked; + if (mcm_control_control_command_response_init(&unpacked) != 0) { + return std::nullopt; + } + if (mcm_control_control_command_response_unpack(&unpacked, frame_copy.data, frame_copy.len) != 0) { + return std::nullopt; + } + response.response_type = SygnalControlCommandResponseType::CONTROL; + response.bus_id = unpacked.bus_address; + response.interface_id = unpacked.interface_id; + response.value = mcm_control_control_command_response_value_decode(unpacked.value); + return response; + } + + case MCM_RELAY_RELAY_COMMAND_RESPONSE_FRAME_ID: { + mcm_relay_relay_command_response_t unpacked; + if (mcm_relay_relay_command_response_init(&unpacked) != 0) { + return std::nullopt; + } + if (mcm_relay_relay_command_response_unpack(&unpacked, frame_copy.data, frame_copy.len) != 0) { + return std::nullopt; + } + response.response_type = SygnalControlCommandResponseType::RELAY; + response.bus_id = unpacked.bus_address; + response.interface_id = unpacked.subsystem_id; + response.value = static_cast(unpacked.enable); + return response; + } + + default: + return std::nullopt; + } +} + +} // namespace polymath::sygnal 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 new file mode 100644 index 0000000..1bf5783 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_mcm_interface.cpp @@ -0,0 +1,104 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// Proprietary. Any unauthorized copying, distribution, or modification of this software is strictly prohibited. + +#include "sygnal_can_interface_lib/sygnal_mcm_interface.hpp" + +#include "sygnal_dbc/mcm_heartbeat.h" + +namespace polymath::sygnal +{ + +namespace +{ + +/// @brief Generate the crc8 checksum assuming 8 byte data with crc at data[7] +/// @param data Data buffer to calculate the checksum for +/// @return crc8 checksum +uint8_t generate_crc8(uint8_t * data) +{ + uint8_t crc = 0x00; + size_t i, j; + // Assumes length 8 data with last bit being checksum + for (i = 0; i < 7; i++) { + crc ^= data[i]; + for (j = 0; j < 8; j++) { + if ((crc & 0x80) != 0) { + crc = (uint8_t)((crc << 1) ^ 0x07); + } else { + crc <<= 1; + } + } + } + return crc; +} + +/// @brief Check the crc8 checksum of incoming data assuming 8 byte data with crc at data[7] +/// @param data Data buffer to check +/// @return true if checksum matches, false otherwise +bool check_crc8(uint8_t * data) +{ + return data[7] == generate_crc8(data); +} + +} // namespace + +SygnalMcmInterface::SygnalMcmInterface() +: sygnal_mcm_0_state_(SygnalSystemState::FAIL_HARD) +, sygnal_mcm_1_state_(SygnalSystemState::FAIL_HARD) +{ + sygnal_interface_states_.fill(SygnalSystemState::FAIL_HARD); +} + +bool SygnalMcmInterface::parseMcmHeartbeatFrame(const socketcan::CanFrame & frame) +{ + if (frame.get_id() != MCM_HEARTBEAT_HEARTBEAT_FRAME_ID) { + return false; + } + + auto frame_copy = frame.get_frame(); + + if (!check_crc8(reinterpret_cast(frame_copy.data))) { + return false; + } + + mcm_heartbeat_heartbeat_t unpacked_heartbeat_t; + if (mcm_heartbeat_heartbeat_init(&unpacked_heartbeat_t) != 0) { + return false; + } + + if (mcm_heartbeat_heartbeat_unpack(&unpacked_heartbeat_t, frame_copy.data, frame_copy.len) != 0) { + return false; + } + + // Set the sygnal states based on subsystem_id + if (unpacked_heartbeat_t.subsystem_id == 0) { + sygnal_mcm_0_state_ = SygnalSystemState(unpacked_heartbeat_t.system_state); + } else if (unpacked_heartbeat_t.subsystem_id == 1) { + sygnal_mcm_1_state_ = SygnalSystemState(unpacked_heartbeat_t.system_state); + } + + // Get interface states + sygnal_interface_states_[0] = SygnalSystemState(unpacked_heartbeat_t.interface0_state); + sygnal_interface_states_[1] = SygnalSystemState(unpacked_heartbeat_t.interface1_state); + sygnal_interface_states_[2] = SygnalSystemState(unpacked_heartbeat_t.interface2_state); + sygnal_interface_states_[3] = SygnalSystemState(unpacked_heartbeat_t.interface3_state); + sygnal_interface_states_[4] = SygnalSystemState(unpacked_heartbeat_t.interface4_state); + + return true; +} + +} // namespace polymath::sygnal From 7e2cf634ea7c31546da51cf9c92474e87f27c521 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 22 Jan 2026 01:03:25 +0000 Subject: [PATCH 02/15] Clean up interfaec --- sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt | 3 +-- .../sygnal_can_interface_lib/sygnal_command_interface.hpp | 3 --- .../include/sygnal_can_interface_lib/sygnal_mcm_interface.hpp | 3 --- .../sygnal_can_interface_lib/src/sygnal_command_interface.cpp | 3 --- .../sygnal_can_interface_lib/src/sygnal_mcm_interface.cpp | 3 --- 5 files changed, 1 insertion(+), 14 deletions(-) diff --git a/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt b/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt index 943a14d..24b0f58 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt +++ b/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt @@ -11,8 +11,7 @@ # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. -# Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved -# Proprietary. Any unauthorized copying, distribution, or modification of this software is strictly prohibited. + cmake_minimum_required(VERSION 3.8) project(sygnal_can_interface_lib) 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 4594e06..0d1175a 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 @@ -12,9 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved -// Proprietary. Any unauthorized copying, distribution, or modification of this software is strictly prohibited. - #ifndef SYGNAL_CAN_INTERFACE_LIB__SYGNAL_COMMAND_INTERFACE_HPP_ #define SYGNAL_CAN_INTERFACE_LIB__SYGNAL_COMMAND_INTERFACE_HPP_ 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 861fa09..828f712 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 @@ -12,9 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved -// Proprietary. Any unauthorized copying, distribution, or modification of this software is strictly prohibited. - #ifndef SYGNAL_CAN_INTERFACE_LIB__SYGNAL_MCM_INTERFACE_HPP_ #define SYGNAL_CAN_INTERFACE_LIB__SYGNAL_MCM_INTERFACE_HPP_ 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 1699c52..e5605ff 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 @@ -12,9 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved -// Proprietary. Any unauthorized copying, distribution, or modification of this software is strictly prohibited. - #include "sygnal_can_interface_lib/sygnal_command_interface.hpp" #include 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 1bf5783..b9db17d 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 @@ -12,9 +12,6 @@ // See the License for the specific language governing permissions and // limitations under the License. -// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved -// Proprietary. Any unauthorized copying, distribution, or modification of this software is strictly prohibited. - #include "sygnal_can_interface_lib/sygnal_mcm_interface.hpp" #include "sygnal_dbc/mcm_heartbeat.h" From f69f12d6a3594667e2cd38668a8b7860e4d9d4a2 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 22 Jan 2026 01:37:29 +0000 Subject: [PATCH 03/15] Add in sygnal socketcan connection as well as tests for the non socketcan pieces --- .../sygnal_can_interface_lib/CMakeLists.txt | 23 +++ .../include/sygnal_can_interface_lib/crc8.hpp | 35 ++++ .../sygnal_interface_socketcan.hpp | 110 +++++++++++ .../sygnal_can_interface_lib/src/crc8.cpp | 44 +++++ .../src/sygnal_command_interface.cpp | 35 +--- .../src/sygnal_interface_socketcan.cpp | 170 +++++++++++++++++ .../src/sygnal_mcm_interface.cpp | 35 +--- .../test/sygnal_command_interface_test.cpp | 175 ++++++++++++++++++ .../test/sygnal_mcm_interface_test.cpp | 123 ++++++++++++ 9 files changed, 682 insertions(+), 68 deletions(-) create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/crc8.hpp create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/src/crc8.cpp create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_command_interface_test.cpp create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_mcm_interface_test.cpp diff --git a/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt b/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt index 24b0f58..1e3ebc9 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt +++ b/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt @@ -30,8 +30,10 @@ find_package(sygnal_dbc REQUIRED) add_library( ${PROJECT_NAME} SHARED + src/crc8.cpp src/sygnal_mcm_interface.cpp src/sygnal_command_interface.cpp + src/sygnal_interface_socketcan.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC @@ -71,4 +73,25 @@ ament_export_dependencies( sygnal_dbc ) +if(BUILD_TESTING) + find_package(Catch2 2 REQUIRED) + include(Catch) + + add_executable(sygnal_mcm_interface_tests + test/sygnal_mcm_interface_test.cpp + ) + target_link_libraries(sygnal_mcm_interface_tests + PRIVATE ${PROJECT_NAME} Catch2::Catch2WithMain + ) + catch_discover_tests(sygnal_mcm_interface_tests) + + add_executable(sygnal_command_interface_tests + test/sygnal_command_interface_test.cpp + ) + target_link_libraries(sygnal_command_interface_tests + PRIVATE ${PROJECT_NAME} Catch2::Catch2WithMain + ) + catch_discover_tests(sygnal_command_interface_tests) +endif() + ament_package() diff --git a/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/crc8.hpp b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/crc8.hpp new file mode 100644 index 0000000..0458776 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/crc8.hpp @@ -0,0 +1,35 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SYGNAL_CAN_INTERFACE_LIB__CRC8_HPP_ +#define SYGNAL_CAN_INTERFACE_LIB__CRC8_HPP_ + +#include + +namespace polymath::sygnal +{ + +/// @brief Generate CRC8 checksum for 8-byte CAN data (checksum at data[7]) +/// @param data Data buffer to calculate the checksum for +/// @return CRC8 checksum +uint8_t generate_crc8(uint8_t * data); + +/// @brief Verify CRC8 checksum of 8-byte CAN data (checksum at data[7]) +/// @param data Data buffer to check +/// @return true if checksum matches, false otherwise +bool check_crc8(uint8_t * data); + +} // namespace polymath::sygnal + +#endif // SYGNAL_CAN_INTERFACE_LIB__CRC8_HPP_ 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 new file mode 100644 index 0000000..64c17cf --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp @@ -0,0 +1,110 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef SYGNAL_CAN_INTERFACE_LIB__SYGNAL_INTERFACE_SOCKETCAN_HPP_ +#define SYGNAL_CAN_INTERFACE_LIB__SYGNAL_INTERFACE_SOCKETCAN_HPP_ + +#include +#include +#include +#include +#include +#include + +#include "socketcan_adapter/socketcan_adapter.hpp" +#include "sygnal_can_interface_lib/sygnal_command_interface.hpp" +#include "sygnal_can_interface_lib/sygnal_mcm_interface.hpp" + +namespace polymath::sygnal +{ + +/// @brief Result of a send command operation +struct SendCommandResult +{ + bool success; + std::optional> response_future; +}; + +/// @brief Combined Sygnal MCM and Control interface with SocketCAN communication +/// Provides thread-safe async communication with promise/future pattern for responses +class SygnalInterfaceSocketcan +{ +public: + /// @brief Constructor + /// @param socketcan_adapter Shared pointer to socketcan adapter for CAN communication + explicit SygnalInterfaceSocketcan(std::shared_ptr socketcan_adapter); + + /// @brief Parse incoming CAN frame for MCM heartbeat and command responses + /// @param frame CAN frame to parse + void parse(const socketcan::CanFrame & frame); + + /// @brief Get interface states array + std::array get_interface_states() 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; + + /// @brief Send control state (enable/disable) command + /// @param bus_id Bus address + /// @param interface_id Interface to enable/disable + /// @param control_state MCM_CONTROL or HUMAN_CONTROL + /// @param expect_reply If true, returns future for response; if false, fire-and-forget + /// @param error_message Populated on failure + /// @return Result with success flag and optional response future + SendCommandResult sendControlStateCommand( + uint8_t bus_id, + uint8_t interface_id, + SygnalControlState control_state, + bool expect_reply, + std::string & error_message); + + /// @brief Send control command with value + /// @param bus_id Bus address + /// @param interface_id Interface to control + /// @param value Control value + /// @param expect_reply If true, returns future for response; if false, fire-and-forget + /// @param error_message Populated on failure + /// @return Result with success flag and optional response future + SendCommandResult sendControlCommand( + uint8_t bus_id, uint8_t interface_id, double value, bool expect_reply, std::string & error_message); + + /// @brief Send relay command + /// @param bus_id Bus address + /// @param subsystem_id Subsystem/relay to control + /// @param relay_state Enable or disable + /// @param expect_reply If true, returns future for response; if false, fire-and-forget + /// @param error_message Populated on failure + /// @return Result with success flag and optional response future + SendCommandResult sendRelayCommand( + uint8_t bus_id, uint8_t subsystem_id, bool relay_state, bool expect_reply, std::string & error_message); + +private: + std::shared_ptr socketcan_adapter_; + SygnalMcmInterface mcm_interface_; + SygnalControlInterface control_interface_; + + // Promise queues for each response type + std::queue> enable_response_promises_; + std::queue> control_response_promises_; + std::queue> relay_response_promises_; + + std::mutex promises_mutex_; +}; + +} // namespace polymath::sygnal + +#endif // SYGNAL_CAN_INTERFACE_LIB__SYGNAL_INTERFACE_SOCKETCAN_HPP_ diff --git a/sygnal_can_interface/sygnal_can_interface_lib/src/crc8.cpp b/sygnal_can_interface/sygnal_can_interface_lib/src/crc8.cpp new file mode 100644 index 0000000..a73b0bc --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/src/crc8.cpp @@ -0,0 +1,44 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "sygnal_can_interface_lib/crc8.hpp" + +#include + +namespace polymath::sygnal +{ + +uint8_t generate_crc8(uint8_t * data) +{ + uint8_t crc = 0x00; + // Assumes 8-byte data with checksum at data[7] + for (size_t i = 0; i < 7; i++) { + crc ^= data[i]; + for (size_t j = 0; j < 8; j++) { + if ((crc & 0x80) != 0) { + crc = static_cast((crc << 1) ^ 0x07); + } else { + crc <<= 1; + } + } + } + return crc; +} + +bool check_crc8(uint8_t * data) +{ + return data[7] == generate_crc8(data); +} + +} // namespace polymath::sygnal 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 e5605ff..22c51b6 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 @@ -16,46 +16,13 @@ #include +#include "sygnal_can_interface_lib/crc8.hpp" #include "sygnal_dbc/mcm_control.h" #include "sygnal_dbc/mcm_relay.h" namespace polymath::sygnal { -namespace -{ - -/// @brief Generate the crc8 checksum assuming 8 byte data with crc at data[7] -/// @param data Data buffer to calculate the checksum for -/// @return crc8 checksum -uint8_t generate_crc8(uint8_t * data) -{ - uint8_t crc = 0x00; - size_t i, j; - // Assumes length 8 data with last bit being checksum - for (i = 0; i < 7; i++) { - crc ^= data[i]; - for (j = 0; j < 8; j++) { - if ((crc & 0x80) != 0) { - crc = (uint8_t)((crc << 1) ^ 0x07); - } else { - crc <<= 1; - } - } - } - return crc; -} - -/// @brief Check the crc8 checksum of incoming data assuming 8 byte data with crc at data[7] -/// @param data Data buffer to check -/// @return true if checksum matches, false otherwise -bool check_crc8(uint8_t * data) -{ - return data[7] == generate_crc8(data); -} - -} // namespace - SygnalControlInterface::SygnalControlInterface() {} 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 new file mode 100644 index 0000000..edc4563 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp @@ -0,0 +1,170 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "sygnal_can_interface_lib/sygnal_interface_socketcan.hpp" + +#include +#include +#include + +namespace polymath::sygnal +{ + +SygnalInterfaceSocketcan::SygnalInterfaceSocketcan(std::shared_ptr socketcan_adapter) +: socketcan_adapter_(std::move(socketcan_adapter)) +, mcm_interface_() +, control_interface_() +{} + +void SygnalInterfaceSocketcan::parse(const socketcan::CanFrame & frame) +{ + // Try parsing as MCM heartbeat + mcm_interface_.parseMcmHeartbeatFrame(frame); + + // Try parsing as command response + auto response = control_interface_.parseCommandResponseFrame(frame); + if (!response.has_value()) { + return; + } + + // Fulfill the appropriate promise based on response type + std::lock_guard lock(promises_mutex_); + + switch (response->response_type) { + case SygnalControlCommandResponseType::ENABLE: { + if (!enable_response_promises_.empty()) { + auto promise = std::move(enable_response_promises_.front()); + enable_response_promises_.pop(); + promise.set_value(*response); + } + break; + } + case SygnalControlCommandResponseType::CONTROL: { + if (!control_response_promises_.empty()) { + auto promise = std::move(control_response_promises_.front()); + control_response_promises_.pop(); + promise.set_value(*response); + } + break; + } + case SygnalControlCommandResponseType::RELAY: { + if (!relay_response_promises_.empty()) { + auto promise = std::move(relay_response_promises_.front()); + relay_response_promises_.pop(); + promise.set_value(*response); + } + break; + } + } +} + +std::array SygnalInterfaceSocketcan::get_interface_states() const +{ + return mcm_interface_.get_interface_states(); +} + +SygnalSystemState SygnalInterfaceSocketcan::get_sygnal_mcm0_state() const +{ + return mcm_interface_.get_sygnal_mcm0_state(); +} + +SygnalSystemState SygnalInterfaceSocketcan::get_sygnal_mcm1_state() const +{ + return mcm_interface_.get_sygnal_mcm1_state(); +} + +SendCommandResult SygnalInterfaceSocketcan::sendControlStateCommand( + uint8_t bus_id, + uint8_t interface_id, + SygnalControlState control_state, + bool expect_reply, + std::string & error_message) +{ + auto frame_opt = + control_interface_.createControlStateCommandFrame(bus_id, interface_id, control_state, error_message); + + if (!frame_opt.has_value()) { + return {false, std::nullopt}; + } + + std::optional> future_opt; + if (expect_reply) { + std::promise promise; + future_opt = promise.get_future(); + std::lock_guard lock(promises_mutex_); + enable_response_promises_.push(std::move(promise)); + } + + auto err = socketcan_adapter_->send(*frame_opt); + if (err.has_value()) { + error_message += "Failed to send control state command: " + err.value() + "\n"; + return {false, std::nullopt}; + } + + return {true, std::move(future_opt)}; +} + +SendCommandResult SygnalInterfaceSocketcan::sendControlCommand( + uint8_t bus_id, uint8_t interface_id, double value, bool expect_reply, std::string & error_message) +{ + auto frame_opt = control_interface_.createControlCommandFrame(bus_id, interface_id, value, error_message); + + if (!frame_opt.has_value()) { + return {false, std::nullopt}; + } + + std::optional> future_opt; + if (expect_reply) { + std::promise promise; + future_opt = promise.get_future(); + std::lock_guard lock(promises_mutex_); + control_response_promises_.push(std::move(promise)); + } + + auto err = socketcan_adapter_->send(*frame_opt); + if (err.has_value()) { + error_message += "Failed to send control command: " + err.value() + "\n"; + return {false, std::nullopt}; + } + + return {true, std::move(future_opt)}; +} + +SendCommandResult SygnalInterfaceSocketcan::sendRelayCommand( + uint8_t bus_id, uint8_t subsystem_id, bool relay_state, bool expect_reply, std::string & error_message) +{ + auto frame_opt = control_interface_.createRelayCommandFrame(bus_id, subsystem_id, relay_state, error_message); + + if (!frame_opt.has_value()) { + return {false, std::nullopt}; + } + + std::optional> future_opt; + if (expect_reply) { + std::promise promise; + future_opt = promise.get_future(); + std::lock_guard lock(promises_mutex_); + relay_response_promises_.push(std::move(promise)); + } + + auto err = socketcan_adapter_->send(*frame_opt); + if (err.has_value()) { + error_message += "Failed to send relay command: " + err.value() + "\n"; + return {false, std::nullopt}; + } + + return {true, std::move(future_opt)}; +} + +} // namespace polymath::sygnal 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 b9db17d..24e40f2 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 @@ -14,45 +14,12 @@ #include "sygnal_can_interface_lib/sygnal_mcm_interface.hpp" +#include "sygnal_can_interface_lib/crc8.hpp" #include "sygnal_dbc/mcm_heartbeat.h" namespace polymath::sygnal { -namespace -{ - -/// @brief Generate the crc8 checksum assuming 8 byte data with crc at data[7] -/// @param data Data buffer to calculate the checksum for -/// @return crc8 checksum -uint8_t generate_crc8(uint8_t * data) -{ - uint8_t crc = 0x00; - size_t i, j; - // Assumes length 8 data with last bit being checksum - for (i = 0; i < 7; i++) { - crc ^= data[i]; - for (j = 0; j < 8; j++) { - if ((crc & 0x80) != 0) { - crc = (uint8_t)((crc << 1) ^ 0x07); - } else { - crc <<= 1; - } - } - } - return crc; -} - -/// @brief Check the crc8 checksum of incoming data assuming 8 byte data with crc at data[7] -/// @param data Data buffer to check -/// @return true if checksum matches, false otherwise -bool check_crc8(uint8_t * data) -{ - return data[7] == generate_crc8(data); -} - -} // namespace - SygnalMcmInterface::SygnalMcmInterface() : sygnal_mcm_0_state_(SygnalSystemState::FAIL_HARD) , sygnal_mcm_1_state_(SygnalSystemState::FAIL_HARD) diff --git a/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_command_interface_test.cpp b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_command_interface_test.cpp new file mode 100644 index 0000000..c9cb969 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_command_interface_test.cpp @@ -0,0 +1,175 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "sygnal_can_interface_lib/sygnal_command_interface.hpp" + +#include +#include + +#include + +#include "socketcan_adapter/can_frame.hpp" + +// Frame IDs from DBC +constexpr uint32_t CONTROL_ENABLE_FRAME_ID = 0x060; +constexpr uint32_t CONTROL_ENABLE_RESPONSE_FRAME_ID = 0x061; +constexpr uint32_t CONTROL_COMMAND_FRAME_ID = 0x160; +constexpr uint32_t CONTROL_COMMAND_RESPONSE_FRAME_ID = 0x161; +constexpr uint32_t RELAY_COMMAND_FRAME_ID = 0x0B0; +constexpr uint32_t RELAY_COMMAND_RESPONSE_FRAME_ID = 0x0B1; + +static polymath::socketcan::CanFrame createTestFrame(uint32_t can_id, const std::vector & data) +{ + polymath::socketcan::CanFrame frame; + frame.set_can_id(can_id); + std::array frame_data; + frame_data.fill(0); + for (size_t i = 0; i < data.size() && i < CAN_MAX_DLC; ++i) { + frame_data[i] = data[i]; + } + frame.set_data(frame_data); + frame.set_len(data.size()); + return frame; +} + +TEST_CASE("SygnalControlInterface creates ControlEnable frame", "[sygnal_command_interface]") +{ + polymath::sygnal::SygnalControlInterface interface; + std::string error_message; + + auto frame_opt = + interface.createControlStateCommandFrame(1, 3, polymath::sygnal::SygnalControlState::MCM_CONTROL, error_message); + + REQUIRE(frame_opt.has_value()); + REQUIRE(error_message.empty()); + + auto frame = frame_opt.value(); + REQUIRE(frame.get_id() == CONTROL_ENABLE_FRAME_ID); + + // Expected from DBC: {0x01, 0x60, 0x01, 0x00, 0x00, 0x00, 0x00, 0xF0} + auto data = frame.get_data(); + REQUIRE(data[0] == 0x01); // BusAddress = 1 + REQUIRE((data[1] & 0x0F) == 0x00); // InterfaceID lower bits + REQUIRE(((data[1] >> 4) & 0x0F) == 0x06); // InterfaceID = 3 encoded +} + +TEST_CASE("SygnalControlInterface creates ControlCommand frame", "[sygnal_command_interface]") +{ + polymath::sygnal::SygnalControlInterface interface; + std::string error_message; + + auto frame_opt = interface.createControlCommandFrame(1, 0, 0.5, error_message); + + REQUIRE(frame_opt.has_value()); + REQUIRE(error_message.empty()); + + auto frame = frame_opt.value(); + REQUIRE(frame.get_id() == CONTROL_COMMAND_FRAME_ID); + + auto data = frame.get_data(); + REQUIRE(data[0] == 0x01); // BusAddress = 1 +} + +TEST_CASE("SygnalControlInterface creates RelayCommand frame", "[sygnal_command_interface]") +{ + polymath::sygnal::SygnalControlInterface interface; + std::string error_message; + + auto frame_opt = interface.createRelayCommandFrame(1, 0, true, error_message); + + REQUIRE(frame_opt.has_value()); + REQUIRE(error_message.empty()); + + auto frame = frame_opt.value(); + REQUIRE(frame.get_id() == RELAY_COMMAND_FRAME_ID); + + // Expected from DBC: {0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0xBD} + auto data = frame.get_data(); + REQUIRE(data[0] == 0x01); // BusAddress = 1 + REQUIRE(data[2] == 0x01); // Enable = 1 +} + +TEST_CASE("SygnalControlInterface parses ControlEnableResponse", "[sygnal_command_interface]") +{ + polymath::sygnal::SygnalControlInterface interface; + + // Generated from DBC: ControlEnableResponse (bus=1, interface=3, enable=1) + std::vector data = {0x01, 0x60, 0x01, 0x00, 0x00, 0x00, 0x00, 0xF0}; + auto frame = createTestFrame(CONTROL_ENABLE_RESPONSE_FRAME_ID, data); + + auto response = interface.parseCommandResponseFrame(frame); + + REQUIRE(response.has_value()); + REQUIRE(response->response_type == polymath::sygnal::SygnalControlCommandResponseType::ENABLE); + REQUIRE(response->bus_id == 1); + REQUIRE(response->value == 1.0); +} + +TEST_CASE("SygnalControlInterface parses ControlCommandResponse", "[sygnal_command_interface]") +{ + polymath::sygnal::SygnalControlInterface interface; + + // Generated from DBC: ControlCommandResponse (bus=1, interface=0, value=0.5) + std::vector data = {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0x62}; + auto frame = createTestFrame(CONTROL_COMMAND_RESPONSE_FRAME_ID, data); + + auto response = interface.parseCommandResponseFrame(frame); + + REQUIRE(response.has_value()); + REQUIRE(response->response_type == polymath::sygnal::SygnalControlCommandResponseType::CONTROL); + REQUIRE(response->bus_id == 1); + REQUIRE(response->interface_id == 0); +} + +TEST_CASE("SygnalControlInterface parses RelayCommandResponse", "[sygnal_command_interface]") +{ + polymath::sygnal::SygnalControlInterface interface; + + // Generated from DBC: RelayCommandResponse (bus=1, subsystem=0, enable=1) + std::vector data = {0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0xBD}; + auto frame = createTestFrame(RELAY_COMMAND_RESPONSE_FRAME_ID, data); + + auto response = interface.parseCommandResponseFrame(frame); + + REQUIRE(response.has_value()); + REQUIRE(response->response_type == polymath::sygnal::SygnalControlCommandResponseType::RELAY); + REQUIRE(response->bus_id == 1); + REQUIRE(response->interface_id == 0); // subsystem_id + REQUIRE(response->value == 1.0); +} + +TEST_CASE("SygnalControlInterface rejects unknown frame ID", "[sygnal_command_interface]") +{ + polymath::sygnal::SygnalControlInterface interface; + + std::vector data = {0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0xBD}; + auto frame = createTestFrame(0x999, data); + + auto response = interface.parseCommandResponseFrame(frame); + + REQUIRE_FALSE(response.has_value()); +} + +TEST_CASE("SygnalControlInterface rejects bad CRC in response", "[sygnal_command_interface]") +{ + polymath::sygnal::SygnalControlInterface interface; + + // Valid data but corrupted CRC + std::vector data = {0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0xFF}; + auto frame = createTestFrame(RELAY_COMMAND_RESPONSE_FRAME_ID, data); + + auto response = interface.parseCommandResponseFrame(frame); + + REQUIRE_FALSE(response.has_value()); +} 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 new file mode 100644 index 0000000..095c1e3 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_mcm_interface_test.cpp @@ -0,0 +1,123 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "sygnal_can_interface_lib/sygnal_mcm_interface.hpp" + +#include +#include + +#include + +#include "socketcan_adapter/can_frame.hpp" + +// Frame ID from DBC +constexpr uint32_t HEARTBEAT_FRAME_ID = 0x170; + +static polymath::socketcan::CanFrame createTestFrame(uint32_t can_id, const std::vector & data) +{ + polymath::socketcan::CanFrame frame; + frame.set_can_id(can_id); + std::array frame_data; + frame_data.fill(0); + for (size_t i = 0; i < data.size() && i < CAN_MAX_DLC; ++i) { + frame_data[i] = data[i]; + } + frame.set_data(frame_data); + frame.set_len(data.size()); + return frame; +} + +TEST_CASE("SygnalMcmInterface constructor initializes to FAIL_HARD", "[sygnal_mcm_interface]") +{ + polymath::sygnal::SygnalMcmInterface interface; + + REQUIRE(interface.get_sygnal_mcm0_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); + REQUIRE(interface.get_sygnal_mcm1_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); + + auto states = interface.get_interface_states(); + for (const auto & state : states) { + REQUIRE(state == polymath::sygnal::SygnalSystemState::FAIL_HARD); + } +} + +TEST_CASE("SygnalMcmInterface parses MCM_CONTROL heartbeat", "[sygnal_mcm_interface]") +{ + polymath::sygnal::SygnalMcmInterface interface; + + // Generated from DBC: MCM_CONTROL state, subsystem 0, all interfaces MCM_CONTROL + std::vector data = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0x98}; + auto frame = createTestFrame(HEARTBEAT_FRAME_ID, data); + + REQUIRE(interface.parseMcmHeartbeatFrame(frame) == true); + REQUIRE(interface.get_sygnal_mcm0_state() == polymath::sygnal::SygnalSystemState::MCM_CONTROL); + + auto states = interface.get_interface_states(); + for (size_t i = 0; i < 5; ++i) { + REQUIRE(states[i] == polymath::sygnal::SygnalSystemState::MCM_CONTROL); + } +} + +TEST_CASE("SygnalMcmInterface parses HUMAN_CONTROL heartbeat", "[sygnal_mcm_interface]") +{ + polymath::sygnal::SygnalMcmInterface interface; + + // Generated from DBC: HUMAN_CONTROL state, subsystem 1 + std::vector data = {0x81, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x86}; + auto frame = createTestFrame(HEARTBEAT_FRAME_ID, data); + + REQUIRE(interface.parseMcmHeartbeatFrame(frame) == true); + REQUIRE(interface.get_sygnal_mcm1_state() == polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); + + auto states = interface.get_interface_states(); + for (size_t i = 0; i < 5; ++i) { + REQUIRE(states[i] == polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); + } +} + +TEST_CASE("SygnalMcmInterface rejects wrong frame ID", "[sygnal_mcm_interface]") +{ + polymath::sygnal::SygnalMcmInterface interface; + + std::vector data = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0x98}; + auto frame = createTestFrame(0x999, data); // Wrong ID + + REQUIRE(interface.parseMcmHeartbeatFrame(frame) == false); + // State should remain FAIL_HARD + REQUIRE(interface.get_sygnal_mcm0_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); +} + +TEST_CASE("SygnalMcmInterface rejects bad CRC", "[sygnal_mcm_interface]") +{ + polymath::sygnal::SygnalMcmInterface interface; + + // Valid data but corrupted CRC (last byte) + std::vector data = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0xFF}; + auto frame = createTestFrame(HEARTBEAT_FRAME_ID, data); + + REQUIRE(interface.parseMcmHeartbeatFrame(frame) == false); + REQUIRE(interface.get_sygnal_mcm0_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); +} + +TEST_CASE("sygnalSystemStateToString returns correct strings", "[sygnal_mcm_interface]") +{ + using polymath::sygnal::SygnalSystemState; + using polymath::sygnal::sygnalSystemStateToString; + + REQUIRE(sygnalSystemStateToString(SygnalSystemState::FAIL_HARD) == "FAIL_HARD"); + REQUIRE(sygnalSystemStateToString(SygnalSystemState::HUMAN_OVERRIDE) == "HUMAN_OVERRIDE"); + REQUIRE(sygnalSystemStateToString(SygnalSystemState::FAIL_OPERATIONAL_2) == "FAIL_OPERATIONAL_2"); + REQUIRE(sygnalSystemStateToString(SygnalSystemState::FAIL_OPERATIONAL_1) == "FAIL_OPERATIONAL_1"); + REQUIRE(sygnalSystemStateToString(SygnalSystemState::MCM_CONTROL) == "MCM_CONTROL"); + REQUIRE(sygnalSystemStateToString(SygnalSystemState::HUMAN_CONTROL) == "HUMAN_CONTROL"); +} From b7c6f3a49764bb34c217683c98ecbc35b8588d02 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 22 Jan 2026 19:51:19 +0000 Subject: [PATCH 04/15] Using mvec as a template, add sygnal can interface ros2 stuff and messages --- .../sygnal_can_interface_ros2/CMakeLists.txt | 82 +++++++ .../sygnal_can_interface_node.hpp | 120 ++++++++++ .../launch/sygnal_can_interface_launch.py | 125 ++++++++++ .../sygnal_can_interface_ros2/package.xml | 27 +++ .../src/sygnal_can_interface_node.cpp | 217 ++++++++++++++++++ .../sygnal_can_msgs/CMakeLists.txt | 48 ++++ .../sygnal_can_msgs/msg/McmHeartbeat.msg | 12 + .../sygnal_can_msgs/package.xml | 19 ++ .../srv/SendControlCommand.srv | 23 ++ .../sygnal_can_msgs/srv/SendRelayCommand.srv | 23 ++ .../sygnal_can_msgs/srv/SetControlState.srv | 23 ++ 11 files changed, 719 insertions(+) create mode 100644 sygnal_can_interface/sygnal_can_interface_ros2/CMakeLists.txt create mode 100644 sygnal_can_interface/sygnal_can_interface_ros2/include/sygnal_can_interface_ros2/sygnal_can_interface_node.hpp create mode 100644 sygnal_can_interface/sygnal_can_interface_ros2/launch/sygnal_can_interface_launch.py create mode 100644 sygnal_can_interface/sygnal_can_interface_ros2/package.xml create mode 100644 sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_node.cpp create mode 100644 sygnal_can_interface/sygnal_can_msgs/CMakeLists.txt create mode 100644 sygnal_can_interface/sygnal_can_msgs/msg/McmHeartbeat.msg create mode 100644 sygnal_can_interface/sygnal_can_msgs/package.xml create mode 100644 sygnal_can_interface/sygnal_can_msgs/srv/SendControlCommand.srv create mode 100644 sygnal_can_interface/sygnal_can_msgs/srv/SendRelayCommand.srv create mode 100644 sygnal_can_interface/sygnal_can_msgs/srv/SetControlState.srv diff --git a/sygnal_can_interface/sygnal_can_interface_ros2/CMakeLists.txt b/sygnal_can_interface/sygnal_can_interface_ros2/CMakeLists.txt new file mode 100644 index 0000000..5aa2dc8 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_ros2/CMakeLists.txt @@ -0,0 +1,82 @@ +# Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +cmake_minimum_required(VERSION 3.8) +project(sygnal_can_interface_ros2) + +# Project-wide language standards +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) +endif() + +# Warnings & link hygiene +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) + add_link_options(-Wl,-no-undefined) +endif() + +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() + +if(CMAKE_BUILD_TYPE STREQUAL "Debug") + message(STATUS "Enabling AddressSanitizer (ASAN) for Debug build") + set(ASAN_FLAGS "-fsanitize=address -fno-omit-frame-pointer") + set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ASAN_FLAGS}") + set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${ASAN_FLAGS}") + set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${ASAN_FLAGS}") + set(CMAKE_SHARED_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} ${ASAN_FLAGS}") +endif() + +add_library(${PROJECT_NAME} SHARED + src/sygnal_can_interface_node.cpp +) +target_include_directories(${PROJECT_NAME} PUBLIC + $ + $ +) +target_compile_definitions(${PROJECT_NAME} PRIVATE "SYGNAL_CAN_INTERFACE_NODE_BUILDING_LIBRARY") + +target_link_libraries(${PROJECT_NAME} PUBLIC + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle + rclcpp_components::component + ${diagnostic_msgs_TARGETS} + magic_enum::magic_enum + ${sygnal_can_msgs_TARGETS} + sygnal_can_interface_lib::sygnal_can_interface_lib + socketcan_adapter::socketcan_adapter +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "polymath::sygnal::SygnalCanInterfaceNode" + EXECUTABLE ${PROJECT_NAME}_node) + +# Installs +install(DIRECTORY include/ DESTINATION include) +install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config) +install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch) + +install(TARGETS ${PROJECT_NAME} + EXPORT ${PROJECT_NAME}_TARGETS + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +install(EXPORT ${PROJECT_NAME}_TARGETS + NAMESPACE ${PROJECT_NAME}:: + DESTINATION share/${PROJECT_NAME}/cmake +) + +ament_export_targets(${PROJECT_NAME}_TARGETS HAS_LIBRARY_TARGET) +ament_package() 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 new file mode 100644 index 0000000..dafd384 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_ros2/include/sygnal_can_interface_ros2/sygnal_can_interface_node.hpp @@ -0,0 +1,120 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#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 "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "socketcan_adapter/socketcan_adapter.hpp" +#include "sygnal_can_interface_lib/sygnal_interface_socketcan.hpp" +#include "sygnal_can_msgs/msg/mcm_heartbeat.hpp" +#include "sygnal_can_msgs/srv/send_control_command.hpp" +#include "sygnal_can_msgs/srv/send_relay_command.hpp" +#include "sygnal_can_msgs/srv/set_control_state.hpp" + +namespace polymath::sygnal +{ + +/// @brief ROS2 lifecycle node for Sygnal CAN interface control system +/// Provides MCM heartbeat publishing and command services +class SygnalCanInterfaceNode : public rclcpp_lifecycle::LifecycleNode +{ +public: + /// @brief Constructor + /// @param options Node options + explicit SygnalCanInterfaceNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions()); + + /// @brief Destructor + ~SygnalCanInterfaceNode(); + +protected: + // Lifecycle interface + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_configure( + const rclcpp_lifecycle::State & state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_activate( + const rclcpp_lifecycle::State & state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State & state) override; + + rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State & state) override; + +private: + /// @brief Timer callback to publish MCM heartbeat and diagnostics + void timerCallback(); + + /// @brief Service callback to set control state + /// @param request Service request containing control state parameters + /// @param response Service response with success flag + void setControlStateCallback( + const std::shared_ptr request, + std::shared_ptr response); + + /// @brief Service callback to send control command + /// @param request Service request containing control command parameters + /// @param response Service response with success flag + void sendControlCommandCallback( + const std::shared_ptr request, + std::shared_ptr response); + + /// @brief Service callback to send relay command + /// @param request Service request containing relay command parameters + /// @param response Service response with success flag + void sendRelayCommandCallback( + const std::shared_ptr request, + std::shared_ptr response); + + /// @brief Create diagnostics array from Sygnal status + /// @return Diagnostic array message + diagnostic_msgs::msg::DiagnosticArray createDiagnosticsMessage(); + + // Parameters + std::string can_interface_; + double publish_rate_; + std::chrono::milliseconds timeout_ms_; + + // SocketCAN and Sygnal components + std::shared_ptr socketcan_adapter_; + std::unique_ptr sygnal_interface_; + + // ROS2 components + rclcpp::TimerBase::SharedPtr timer_; + rclcpp::Service::SharedPtr set_control_state_service_; + rclcpp::Service::SharedPtr send_control_command_service_; + rclcpp::Service::SharedPtr send_relay_command_service_; + + // Publishers + rclcpp_lifecycle::LifecyclePublisher::SharedPtr diagnostics_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr mcm_heartbeat_pub_; + + // Current MCM state storage + std::optional current_mcm_state_; +}; + +} // namespace polymath::sygnal + +RCLCPP_COMPONENTS_REGISTER_NODE(polymath::sygnal::SygnalCanInterfaceNode) + +#endif // SYGNAL_CAN_INTERFACE_NODE__SYGNAL_CAN_INTERFACE_NODE_HPP_ diff --git a/sygnal_can_interface/sygnal_can_interface_ros2/launch/sygnal_can_interface_launch.py b/sygnal_can_interface/sygnal_can_interface_ros2/launch/sygnal_can_interface_launch.py new file mode 100644 index 0000000..d23efb9 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_ros2/launch/sygnal_can_interface_launch.py @@ -0,0 +1,125 @@ +# Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import LifecycleNode +from launch_ros.events.lifecycle import ChangeState +from launch_ros.event_handlers import OnStateTransition +from launch.actions import EmitEvent, RegisterEventHandler +from launch.event_handlers import OnProcessStart +from launch.events import matches_action +from launch.conditions import IfCondition +from lifecycle_msgs.msg import Transition + + +def generate_launch_description(): + # Define launch arguments + can_interface_arg = DeclareLaunchArgument( + "can_interface", + default_value="can0", + description="CAN interface to use for Sygnal communication", + ) + + publish_rate_arg = DeclareLaunchArgument( + "publish_rate", + default_value="3.0", + description="Rate in Hz to publish MCM heartbeat and diagnostics", + ) + + timeout_ms_arg = DeclareLaunchArgument( + "timeout_ms", + default_value="500", + description="Timeout in milliseconds for Sygnal communication", + ) + + auto_configure_arg = DeclareLaunchArgument( + "auto_configure", + default_value="true", + description="Automatically configure the lifecycle node", + ) + + auto_activate_arg = DeclareLaunchArgument( + "auto_activate", + default_value="true", + description="Automatically activate the lifecycle node after configuration", + ) + + # Create the Sygnal CAN Interface node + sygnal_can_interface_node = LifecycleNode( + package="sygnal_can_interface_ros2", + executable="sygnal_can_interface_ros2_node", + name="sygnal_can_interface_ros2_node", + namespace="", + parameters=[ + { + "can_interface": LaunchConfiguration("can_interface"), + "publish_rate": LaunchConfiguration("publish_rate"), + "timeout_ms": LaunchConfiguration("timeout_ms"), + }, + ], + output="screen", + ) + + # Event handler to automatically configure the node + sygnal_configure_event_handler = RegisterEventHandler( + event_handler=OnProcessStart( + target_action=sygnal_can_interface_node, + on_start=[ + EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action( + sygnal_can_interface_node + ), + transition_id=Transition.TRANSITION_CONFIGURE, + ), + ), + ], + ), + condition=IfCondition(LaunchConfiguration("auto_configure")), + ) + + # Event handler to automatically activate the node after configuration + sygnal_activate_event_handler = RegisterEventHandler( + event_handler=OnStateTransition( + target_lifecycle_node=sygnal_can_interface_node, + start_state="configuring", + goal_state="inactive", + entities=[ + EmitEvent( + event=ChangeState( + lifecycle_node_matcher=matches_action( + sygnal_can_interface_node + ), + transition_id=Transition.TRANSITION_ACTIVATE, + ), + ), + ], + ), + condition=IfCondition(LaunchConfiguration("auto_activate")), + ) + + return LaunchDescription( + [ + can_interface_arg, + publish_rate_arg, + timeout_ms_arg, + auto_configure_arg, + auto_activate_arg, + sygnal_can_interface_node, + sygnal_configure_event_handler, + sygnal_activate_event_handler, + ] + ) diff --git a/sygnal_can_interface/sygnal_can_interface_ros2/package.xml b/sygnal_can_interface/sygnal_can_interface_ros2/package.xml new file mode 100644 index 0000000..9e085f9 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_ros2/package.xml @@ -0,0 +1,27 @@ + + + + sygnal_can_interface_ros2 + 0.0.0 + ROS2 node for Sygnal CAN interface control system with MCM heartbeat publishing and command services + Polymath Robotics Engineering + Apache-2.0 + + ament_cmake + ament_cmake_auto + + rclcpp + rclcpp_lifecycle + rclcpp_components + diagnostic_msgs + magic_enum + sygnal_can_msgs + sygnal_can_interface_lib + socketcan_adapter + + ament_cmake_test + + + ament_cmake + + 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 new file mode 100644 index 0000000..4442090 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_node.cpp @@ -0,0 +1,217 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "sygnal_can_interface_ros2/sygnal_can_interface_node.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace polymath::sygnal +{ + +SygnalCanInterfaceNode::SygnalCanInterfaceNode(const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("sygnal_can_interface_node", "", options) +{ + // Declare parameters + declare_parameter("can_interface", std::string("can0")); + declare_parameter("publish_rate", 3.0); // Hz + declare_parameter("timeout_ms", 500); // ms +} + +SygnalCanInterfaceNode::~SygnalCanInterfaceNode() +{ + if (socketcan_adapter_) { + socketcan_adapter_->joinReceptionThread(); + socketcan_adapter_->closeSocket(); + } +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SygnalCanInterfaceNode::on_configure( + const rclcpp_lifecycle::State &) +{ + // Get parameters + can_interface_ = get_parameter("can_interface").as_string(); + publish_rate_ = get_parameter("publish_rate").as_double(); + auto timeout_ms = get_parameter("timeout_ms").as_int(); + timeout_ms_ = std::chrono::milliseconds(timeout_ms); + + RCLCPP_INFO(get_logger(), "Configuring Sygnal CAN Interface node with CAN interface: %s", can_interface_.c_str()); + + try { + // Initialize SocketCAN adapter + socketcan_adapter_ = std::make_shared(can_interface_); + + if (!socketcan_adapter_->openSocket()) { + RCLCPP_ERROR(get_logger(), "Failed to open SocketCAN interface: %s", can_interface_.c_str()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + + // Initialize Sygnal Interface SocketCAN controller + sygnal_interface_ = std::make_unique(socketcan_adapter_); + + // Set up callback to parse incoming messages + socketcan_adapter_->setOnReceiveCallback( + [this](std::unique_ptr frame) { sygnal_interface_->parse(*frame); }); + + // Start receiving messages + if (!socketcan_adapter_->startReceptionThread()) { + RCLCPP_ERROR(get_logger(), "Failed to start SocketCAN reception thread"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } + + // Create publishers + diagnostics_pub_ = create_publisher("/diagnostics", rclcpp::QoS(10)); + mcm_heartbeat_pub_ = create_publisher("~/mcm_heartbeat", rclcpp::QoS(10)); + + // Create services + set_control_state_service_ = create_service( + "~/set_control_state", + std::bind(&SygnalCanInterfaceNode::setControlStateCallback, this, std::placeholders::_1, std::placeholders::_2)); + + send_control_command_service_ = create_service( + "~/send_control_command", + std::bind( + &SygnalCanInterfaceNode::sendControlCommandCallback, this, std::placeholders::_1, std::placeholders::_2)); + + send_relay_command_service_ = create_service( + "~/send_relay_command", + std::bind(&SygnalCanInterfaceNode::sendRelayCommandCallback, this, std::placeholders::_1, std::placeholders::_2)); + + // Create timer for publishing + auto period = std::chrono::duration(1.0 / publish_rate_); + timer_ = create_wall_timer( + std::chrono::duration_cast(period), + std::bind(&SygnalCanInterfaceNode::timerCallback, this)); + + RCLCPP_INFO(get_logger(), "Sygnal CAN Interface node configured successfully"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; + } catch (const std::exception & e) { + RCLCPP_ERROR(get_logger(), "Exception during configuration: %s", e.what()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SygnalCanInterfaceNode::on_activate( + const rclcpp_lifecycle::State &) +{ + // Activate publishers + diagnostics_pub_->on_activate(); + mcm_heartbeat_pub_->on_activate(); + + RCLCPP_INFO(get_logger(), "Sygnal CAN Interface node activated"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SygnalCanInterfaceNode::on_deactivate( + const rclcpp_lifecycle::State &) +{ + // Deactivate publishers + diagnostics_pub_->on_deactivate(); + mcm_heartbeat_pub_->on_deactivate(); + + RCLCPP_INFO(get_logger(), "Sygnal CAN Interface node deactivated"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SygnalCanInterfaceNode::on_cleanup( + const rclcpp_lifecycle::State &) +{ + // Clean up resources + timer_.reset(); + set_control_state_service_.reset(); + send_control_command_service_.reset(); + send_relay_command_service_.reset(); + diagnostics_pub_.reset(); + mcm_heartbeat_pub_.reset(); + + if (socketcan_adapter_) { + socketcan_adapter_->joinReceptionThread(); + socketcan_adapter_->closeSocket(); + } + + sygnal_interface_.reset(); + socketcan_adapter_.reset(); + + RCLCPP_INFO(get_logger(), "Sygnal CAN Interface node cleaned up"); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +void SygnalCanInterfaceNode::timerCallback() +{ + /// TODO: (Zeerek) Get MCM state from sygnal_interface_ and publish + /// TODO: (Zeerek) Create and publish diagnostics message + RCLCPP_DEBUG(get_logger(), "Timer callback - publishing MCM heartbeat and diagnostics"); +} + +void SygnalCanInterfaceNode::setControlStateCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + /// TODO: (Zeerek) Implement control state command + RCLCPP_INFO( + get_logger(), + "Set control state service called - bus_id: %d, interface_id: %d, state: %d", + request->bus_id, + request->interface_id, + request->control_state); + response->success = false; + response->message = "Not implemented yet"; +} + +void SygnalCanInterfaceNode::sendControlCommandCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + /// TODO: (Zeerek) Implement control command + RCLCPP_INFO( + get_logger(), + "Send control command service called - bus_id: %d, interface_id: %d, value: %f", + request->bus_id, + request->interface_id, + request->value); + response->success = false; + response->message = "Not implemented yet"; +} + +void SygnalCanInterfaceNode::sendRelayCommandCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + /// TODO: (Zeerek) Implement relay command + RCLCPP_INFO( + get_logger(), + "Send relay command service called - bus_id: %d, subsystem_id: %d, state: %d", + request->bus_id, + request->subsystem_id, + request->relay_state); + response->success = false; + response->message = "Not implemented yet"; +} + +diagnostic_msgs::msg::DiagnosticArray SygnalCanInterfaceNode::createDiagnosticsMessage() +{ + /// TODO: (Zeerek) Implement diagnostics message creation + diagnostic_msgs::msg::DiagnosticArray diagnostics; + diagnostics.header.stamp = now(); + return diagnostics; +} + +} // namespace polymath::sygnal diff --git a/sygnal_can_interface/sygnal_can_msgs/CMakeLists.txt b/sygnal_can_interface/sygnal_can_msgs/CMakeLists.txt new file mode 100644 index 0000000..c6ffeaa --- /dev/null +++ b/sygnal_can_interface/sygnal_can_msgs/CMakeLists.txt @@ -0,0 +1,48 @@ +# Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required(VERSION 3.8) +project(sygnal_can_msgs) + +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(std_msgs REQUIRED) + +set(msg_files + "msg/McmHeartbeat.msg" +) + +set(srv_files + "srv/SetControlState.srv" + "srv/SendControlCommand.srv" + "srv/SendRelayCommand.srv" +) + +set(msg_dependencies + rosidl_default_generators + builtin_interfaces + std_msgs +) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${msg_files} + ${srv_files} + DEPENDENCIES ${msg_dependencies} +) + +install(DIRECTORY msg DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY srv DESTINATION share/${PROJECT_NAME}) + +ament_package() diff --git a/sygnal_can_interface/sygnal_can_msgs/msg/McmHeartbeat.msg b/sygnal_can_interface/sygnal_can_msgs/msg/McmHeartbeat.msg new file mode 100644 index 0000000..13e5874 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_msgs/msg/McmHeartbeat.msg @@ -0,0 +1,12 @@ +# MCM heartbeat message containing system state information +# Header for timestamp +std_msgs/Header header + +# System state for MCM subsystem 0 (0-255) +uint8 mcm0_state + +# System state for MCM subsystem 1 (0-255) +uint8 mcm1_state + +# Interface states (5 interfaces, values 0-255 each) +uint8[] interface_states diff --git a/sygnal_can_interface/sygnal_can_msgs/package.xml b/sygnal_can_interface/sygnal_can_msgs/package.xml new file mode 100644 index 0000000..201f4a7 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_msgs/package.xml @@ -0,0 +1,19 @@ + + + + sygnal_can_msgs + 0.1.0 + Message and service definitions for Sygnal CAN interface control system + Polymath Robotics Engineering + Apache-2.0 + + ament_cmake + + rosidl_default_generators + + rosidl_interface_packages + + + ament_cmake + + diff --git a/sygnal_can_interface/sygnal_can_msgs/srv/SendControlCommand.srv b/sygnal_can_interface/sygnal_can_msgs/srv/SendControlCommand.srv new file mode 100644 index 0000000..861f7e1 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_msgs/srv/SendControlCommand.srv @@ -0,0 +1,23 @@ +# Service to send a control command with a value +# Request + +# Bus ID (0-127) +uint8 bus_id + +# Interface ID (0-4) +uint8 interface_id + +# Control value (interpretation depends on interface) +float64 value + +# Whether to expect a reply +bool expect_reply + +--- +# Response + +# Success flag - true if command was acknowledged +bool success + +# Optional message describing result or error +string message diff --git a/sygnal_can_interface/sygnal_can_msgs/srv/SendRelayCommand.srv b/sygnal_can_interface/sygnal_can_msgs/srv/SendRelayCommand.srv new file mode 100644 index 0000000..003b88d --- /dev/null +++ b/sygnal_can_interface/sygnal_can_msgs/srv/SendRelayCommand.srv @@ -0,0 +1,23 @@ +# Service to send a relay command (enable/disable) +# Request + +# Bus ID (0-127) +uint8 bus_id + +# Subsystem ID +uint8 subsystem_id + +# Relay state: 0 = DISABLE, 1 = ENABLE +uint8 relay_state + +# Whether to expect a reply +bool expect_reply + +--- +# Response + +# Success flag - true if command was acknowledged +bool success + +# Optional message describing result or error +string message diff --git a/sygnal_can_interface/sygnal_can_msgs/srv/SetControlState.srv b/sygnal_can_interface/sygnal_can_msgs/srv/SetControlState.srv new file mode 100644 index 0000000..135004f --- /dev/null +++ b/sygnal_can_interface/sygnal_can_msgs/srv/SetControlState.srv @@ -0,0 +1,23 @@ +# Service to set control state (MCM_CONTROL or HUMAN_CONTROL) +# Request + +# Bus ID (0-127) +uint8 bus_id + +# Interface ID (0-4) +uint8 interface_id + +# Control state: 0 = HUMAN_CONTROL, 1 = MCM_CONTROL +uint8 control_state + +# Whether to expect a reply +bool expect_reply + +--- +# Response + +# Success flag - true if command was acknowledged +bool success + +# Optional message describing result or error +string message From 8b68166e621ae5a9fb9f20dc9d2b758763f7033d Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 22 Jan 2026 20:12:57 +0000 Subject: [PATCH 05/15] Fill out some of the remaining TODOs --- .../src/sygnal_can_interface_node.cpp | 262 ++++++++++++++++-- 1 file changed, 246 insertions(+), 16 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 4442090..b1dda27 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 @@ -24,6 +24,9 @@ #include +#include "sygnal_can_interface_lib/sygnal_command_interface.hpp" +#include "sygnal_can_interface_lib/sygnal_mcm_interface.hpp" + namespace polymath::sygnal { @@ -156,62 +159,289 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal void SygnalCanInterfaceNode::timerCallback() { - /// TODO: (Zeerek) Get MCM state from sygnal_interface_ and publish - /// TODO: (Zeerek) Create and publish diagnostics message - RCLCPP_DEBUG(get_logger(), "Timer callback - publishing MCM heartbeat and diagnostics"); + // 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 = sygnal_interface_->get_interface_states(); + + // Create and populate MCM heartbeat message + sygnal_can_msgs::msg::McmHeartbeat heartbeat_msg; + heartbeat_msg.header.stamp = now(); + heartbeat_msg.mcm0_state = static_cast(mcm0_state); + heartbeat_msg.mcm1_state = static_cast(mcm1_state); + + // Convert interface states array to vector + heartbeat_msg.interface_states.clear(); + for (const auto & state : interface_states) { + heartbeat_msg.interface_states.push_back(static_cast(state)); + } + + // Store current state and publish + current_mcm_state_ = heartbeat_msg; + mcm_heartbeat_pub_->publish(heartbeat_msg); + + // Create and publish diagnostics + auto diagnostics = createDiagnosticsMessage(); + diagnostics_pub_->publish(diagnostics); + + RCLCPP_DEBUG(get_logger(), "Published MCM heartbeat and diagnostics"); } void SygnalCanInterfaceNode::setControlStateCallback( const std::shared_ptr request, std::shared_ptr response) { - /// TODO: (Zeerek) Implement control state command RCLCPP_INFO( get_logger(), "Set control state service called - bus_id: %d, interface_id: %d, state: %d", request->bus_id, request->interface_id, request->control_state); - response->success = false; - response->message = "Not implemented yet"; + + // Convert uint8 to SygnalControlState enum + SygnalControlState control_state = + request->control_state == 1 ? SygnalControlState::MCM_CONTROL : SygnalControlState::HUMAN_CONTROL; + + // Send control state command + std::string error_message; + auto result = sygnal_interface_->sendControlStateCommand( + request->bus_id, request->interface_id, control_state, request->expect_reply, error_message); + + if (!result.success) { + response->success = false; + response->message = "Failed to send control state command: " + error_message; + RCLCPP_WARN(get_logger(), "%s", response->message.c_str()); + return; + } + + // If we expect a reply, wait for response with timeout + if (request->expect_reply && result.response_future.has_value()) { + auto future = std::move(result.response_future.value()); + auto status = future.wait_for(timeout_ms_); + + if (status == std::future_status::ready) { + auto command_response = future.get(); + RCLCPP_INFO( + get_logger(), + "Control state command acknowledged - bus_id: %d, interface_id: %d", + command_response.bus_id, + command_response.interface_id); + response->success = true; + response->message = "Control state command successful"; + } else { + response->success = false; + response->message = "Timeout waiting for control state command response"; + RCLCPP_WARN(get_logger(), "%s", response->message.c_str()); + } + } else { + // Fire-and-forget, command sent successfully + response->success = true; + response->message = "Control state command sent (no reply expected)"; + } } void SygnalCanInterfaceNode::sendControlCommandCallback( const std::shared_ptr request, std::shared_ptr response) { - /// TODO: (Zeerek) Implement control command RCLCPP_INFO( get_logger(), "Send control command service called - bus_id: %d, interface_id: %d, value: %f", request->bus_id, request->interface_id, request->value); - response->success = false; - response->message = "Not implemented yet"; + + // Send control command + std::string error_message; + auto result = sygnal_interface_->sendControlCommand( + request->bus_id, request->interface_id, request->value, request->expect_reply, error_message); + + if (!result.success) { + response->success = false; + response->message = "Failed to send control command: " + error_message; + RCLCPP_WARN(get_logger(), "%s", response->message.c_str()); + return; + } + + // If we expect a reply, wait for response with timeout + if (request->expect_reply && result.response_future.has_value()) { + auto future = std::move(result.response_future.value()); + auto status = future.wait_for(timeout_ms_); + + if (status == std::future_status::ready) { + auto command_response = future.get(); + RCLCPP_INFO( + get_logger(), + "Control command acknowledged - bus_id: %d, interface_id: %d, value: %f", + command_response.bus_id, + command_response.interface_id, + command_response.value); + response->success = true; + response->message = "Control command successful"; + } else { + response->success = false; + response->message = "Timeout waiting for control command response"; + RCLCPP_WARN(get_logger(), "%s", response->message.c_str()); + } + } else { + // Fire-and-forget, command sent successfully + response->success = true; + response->message = "Control command sent (no reply expected)"; + } } void SygnalCanInterfaceNode::sendRelayCommandCallback( const std::shared_ptr request, std::shared_ptr response) { - /// TODO: (Zeerek) Implement relay command RCLCPP_INFO( get_logger(), "Send relay command service called - bus_id: %d, subsystem_id: %d, state: %d", request->bus_id, request->subsystem_id, request->relay_state); - response->success = false; - response->message = "Not implemented yet"; + + // Convert uint8 to bool (1 = enable, 0 = disable) + bool relay_state = (request->relay_state == 1); + + // Send relay command + std::string error_message; + auto result = sygnal_interface_->sendRelayCommand( + request->bus_id, request->subsystem_id, relay_state, request->expect_reply, error_message); + + if (!result.success) { + response->success = false; + response->message = "Failed to send relay command: " + error_message; + RCLCPP_WARN(get_logger(), "%s", response->message.c_str()); + return; + } + + // If we expect a reply, wait for response with timeout + if (request->expect_reply && result.response_future.has_value()) { + auto future = std::move(result.response_future.value()); + auto status = future.wait_for(timeout_ms_); + + if (status == std::future_status::ready) { + auto command_response = future.get(); + RCLCPP_INFO( + get_logger(), + "Relay command acknowledged - bus_id: %d, subsystem_id: %d", + command_response.bus_id, + static_cast(command_response.value)); + response->success = true; + response->message = "Relay command successful"; + } else { + response->success = false; + response->message = "Timeout waiting for relay command response"; + RCLCPP_WARN(get_logger(), "%s", response->message.c_str()); + } + } else { + // Fire-and-forget, command sent successfully + response->success = true; + response->message = "Relay command sent (no reply expected)"; + } } diagnostic_msgs::msg::DiagnosticArray SygnalCanInterfaceNode::createDiagnosticsMessage() { - /// TODO: (Zeerek) Implement diagnostics message creation - diagnostic_msgs::msg::DiagnosticArray diagnostics; - diagnostics.header.stamp = now(); - return diagnostics; + diagnostic_msgs::msg::DiagnosticArray array_msg; + + // Set header + 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 = sygnal_interface_->get_interface_states(); + + // 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; + mcm0_diag.message = "MCM0 operating normally: " + sygnalSystemStateToString(mcm0_state); + } else if ( + mcm0_state == SygnalSystemState::FAIL_OPERATIONAL_1 || mcm0_state == SygnalSystemState::FAIL_OPERATIONAL_2) + { + mcm0_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + mcm0_diag.message = "MCM0 in degraded mode: " + sygnalSystemStateToString(mcm0_state); + } else { + mcm0_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mcm0_diag.message = "MCM0 fault: " + sygnalSystemStateToString(mcm0_state); + } + + diagnostic_msgs::msg::KeyValue kv; + kv.key = "state"; + kv.value = sygnalSystemStateToString(mcm0_state); + mcm0_diag.values.push_back(kv); + + array_msg.status.push_back(mcm0_diag); + } + + // 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; + mcm1_diag.message = "MCM1 operating normally: " + sygnalSystemStateToString(mcm1_state); + } else if ( + mcm1_state == SygnalSystemState::FAIL_OPERATIONAL_1 || mcm1_state == SygnalSystemState::FAIL_OPERATIONAL_2) + { + mcm1_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + mcm1_diag.message = "MCM1 in degraded mode: " + sygnalSystemStateToString(mcm1_state); + } else { + mcm1_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + mcm1_diag.message = "MCM1 fault: " + sygnalSystemStateToString(mcm1_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) + for (size_t i = 0; i < interface_states.size(); ++i) { + diagnostic_msgs::msg::DiagnosticStatus interface_diag; + interface_diag.name = "sygnal_interface_" + std::to_string(i); + interface_diag.hardware_id = "sygnal_can_interface"; + + auto state = interface_states[i]; + + // Determine diagnostic level based on state + if (state == SygnalSystemState::MCM_CONTROL || state == SygnalSystemState::HUMAN_CONTROL) { + interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + interface_diag.message = + "Interface " + std::to_string(i) + " operating normally: " + sygnalSystemStateToString(state); + } else if (state == SygnalSystemState::FAIL_OPERATIONAL_1 || state == SygnalSystemState::FAIL_OPERATIONAL_2) { + interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + interface_diag.message = + "Interface " + std::to_string(i) + " in degraded mode: " + sygnalSystemStateToString(state); + } else { + interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + interface_diag.message = "Interface " + std::to_string(i) + " fault: " + sygnalSystemStateToString(state); + } + + diagnostic_msgs::msg::KeyValue kv; + kv.key = "state"; + kv.value = sygnalSystemStateToString(state); + interface_diag.values.push_back(kv); + + array_msg.status.push_back(interface_diag); + } + + return array_msg; } } // namespace polymath::sygnal From 5942dc33ca1d2d6e5084c620b3ef47ad85f50d7c Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 22 Jan 2026 20:50:18 +0000 Subject: [PATCH 06/15] Add sygnal interface states for mcm 2 --- .../sygnal_interface_socketcan.hpp | 10 +- .../sygnal_mcm_interface.hpp | 15 +-- .../src/sygnal_interface_socketcan.cpp | 21 ++- .../src/sygnal_mcm_interface.cpp | 17 +-- .../test/sygnal_mcm_interface_test.cpp | 60 +++++++-- .../sygnal_can_interface_node.hpp | 6 +- .../src/sygnal_can_interface_node.cpp | 124 ++++++++++++------ .../sygnal_can_msgs/msg/McmHeartbeat.msg | 13 +- 8 files changed, 178 insertions(+), 88 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 64c17cf..0034a99 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 @@ -49,8 +49,11 @@ class SygnalInterfaceSocketcan /// @param frame CAN frame to parse void parse(const socketcan::CanFrame & frame); - /// @brief Get interface states array - std::array get_interface_states() const; + /// @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; /// @brief Get MCM 0 system state SygnalSystemState get_sygnal_mcm0_state() const; @@ -94,7 +97,8 @@ class SygnalInterfaceSocketcan private: std::shared_ptr socketcan_adapter_; - SygnalMcmInterface mcm_interface_; + SygnalMcmInterface mcm_interface_0_; + SygnalMcmInterface mcm_interface_1_; 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 828f712..0b67e75 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 @@ -61,7 +61,7 @@ inline std::string sygnalSystemStateToString(SygnalSystemState state) class SygnalMcmInterface { public: - SygnalMcmInterface(); + explicit SygnalMcmInterface(uint8_t subsystem_id); ~SygnalMcmInterface() = default; std::array get_interface_states() const @@ -69,21 +69,16 @@ class SygnalMcmInterface return sygnal_interface_states_; } - SygnalSystemState get_sygnal_mcm0_state() const + SygnalSystemState get_mcm_state() const { - return sygnal_mcm_0_state_; - } - - SygnalSystemState get_sygnal_mcm1_state() const - { - return sygnal_mcm_1_state_; + return sygnal_mcm_state_; } bool parseMcmHeartbeatFrame(const socketcan::CanFrame & frame); private: - SygnalSystemState sygnal_mcm_0_state_; - SygnalSystemState sygnal_mcm_1_state_; + uint8_t subsystem_id_; + SygnalSystemState sygnal_mcm_state_; std::array sygnal_interface_states_; }; 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 edc4563..ab7c542 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 @@ -23,14 +23,16 @@ namespace polymath::sygnal SygnalInterfaceSocketcan::SygnalInterfaceSocketcan(std::shared_ptr socketcan_adapter) : socketcan_adapter_(std::move(socketcan_adapter)) -, mcm_interface_() +, mcm_interface_0_(0) +, mcm_interface_1_(1) , control_interface_() {} void SygnalInterfaceSocketcan::parse(const socketcan::CanFrame & frame) { - // Try parsing as MCM heartbeat - mcm_interface_.parseMcmHeartbeatFrame(frame); + // Try parsing as MCM heartbeat (both interfaces will check subsystem_id) + mcm_interface_0_.parseMcmHeartbeatFrame(frame); + mcm_interface_1_.parseMcmHeartbeatFrame(frame); // Try parsing as command response auto response = control_interface_.parseCommandResponseFrame(frame); @@ -69,19 +71,24 @@ void SygnalInterfaceSocketcan::parse(const socketcan::CanFrame & frame) } } -std::array SygnalInterfaceSocketcan::get_interface_states() const +std::array SygnalInterfaceSocketcan::get_interface_states_0() const { - return mcm_interface_.get_interface_states(); + return mcm_interface_0_.get_interface_states(); +} + +std::array SygnalInterfaceSocketcan::get_interface_states_1() const +{ + return mcm_interface_1_.get_interface_states(); } SygnalSystemState SygnalInterfaceSocketcan::get_sygnal_mcm0_state() const { - return mcm_interface_.get_sygnal_mcm0_state(); + return mcm_interface_0_.get_mcm_state(); } SygnalSystemState SygnalInterfaceSocketcan::get_sygnal_mcm1_state() const { - return mcm_interface_.get_sygnal_mcm1_state(); + return mcm_interface_1_.get_mcm_state(); } SendCommandResult SygnalInterfaceSocketcan::sendControlStateCommand( 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 24e40f2..828b2bb 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 @@ -20,9 +20,9 @@ namespace polymath::sygnal { -SygnalMcmInterface::SygnalMcmInterface() -: sygnal_mcm_0_state_(SygnalSystemState::FAIL_HARD) -, sygnal_mcm_1_state_(SygnalSystemState::FAIL_HARD) +SygnalMcmInterface::SygnalMcmInterface(uint8_t subsystem_id) +: subsystem_id_(subsystem_id) +, sygnal_mcm_state_(SygnalSystemState::FAIL_HARD) { sygnal_interface_states_.fill(SygnalSystemState::FAIL_HARD); } @@ -48,13 +48,14 @@ bool SygnalMcmInterface::parseMcmHeartbeatFrame(const socketcan::CanFrame & fram return false; } - // Set the sygnal states based on subsystem_id - if (unpacked_heartbeat_t.subsystem_id == 0) { - sygnal_mcm_0_state_ = SygnalSystemState(unpacked_heartbeat_t.system_state); - } else if (unpacked_heartbeat_t.subsystem_id == 1) { - sygnal_mcm_1_state_ = SygnalSystemState(unpacked_heartbeat_t.system_state); + // Only parse if this frame is for our subsystem + if (unpacked_heartbeat_t.subsystem_id != subsystem_id_) { + return false; } + // Set the MCM state + sygnal_mcm_state_ = SygnalSystemState(unpacked_heartbeat_t.system_state); + // Get interface states sygnal_interface_states_[0] = SygnalSystemState(unpacked_heartbeat_t.interface0_state); sygnal_interface_states_[1] = SygnalSystemState(unpacked_heartbeat_t.interface1_state); 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 095c1e3..427ecc8 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 @@ -38,12 +38,23 @@ static polymath::socketcan::CanFrame createTestFrame(uint32_t can_id, const std: return frame; } -TEST_CASE("SygnalMcmInterface constructor initializes to FAIL_HARD", "[sygnal_mcm_interface]") +TEST_CASE("SygnalMcmInterface constructor initializes to FAIL_HARD for subsystem 0", "[sygnal_mcm_interface]") { - polymath::sygnal::SygnalMcmInterface interface; + polymath::sygnal::SygnalMcmInterface interface(0); - REQUIRE(interface.get_sygnal_mcm0_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); - REQUIRE(interface.get_sygnal_mcm1_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); + REQUIRE(interface.get_mcm_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); + + auto states = interface.get_interface_states(); + for (const auto & state : states) { + REQUIRE(state == polymath::sygnal::SygnalSystemState::FAIL_HARD); + } +} + +TEST_CASE("SygnalMcmInterface constructor initializes to FAIL_HARD for subsystem 1", "[sygnal_mcm_interface]") +{ + polymath::sygnal::SygnalMcmInterface interface(1); + + REQUIRE(interface.get_mcm_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); auto states = interface.get_interface_states(); for (const auto & state : states) { @@ -53,14 +64,14 @@ TEST_CASE("SygnalMcmInterface constructor initializes to FAIL_HARD", "[sygnal_mc TEST_CASE("SygnalMcmInterface parses MCM_CONTROL heartbeat", "[sygnal_mcm_interface]") { - polymath::sygnal::SygnalMcmInterface interface; + polymath::sygnal::SygnalMcmInterface interface(0); // Generated from DBC: MCM_CONTROL state, subsystem 0, all interfaces MCM_CONTROL std::vector data = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0x98}; auto frame = createTestFrame(HEARTBEAT_FRAME_ID, data); REQUIRE(interface.parseMcmHeartbeatFrame(frame) == true); - REQUIRE(interface.get_sygnal_mcm0_state() == polymath::sygnal::SygnalSystemState::MCM_CONTROL); + REQUIRE(interface.get_mcm_state() == polymath::sygnal::SygnalSystemState::MCM_CONTROL); auto states = interface.get_interface_states(); for (size_t i = 0; i < 5; ++i) { @@ -70,14 +81,14 @@ TEST_CASE("SygnalMcmInterface parses MCM_CONTROL heartbeat", "[sygnal_mcm_interf TEST_CASE("SygnalMcmInterface parses HUMAN_CONTROL heartbeat", "[sygnal_mcm_interface]") { - polymath::sygnal::SygnalMcmInterface interface; + polymath::sygnal::SygnalMcmInterface interface(1); // Generated from DBC: HUMAN_CONTROL state, subsystem 1 std::vector data = {0x81, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x86}; auto frame = createTestFrame(HEARTBEAT_FRAME_ID, data); REQUIRE(interface.parseMcmHeartbeatFrame(frame) == true); - REQUIRE(interface.get_sygnal_mcm1_state() == polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); + REQUIRE(interface.get_mcm_state() == polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); auto states = interface.get_interface_states(); for (size_t i = 0; i < 5; ++i) { @@ -87,26 +98,51 @@ TEST_CASE("SygnalMcmInterface parses HUMAN_CONTROL heartbeat", "[sygnal_mcm_inte TEST_CASE("SygnalMcmInterface rejects wrong frame ID", "[sygnal_mcm_interface]") { - polymath::sygnal::SygnalMcmInterface interface; + polymath::sygnal::SygnalMcmInterface interface(0); std::vector data = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0x98}; auto frame = createTestFrame(0x999, data); // Wrong ID REQUIRE(interface.parseMcmHeartbeatFrame(frame) == false); // State should remain FAIL_HARD - REQUIRE(interface.get_sygnal_mcm0_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); + REQUIRE(interface.get_mcm_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); } TEST_CASE("SygnalMcmInterface rejects bad CRC", "[sygnal_mcm_interface]") { - polymath::sygnal::SygnalMcmInterface interface; + polymath::sygnal::SygnalMcmInterface interface(0); // Valid data but corrupted CRC (last byte) std::vector data = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0xFF}; auto frame = createTestFrame(HEARTBEAT_FRAME_ID, data); REQUIRE(interface.parseMcmHeartbeatFrame(frame) == false); - REQUIRE(interface.get_sygnal_mcm0_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); + REQUIRE(interface.get_mcm_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); +} + +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); + + // Heartbeat frame for subsystem 1 + std::vector data_sub1 = {0x81, 0x00, 0x00, 0x00, 0x00, 0x32, 0x00, 0x86}; + auto frame_sub1 = createTestFrame(HEARTBEAT_FRAME_ID, data_sub1); + + // Should return false and state should remain FAIL_HARD + REQUIRE(interface_0.parseMcmHeartbeatFrame(frame_sub1) == false); + 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); + + // Heartbeat frame for subsystem 0 + std::vector data_sub0 = {0x01, 0x00, 0x01, 0x9F, 0x00, 0x64, 0x00, 0x98}; + auto frame_sub0 = createTestFrame(HEARTBEAT_FRAME_ID, data_sub0); + + // Should return false and state should remain FAIL_HARD + REQUIRE(interface_1.parseMcmHeartbeatFrame(frame_sub0) == false); + REQUIRE(interface_1.get_mcm_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); } TEST_CASE("sygnalSystemStateToString returns correct strings", "[sygnal_mcm_interface]") 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 dafd384..3540cbd 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 @@ -107,10 +107,12 @@ class SygnalCanInterfaceNode : public rclcpp_lifecycle::LifecycleNode // Publishers rclcpp_lifecycle::LifecyclePublisher::SharedPtr diagnostics_pub_; - rclcpp_lifecycle::LifecyclePublisher::SharedPtr mcm_heartbeat_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr mcm0_heartbeat_pub_; + rclcpp_lifecycle::LifecyclePublisher::SharedPtr mcm1_heartbeat_pub_; // Current MCM state storage - std::optional current_mcm_state_; + 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 b1dda27..82262ec 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 @@ -82,7 +82,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal // Create publishers diagnostics_pub_ = create_publisher("/diagnostics", rclcpp::QoS(10)); - mcm_heartbeat_pub_ = create_publisher("~/mcm_heartbeat", rclcpp::QoS(10)); + mcm0_heartbeat_pub_ = create_publisher("~/mcm0_heartbeat", rclcpp::QoS(10)); + mcm1_heartbeat_pub_ = create_publisher("~/mcm1_heartbeat", rclcpp::QoS(10)); // Create services set_control_state_service_ = create_service( @@ -117,7 +118,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal { // Activate publishers diagnostics_pub_->on_activate(); - mcm_heartbeat_pub_->on_activate(); + mcm0_heartbeat_pub_->on_activate(); + mcm1_heartbeat_pub_->on_activate(); RCLCPP_INFO(get_logger(), "Sygnal CAN Interface node activated"); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; @@ -128,7 +130,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal { // Deactivate publishers diagnostics_pub_->on_deactivate(); - mcm_heartbeat_pub_->on_deactivate(); + mcm0_heartbeat_pub_->on_deactivate(); + mcm1_heartbeat_pub_->on_deactivate(); RCLCPP_INFO(get_logger(), "Sygnal CAN Interface node deactivated"); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; @@ -143,7 +146,8 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal send_control_command_service_.reset(); send_relay_command_service_.reset(); diagnostics_pub_.reset(); - mcm_heartbeat_pub_.reset(); + mcm0_heartbeat_pub_.reset(); + mcm1_heartbeat_pub_.reset(); if (socketcan_adapter_) { socketcan_adapter_->joinReceptionThread(); @@ -162,29 +166,44 @@ 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 = sygnal_interface_->get_interface_states(); - - // Create and populate MCM heartbeat message - sygnal_can_msgs::msg::McmHeartbeat heartbeat_msg; - heartbeat_msg.header.stamp = now(); - heartbeat_msg.mcm0_state = static_cast(mcm0_state); - heartbeat_msg.mcm1_state = static_cast(mcm1_state); - - // Convert interface states array to vector - heartbeat_msg.interface_states.clear(); - for (const auto & state : interface_states) { - heartbeat_msg.interface_states.push_back(static_cast(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)); } - // Store current state and publish - current_mcm_state_ = heartbeat_msg; - mcm_heartbeat_pub_->publish(heartbeat_msg); + // Store current state and publish MCM 0 + current_mcm0_state_ = mcm0_msg; + mcm0_heartbeat_pub_->publish(mcm0_msg); + + // 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); + + // 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)); + } + + // Store current state and publish MCM 1 + current_mcm1_state_ = mcm1_msg; + mcm1_heartbeat_pub_->publish(mcm1_msg); // Create and publish diagnostics auto diagnostics = createDiagnosticsMessage(); diagnostics_pub_->publish(diagnostics); - RCLCPP_DEBUG(get_logger(), "Published MCM heartbeat and diagnostics"); + RCLCPP_DEBUG(get_logger(), "Published MCM 0 and MCM 1 heartbeats and diagnostics"); } void SygnalCanInterfaceNode::setControlStateCallback( @@ -353,7 +372,8 @@ diagnostic_msgs::msg::DiagnosticArray SygnalCanInterfaceNode::createDiagnosticsM // 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 = sygnal_interface_->get_interface_states(); + auto interface_states_0 = sygnal_interface_->get_interface_states_0(); + auto interface_states_1 = sygnal_interface_->get_interface_states_1(); // Create MCM 0 diagnostics { @@ -364,17 +384,16 @@ diagnostic_msgs::msg::DiagnosticArray SygnalCanInterfaceNode::createDiagnosticsM // 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; - mcm0_diag.message = "MCM0 operating normally: " + sygnalSystemStateToString(mcm0_state); } else if ( mcm0_state == SygnalSystemState::FAIL_OPERATIONAL_1 || mcm0_state == SygnalSystemState::FAIL_OPERATIONAL_2) { mcm0_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - mcm0_diag.message = "MCM0 in degraded mode: " + sygnalSystemStateToString(mcm0_state); } else { mcm0_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mcm0_diag.message = "MCM0 fault: " + sygnalSystemStateToString(mcm0_state); } + mcm0_diag.message = "MCM0 state: " + sygnalSystemStateToString(mcm0_state); + diagnostic_msgs::msg::KeyValue kv; kv.key = "state"; kv.value = sygnalSystemStateToString(mcm0_state); @@ -392,17 +411,16 @@ diagnostic_msgs::msg::DiagnosticArray SygnalCanInterfaceNode::createDiagnosticsM // 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; - mcm1_diag.message = "MCM1 operating normally: " + sygnalSystemStateToString(mcm1_state); } else if ( mcm1_state == SygnalSystemState::FAIL_OPERATIONAL_1 || mcm1_state == SygnalSystemState::FAIL_OPERATIONAL_2) { mcm1_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - mcm1_diag.message = "MCM1 in degraded mode: " + sygnalSystemStateToString(mcm1_state); } else { mcm1_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - mcm1_diag.message = "MCM1 fault: " + sygnalSystemStateToString(mcm1_state); } + mcm1_diag.message = "MCM1 state: " + sygnalSystemStateToString(mcm1_state); + diagnostic_msgs::msg::KeyValue kv; kv.key = "state"; kv.value = sygnalSystemStateToString(mcm1_state); @@ -411,32 +429,54 @@ diagnostic_msgs::msg::DiagnosticArray SygnalCanInterfaceNode::createDiagnosticsM array_msg.status.push_back(mcm1_diag); } - // Create diagnostics for each interface (0-4) - for (size_t i = 0; i < interface_states.size(); ++i) { + // 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"; - auto state = interface_states[i]; + // 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; + } - // Determine diagnostic level based on state - if (state == SygnalSystemState::MCM_CONTROL || state == SygnalSystemState::HUMAN_CONTROL) { + if (worst_state == SygnalSystemState::MCM_CONTROL || worst_state == SygnalSystemState::HUMAN_CONTROL) { interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; - interface_diag.message = - "Interface " + std::to_string(i) + " operating normally: " + sygnalSystemStateToString(state); - } else if (state == SygnalSystemState::FAIL_OPERATIONAL_1 || state == SygnalSystemState::FAIL_OPERATIONAL_2) { + } else if ( + worst_state == SygnalSystemState::FAIL_OPERATIONAL_1 || worst_state == SygnalSystemState::FAIL_OPERATIONAL_2) + { interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; - interface_diag.message = - "Interface " + std::to_string(i) + " in degraded mode: " + sygnalSystemStateToString(state); } else { interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; - interface_diag.message = "Interface " + std::to_string(i) + " fault: " + sygnalSystemStateToString(state); } - diagnostic_msgs::msg::KeyValue kv; - kv.key = "state"; - kv.value = sygnalSystemStateToString(state); - interface_diag.values.push_back(kv); + // 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); } diff --git a/sygnal_can_interface/sygnal_can_msgs/msg/McmHeartbeat.msg b/sygnal_can_interface/sygnal_can_msgs/msg/McmHeartbeat.msg index 13e5874..fb35345 100644 --- a/sygnal_can_interface/sygnal_can_msgs/msg/McmHeartbeat.msg +++ b/sygnal_can_interface/sygnal_can_msgs/msg/McmHeartbeat.msg @@ -2,11 +2,16 @@ # Header for timestamp std_msgs/Header header -# System state for MCM subsystem 0 (0-255) -uint8 mcm0_state +# System state constants +uint8 FAIL_HARD = 254 +uint8 HUMAN_OVERRIDE = 253 +uint8 FAIL_OPERATIONAL_2 = 242 +uint8 FAIL_OPERATIONAL_1 = 241 +uint8 MCM_CONTROL = 1 +uint8 HUMAN_CONTROL = 0 -# System state for MCM subsystem 1 (0-255) -uint8 mcm1_state +# System state for this MCM subsystem (0-255) +uint8 state # Interface states (5 interfaces, values 0-255 each) uint8[] interface_states From 31ce69e32885481b4d1a578d3cb669011ccb9d11 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 22 Jan 2026 21:32:05 +0000 Subject: [PATCH 07/15] Add in callback for control commands --- .../sygnal_can_interface_node.hpp | 6 +++ .../src/sygnal_can_interface_node.cpp | 43 +++++++++++++++++-- .../sygnal_can_msgs/CMakeLists.txt | 1 + .../sygnal_can_msgs/msg/ControlCommand.msg | 10 +++++ .../srv/SendControlCommand.srv | 9 +--- 5 files changed, 57 insertions(+), 12 deletions(-) create mode 100644 sygnal_can_interface/sygnal_can_msgs/msg/ControlCommand.msg 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 3540cbd..1dce894 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 @@ -86,6 +86,10 @@ class SygnalCanInterfaceNode : public rclcpp_lifecycle::LifecycleNode const std::shared_ptr request, std::shared_ptr response); + /// @brief Subscription callback to handle incoming control commands + /// @param msg Incoming control command message + void controlCommandCallback(const sygnal_can_msgs::msg::ControlCommand::UniquePtr msg); + /// @brief Create diagnostics array from Sygnal status /// @return Diagnostic array message diagnostic_msgs::msg::DiagnosticArray createDiagnosticsMessage(); @@ -110,6 +114,8 @@ class SygnalCanInterfaceNode : public rclcpp_lifecycle::LifecycleNode rclcpp_lifecycle::LifecyclePublisher::SharedPtr mcm0_heartbeat_pub_; rclcpp_lifecycle::LifecyclePublisher::SharedPtr mcm1_heartbeat_pub_; + rclcpp::Subscription::SharedPtr control_command_sub_; + // Current MCM state storage std::optional current_mcm0_state_; std::optional current_mcm1_state_; 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 82262ec..8362f20 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 @@ -121,6 +121,11 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal mcm0_heartbeat_pub_->on_activate(); mcm1_heartbeat_pub_->on_activate(); + create_subscription( + "~/control_command", + rclcpp::QoS(10), + std::bind(&SygnalCanInterfaceNode::controlCommandCallback, this, std::placeholders::_1)); + RCLCPP_INFO(get_logger(), "Sygnal CAN Interface node activated"); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::SUCCESS; } @@ -266,14 +271,18 @@ void SygnalCanInterfaceNode::sendControlCommandCallback( RCLCPP_INFO( get_logger(), "Send control command service called - bus_id: %d, interface_id: %d, value: %f", - request->bus_id, - request->interface_id, - request->value); + request->command.bus_id, + request->command.interface_id, + request->command.value); // Send control command std::string error_message; auto result = sygnal_interface_->sendControlCommand( - request->bus_id, request->interface_id, request->value, request->expect_reply, error_message); + request->command.bus_id, + request->command.interface_id, + request->command.value, + request->expect_reply, + error_message); if (!result.success) { response->success = false; @@ -309,6 +318,32 @@ void SygnalCanInterfaceNode::sendControlCommandCallback( } } +void SygnalCanInterfaceNode::controlCommandCallback(const sygnal_can_msgs::msg::ControlCommand::UniquePtr msg) +{ + RCLCPP_INFO_THROTTLE( + get_logger(), + *get_clock(), + 500, + "Received control command - bus_id: %d, interface_id: %d, value: %f", + msg->bus_id, + msg->interface_id, + msg->value); + + // Send control command without expecting a reply + std::string error_message; + auto result = sygnal_interface_->sendControlCommand(msg->bus_id, msg->interface_id, msg->value, false, error_message); + + if (!result.success) { + RCLCPP_WARN(get_logger(), "Failed to send control command from subscription: %s", error_message.c_str()); + } else { + RCLCPP_DEBUG( + get_logger(), + "Control command sent successfully from subscription - bus_id: %d, interface_id: %d", + msg->bus_id, + msg->interface_id); + } +} + void SygnalCanInterfaceNode::sendRelayCommandCallback( const std::shared_ptr request, std::shared_ptr response) diff --git a/sygnal_can_interface/sygnal_can_msgs/CMakeLists.txt b/sygnal_can_interface/sygnal_can_msgs/CMakeLists.txt index c6ffeaa..bd7e46a 100644 --- a/sygnal_can_interface/sygnal_can_msgs/CMakeLists.txt +++ b/sygnal_can_interface/sygnal_can_msgs/CMakeLists.txt @@ -22,6 +22,7 @@ find_package(std_msgs REQUIRED) set(msg_files "msg/McmHeartbeat.msg" + "msg/ControlCommand.msg" ) set(srv_files diff --git a/sygnal_can_interface/sygnal_can_msgs/msg/ControlCommand.msg b/sygnal_can_interface/sygnal_can_msgs/msg/ControlCommand.msg new file mode 100644 index 0000000..2693e18 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_msgs/msg/ControlCommand.msg @@ -0,0 +1,10 @@ +# Command to forward to Sygnal's Systems + +# Bus ID (0-127) +uint8 bus_id + +# Interface ID (0-4) +uint8 interface_id + +# Control value (interpretation depends on interface) +float64 value diff --git a/sygnal_can_interface/sygnal_can_msgs/srv/SendControlCommand.srv b/sygnal_can_interface/sygnal_can_msgs/srv/SendControlCommand.srv index 861f7e1..43c858d 100644 --- a/sygnal_can_interface/sygnal_can_msgs/srv/SendControlCommand.srv +++ b/sygnal_can_interface/sygnal_can_msgs/srv/SendControlCommand.srv @@ -1,14 +1,7 @@ # Service to send a control command with a value # Request -# Bus ID (0-127) -uint8 bus_id - -# Interface ID (0-4) -uint8 interface_id - -# Control value (interpretation depends on interface) -float64 value +sygnal_can_msgs/ControlCommand command # Whether to expect a reply bool expect_reply From 4ca8510a6a581bc55b1af58b6a1da4b367941248 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 22 Jan 2026 21:35:11 +0000 Subject: [PATCH 08/15] Fill out readme slightly --- sygnal_can_interface/sygnal_can_interface_lib/README.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/sygnal_can_interface/sygnal_can_interface_lib/README.md b/sygnal_can_interface/sygnal_can_interface_lib/README.md index 3774f9d..ad8743b 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/README.md +++ b/sygnal_can_interface/sygnal_can_interface_lib/README.md @@ -1,2 +1,3 @@ +# Sygnal Can Interface Lib -TO BE FILLED OUT +C++ Libarary for interfacing with Sygnal's CAN Interfaces. Currently only supports MCM command and feedback. From 368f14688c9c8b8f1f83c95daae118270f8188e7 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 22 Jan 2026 21:48:58 +0000 Subject: [PATCH 09/15] Fix builds and tests and update for new sygnal subsystem id in mcm --- .../sygnal_command_interface.hpp | 8 +++++++- .../sygnal_interface_socketcan.hpp | 8 +++++++- .../src/sygnal_command_interface.cpp | 12 ++++++++++-- .../src/sygnal_interface_socketcan.cpp | 13 ++++++++++--- .../test/sygnal_command_interface_test.cpp | 4 ++-- .../sygnal_can_interface_ros2/CMakeLists.txt | 1 - .../src/sygnal_can_interface_node.cpp | 15 ++++++++++----- .../sygnal_can_msgs/msg/ControlCommand.msg | 3 +++ .../sygnal_can_msgs/srv/SetControlState.srv | 6 ++++++ 9 files changed, 55 insertions(+), 15 deletions(-) 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 0d1175a..ceef5d2 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 @@ -50,6 +50,7 @@ struct SygnalControlCommandResponse SygnalControlCommandResponseType response_type; uint8_t interface_id; uint8_t bus_id; + uint8_t subsystem_id; // Value can represent different things based on response type double value; }; @@ -68,6 +69,7 @@ class SygnalControlInterface std::optional createControlStateCommandFrame( const uint8_t bus_id, const uint8_t interface_id, + const uint8_t subsystem_id, const SygnalControlState control_state, std::string & error_message); @@ -77,7 +79,11 @@ class SygnalControlInterface /// @param error_message Error in case of failure /// @return can frame if succesful std::optional createControlCommandFrame( - const uint8_t bus_id, const uint8_t interface_id, const double value, std::string & error_message); + const uint8_t bus_id, + const uint8_t interface_id, + const uint8_t subsystem_id, + const double value, + std::string & error_message); /// @brief Set the relay state for engine control or gears /// @param subsystem_id Sygnal Subsystem Relay to enable/disable 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 0034a99..51a2673 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 @@ -71,6 +71,7 @@ class SygnalInterfaceSocketcan SendCommandResult sendControlStateCommand( uint8_t bus_id, uint8_t interface_id, + uint8_t subsystem_id, SygnalControlState control_state, bool expect_reply, std::string & error_message); @@ -83,7 +84,12 @@ class SygnalInterfaceSocketcan /// @param error_message Populated on failure /// @return Result with success flag and optional response future SendCommandResult sendControlCommand( - uint8_t bus_id, uint8_t interface_id, double value, bool expect_reply, std::string & error_message); + uint8_t bus_id, + uint8_t interface_id, + uint8_t subsystem_id, + double value, + bool expect_reply, + std::string & error_message); /// @brief Send relay command /// @param bus_id Bus address 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 22c51b6..744e17c 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 @@ -27,7 +27,11 @@ SygnalControlInterface::SygnalControlInterface() {} std::optional SygnalControlInterface::createControlStateCommandFrame( - const uint8_t bus_id, const uint8_t interface_id, const SygnalControlState control_state, std::string & error_message) + const uint8_t bus_id, + const uint8_t interface_id, + const uint8_t subsystem_id, + const SygnalControlState control_state, + std::string & error_message) { polymath::socketcan::CanFrame frame; uint8_t control_enable_buffer[CAN_MAX_DLC]; @@ -72,7 +76,11 @@ std::optional SygnalControlInterface::createContr } std::optional SygnalControlInterface::createControlCommandFrame( - const uint8_t bus_id, const uint8_t interface_id, const double value, std::string & error_message) + const uint8_t bus_id, + const uint8_t interface_id, + const uint8_t subsystem_id, + const double value, + std::string & error_message) { polymath::socketcan::CanFrame frame; uint8_t control_command_buffer[CAN_MAX_DLC]; 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 ab7c542..981e601 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 @@ -94,12 +94,13 @@ SygnalSystemState SygnalInterfaceSocketcan::get_sygnal_mcm1_state() const SendCommandResult SygnalInterfaceSocketcan::sendControlStateCommand( uint8_t bus_id, uint8_t interface_id, + uint8_t subsystem_id, SygnalControlState control_state, bool expect_reply, std::string & error_message) { auto frame_opt = - control_interface_.createControlStateCommandFrame(bus_id, interface_id, control_state, error_message); + control_interface_.createControlStateCommandFrame(bus_id, interface_id, subsystem_id, control_state, error_message); if (!frame_opt.has_value()) { return {false, std::nullopt}; @@ -123,9 +124,15 @@ SendCommandResult SygnalInterfaceSocketcan::sendControlStateCommand( } SendCommandResult SygnalInterfaceSocketcan::sendControlCommand( - uint8_t bus_id, uint8_t interface_id, double value, bool expect_reply, std::string & error_message) + uint8_t bus_id, + uint8_t interface_id, + uint8_t subsystem_id, + double value, + bool expect_reply, + std::string & error_message) { - auto frame_opt = control_interface_.createControlCommandFrame(bus_id, interface_id, value, error_message); + auto frame_opt = + control_interface_.createControlCommandFrame(bus_id, interface_id, subsystem_id, value, error_message); if (!frame_opt.has_value()) { return {false, std::nullopt}; diff --git a/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_command_interface_test.cpp b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_command_interface_test.cpp index c9cb969..a6ebbb8 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_command_interface_test.cpp +++ b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_command_interface_test.cpp @@ -49,7 +49,7 @@ TEST_CASE("SygnalControlInterface creates ControlEnable frame", "[sygnal_command std::string error_message; auto frame_opt = - interface.createControlStateCommandFrame(1, 3, polymath::sygnal::SygnalControlState::MCM_CONTROL, error_message); + interface.createControlStateCommandFrame(1, 3, 0, polymath::sygnal::SygnalControlState::MCM_CONTROL, error_message); REQUIRE(frame_opt.has_value()); REQUIRE(error_message.empty()); @@ -69,7 +69,7 @@ TEST_CASE("SygnalControlInterface creates ControlCommand frame", "[sygnal_comman polymath::sygnal::SygnalControlInterface interface; std::string error_message; - auto frame_opt = interface.createControlCommandFrame(1, 0, 0.5, error_message); + auto frame_opt = interface.createControlCommandFrame(1, 0, 0, 0.5, error_message); REQUIRE(frame_opt.has_value()); REQUIRE(error_message.empty()); diff --git a/sygnal_can_interface/sygnal_can_interface_ros2/CMakeLists.txt b/sygnal_can_interface/sygnal_can_interface_ros2/CMakeLists.txt index 5aa2dc8..39ddf0d 100644 --- a/sygnal_can_interface/sygnal_can_interface_ros2/CMakeLists.txt +++ b/sygnal_can_interface/sygnal_can_interface_ros2/CMakeLists.txt @@ -63,7 +63,6 @@ rclcpp_components_register_node(${PROJECT_NAME} # Installs install(DIRECTORY include/ DESTINATION include) -install(DIRECTORY config/ DESTINATION share/${PROJECT_NAME}/config) install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch) install(TARGETS ${PROJECT_NAME} 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 8362f20..312299b 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 @@ -217,9 +217,10 @@ void SygnalCanInterfaceNode::setControlStateCallback( { RCLCPP_INFO( get_logger(), - "Set control state service called - bus_id: %d, interface_id: %d, state: %d", + "Set control state service called - bus_id: %d, interface_id: %d, subsystem_id: %d, state: %d", request->bus_id, request->interface_id, + request->subsystem_id, request->control_state); // Convert uint8 to SygnalControlState enum @@ -229,7 +230,7 @@ void SygnalCanInterfaceNode::setControlStateCallback( // Send control state command std::string error_message; auto result = sygnal_interface_->sendControlStateCommand( - request->bus_id, request->interface_id, control_state, request->expect_reply, error_message); + request->bus_id, request->interface_id, request->subsystem_id, control_state, request->expect_reply, error_message); if (!result.success) { response->success = false; @@ -270,9 +271,10 @@ void SygnalCanInterfaceNode::sendControlCommandCallback( { RCLCPP_INFO( get_logger(), - "Send control command service called - bus_id: %d, interface_id: %d, value: %f", + "Send control command service called - bus_id: %d, interface_id: %d, subsystem_id: %d, value: %f", request->command.bus_id, request->command.interface_id, + request->command.subsystem_id, request->command.value); // Send control command @@ -280,6 +282,7 @@ void SygnalCanInterfaceNode::sendControlCommandCallback( auto result = sygnal_interface_->sendControlCommand( request->command.bus_id, request->command.interface_id, + request->command.subsystem_id, request->command.value, request->expect_reply, error_message); @@ -324,14 +327,16 @@ void SygnalCanInterfaceNode::controlCommandCallback(const sygnal_can_msgs::msg:: get_logger(), *get_clock(), 500, - "Received control command - bus_id: %d, interface_id: %d, value: %f", + "Received control command - bus_id: %d, interface_id: %d, subsystem_id: %d, value: %f", msg->bus_id, msg->interface_id, + msg->subsystem_id, msg->value); // Send control command without expecting a reply std::string error_message; - auto result = sygnal_interface_->sendControlCommand(msg->bus_id, msg->interface_id, msg->value, false, error_message); + auto result = sygnal_interface_->sendControlCommand( + msg->bus_id, msg->interface_id, msg->subsystem_id, msg->value, false, error_message); if (!result.success) { RCLCPP_WARN(get_logger(), "Failed to send control command from subscription: %s", error_message.c_str()); diff --git a/sygnal_can_interface/sygnal_can_msgs/msg/ControlCommand.msg b/sygnal_can_interface/sygnal_can_msgs/msg/ControlCommand.msg index 2693e18..2f90c29 100644 --- a/sygnal_can_interface/sygnal_can_msgs/msg/ControlCommand.msg +++ b/sygnal_can_interface/sygnal_can_msgs/msg/ControlCommand.msg @@ -6,5 +6,8 @@ uint8 bus_id # Interface ID (0-4) uint8 interface_id +# Subsystem ID (0-1) +uint8 subsystem_id + # Control value (interpretation depends on interface) float64 value diff --git a/sygnal_can_interface/sygnal_can_msgs/srv/SetControlState.srv b/sygnal_can_interface/sygnal_can_msgs/srv/SetControlState.srv index 135004f..f38b5ac 100644 --- a/sygnal_can_interface/sygnal_can_msgs/srv/SetControlState.srv +++ b/sygnal_can_interface/sygnal_can_msgs/srv/SetControlState.srv @@ -1,12 +1,18 @@ # Service to set control state (MCM_CONTROL or HUMAN_CONTROL) # Request +uint8 HUMAN_CONTROL=0 +uint8 MCM_CONTROL=1 + # Bus ID (0-127) uint8 bus_id # Interface ID (0-4) uint8 interface_id +# Subsystem ID (0-1) +uint8 subsystem_id + # Control state: 0 = HUMAN_CONTROL, 1 = MCM_CONTROL uint8 control_state From f596ae62cacb0e7d7e724941533643cd8880ef2a Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 22 Jan 2026 22:08:15 +0000 Subject: [PATCH 10/15] Add failures for bad number of subsystems or interfaces --- .../sygnal_command_interface.hpp | 3 +++ .../src/sygnal_command_interface.cpp | 20 +++++++++++++++++++ 2 files changed, 23 insertions(+) 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 ceef5d2..ef5b924 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 @@ -45,6 +45,9 @@ enum class SygnalControlCommandResponseType : uint8_t RELAY = 2 }; +constexpr uint32_t MAX_SYGNAL_INTERFACES = 6; +constexpr uint32_t MAX_SYGNAL_SUBSYSTEMS = 1; + struct SygnalControlCommandResponse { SygnalControlCommandResponseType response_type; 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 744e17c..b84069e 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 @@ -33,6 +33,16 @@ std::optional SygnalControlInterface::createContr const SygnalControlState control_state, std::string & error_message) { + if (interface_id >= MAX_SYGNAL_INTERFACES) { + error_message = error_message + "Interface ID exceeds maximum allowed interfaces. \n"; + return std::nullopt; + } + + if (subsystem_id >= MAX_SYGNAL_SUBSYSTEMS) { + error_message = error_message + "Subsystem ID exceeds maximum allowed subsystems. \n"; + return std::nullopt; + } + polymath::socketcan::CanFrame frame; uint8_t control_enable_buffer[CAN_MAX_DLC]; mcm_control_control_enable_t mcm_unpacked_control_enable_t; @@ -82,6 +92,16 @@ std::optional SygnalControlInterface::createContr const double value, std::string & error_message) { + if (interface_id >= MAX_SYGNAL_INTERFACES) { + error_message = error_message + "Interface ID exceeds maximum allowed interfaces. \n"; + return std::nullopt; + } + + if (subsystem_id >= MAX_SYGNAL_SUBSYSTEMS) { + error_message = error_message + "Subsystem ID exceeds maximum allowed subsystems. \n"; + return std::nullopt; + } + polymath::socketcan::CanFrame frame; uint8_t control_command_buffer[CAN_MAX_DLC]; mcm_control_control_command_t mcm_unpacked_control_command_t; From 0f8bdc90a34a8f91e02b9ec87284fcd4916fe205 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 22 Jan 2026 22:14:59 +0000 Subject: [PATCH 11/15] Make socketcan return true --- .../src/sygnal_interface_socketcan.cpp | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) 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 981e601..328f3ae 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 @@ -28,16 +28,21 @@ SygnalInterfaceSocketcan::SygnalInterfaceSocketcan(std::shared_ptr SygnalInterfaceSocketcan::get_interface_states_0() const From 80300a22f871e0aeecfde9c4e047b4e85ea41e5f Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 22 Jan 2026 22:31:08 +0000 Subject: [PATCH 12/15] Forgot to save and commit hpp file hange --- .../sygnal_can_interface_lib/sygnal_interface_socketcan.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 51a2673..44a9740 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 @@ -47,7 +47,7 @@ class SygnalInterfaceSocketcan /// @brief Parse incoming CAN frame for MCM heartbeat and command responses /// @param frame CAN frame to parse - void parse(const socketcan::CanFrame & frame); + bool parse(const socketcan::CanFrame & frame); /// @brief Get interface states array from MCM 0 std::array get_interface_states_0() const; From 449fd270f4bd0b523964bb37f0a762b31ae29459 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 29 Jan 2026 20:06:48 +0000 Subject: [PATCH 13/15] Adding in socketcan enabled tests that verify transmission and reception as well as future promise behaviors --- .../sygnal_can_interface_lib/CMakeLists.txt | 11 + .../test/sygnal_interface_socketcan_test.cpp | 356 ++++++++++++++++++ 2 files changed, 367 insertions(+) create mode 100644 sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_test.cpp diff --git a/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt b/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt index 1e3ebc9..34c114a 100644 --- a/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt +++ b/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt @@ -92,6 +92,17 @@ if(BUILD_TESTING) PRIVATE ${PROJECT_NAME} Catch2::Catch2WithMain ) catch_discover_tests(sygnal_command_interface_tests) + + if(DEFINED ENV{CAN_AVAILABLE}) + add_executable(sygnal_interface_socketcan_tests + test/sygnal_interface_socketcan_test.cpp + ) + target_link_libraries(sygnal_interface_socketcan_tests + PRIVATE ${PROJECT_NAME} Catch2::Catch2WithMain + ) + catch_discover_tests(sygnal_interface_socketcan_tests) + message(STATUS "CAN_AVAILABLE set - including hardware tests") + endif() endif() ament_package() 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 new file mode 100644 index 0000000..3a3907e --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_test.cpp @@ -0,0 +1,356 @@ +// Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include +#include +#include +#include + +#if __has_include() + #include +#else + #include +#endif + +#include +#include + +#include "socketcan_adapter/socketcan_adapter.hpp" +#include "sygnal_can_interface_lib/crc8.hpp" +#include "sygnal_can_interface_lib/sygnal_interface_socketcan.hpp" +#include "sygnal_dbc/mcm_control.h" +#include "sygnal_dbc/mcm_heartbeat.h" +#include "sygnal_dbc/mcm_relay.h" + +namespace +{ +constexpr const char * VCAN_INTERFACE = "vcan0"; + +polymath::socketcan::CanFrame createEnableResponseFrame(uint8_t bus_id, uint8_t interface_id, uint8_t enable) +{ + uint8_t buffer[CAN_MAX_DLC] = {0}; + mcm_control_control_enable_response_t response; + mcm_control_control_enable_response_init(&response); + response.bus_address = bus_id; + response.interface_id = interface_id; + response.enable = enable; + response.crc = 0; + + mcm_control_control_enable_response_pack(buffer, &response, sizeof(buffer)); + response.crc = polymath::sygnal::generate_crc8(buffer); + mcm_control_control_enable_response_pack(buffer, &response, sizeof(buffer)); + + std::array data; + std::copy(std::begin(buffer), std::end(buffer), data.begin()); + + polymath::socketcan::CanFrame frame; + frame.set_can_id(MCM_CONTROL_CONTROL_ENABLE_RESPONSE_FRAME_ID); + frame.set_len(MCM_CONTROL_CONTROL_ENABLE_RESPONSE_LENGTH); + frame.set_data(data); + return frame; +} + +polymath::socketcan::CanFrame createControlResponseFrame(uint8_t bus_id, uint8_t interface_id, float value) +{ + uint8_t buffer[CAN_MAX_DLC] = {0}; + mcm_control_control_command_response_t response; + mcm_control_control_command_response_init(&response); + response.bus_address = bus_id; + response.interface_id = interface_id; + response.count8 = 0; + response.value = mcm_control_control_command_response_value_encode(value); + response.crc = 0; + + mcm_control_control_command_response_pack(buffer, &response, sizeof(buffer)); + response.crc = polymath::sygnal::generate_crc8(buffer); + mcm_control_control_command_response_pack(buffer, &response, sizeof(buffer)); + + std::array data; + std::copy(std::begin(buffer), std::end(buffer), data.begin()); + + polymath::socketcan::CanFrame frame; + frame.set_can_id(MCM_CONTROL_CONTROL_COMMAND_RESPONSE_FRAME_ID); + frame.set_len(MCM_CONTROL_CONTROL_COMMAND_RESPONSE_LENGTH); + frame.set_data(data); + return frame; +} + +polymath::socketcan::CanFrame createRelayResponseFrame(uint8_t bus_id, uint8_t subsystem_id, uint8_t enable) +{ + uint8_t buffer[CAN_MAX_DLC] = {0}; + mcm_relay_relay_command_response_t response; + mcm_relay_relay_command_response_init(&response); + response.bus_address = bus_id; + response.subsystem_id = subsystem_id; + response.enable = enable; + response.crc = 0; + + mcm_relay_relay_command_response_pack(buffer, &response, sizeof(buffer)); + response.crc = polymath::sygnal::generate_crc8(buffer); + mcm_relay_relay_command_response_pack(buffer, &response, sizeof(buffer)); + + std::array data; + std::copy(std::begin(buffer), std::end(buffer), data.begin()); + + polymath::socketcan::CanFrame frame; + frame.set_can_id(MCM_RELAY_RELAY_COMMAND_RESPONSE_FRAME_ID); + frame.set_len(MCM_RELAY_RELAY_COMMAND_RESPONSE_LENGTH); + frame.set_data(data); + return frame; +} + +polymath::socketcan::CanFrame createHeartbeatFrame( + uint8_t bus_id, uint8_t subsystem_id, uint8_t system_state, std::array interface_states) +{ + uint8_t buffer[CAN_MAX_DLC] = {0}; + mcm_heartbeat_heartbeat_t heartbeat; + mcm_heartbeat_heartbeat_init(&heartbeat); + heartbeat.bus_address = bus_id; + heartbeat.subsystem_id = subsystem_id; + heartbeat.system_state = system_state; + heartbeat.interface0_state = interface_states[0]; + heartbeat.interface1_state = interface_states[1]; + heartbeat.interface2_state = interface_states[2]; + heartbeat.interface3_state = interface_states[3]; + heartbeat.interface4_state = interface_states[4]; + heartbeat.interface5_state = 0; + heartbeat.interface6_state = 0; + heartbeat.overall_interface_state = 0; + heartbeat.count16 = 0; + heartbeat.crc = 0; + + mcm_heartbeat_heartbeat_pack(buffer, &heartbeat, sizeof(buffer)); + heartbeat.crc = polymath::sygnal::generate_crc8(buffer); + mcm_heartbeat_heartbeat_pack(buffer, &heartbeat, sizeof(buffer)); + + std::array data; + std::copy(std::begin(buffer), std::end(buffer), data.begin()); + + polymath::socketcan::CanFrame frame; + frame.set_can_id(MCM_HEARTBEAT_HEARTBEAT_FRAME_ID); + frame.set_len(MCM_HEARTBEAT_HEARTBEAT_LENGTH); + frame.set_data(data); + return frame; +} + +} // namespace + +TEST_CASE("SygnalInterfaceSocketcan vcan0 integration tests", "[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()); + + auto sygnal_interface = std::make_unique(interface_adapter); + + REQUIRE(interface_adapter->setOnReceiveCallback( + [&sygnal_interface](std::unique_ptr frame) { + sygnal_interface->parse(*frame); + })); + + REQUIRE(interface_adapter->startReceptionThread()); + + SECTION("Send control state command and receive response") + { + constexpr uint8_t bus_id = 1; + constexpr uint8_t interface_id = 2; + constexpr uint8_t subsystem_id = 0; + + std::string error_message; + auto result = sygnal_interface->sendControlStateCommand( + bus_id, interface_id, subsystem_id, polymath::sygnal::SygnalControlState::MCM_CONTROL, true, error_message); + + REQUIRE(result.success); + REQUIRE(result.response_future.has_value()); + + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + auto response_frame = createEnableResponseFrame(bus_id, interface_id, 1); + auto send_err = simulator_adapter->send(response_frame); + REQUIRE_FALSE(send_err.has_value()); + + auto status = result.response_future->wait_for(std::chrono::seconds(2)); + REQUIRE(status == std::future_status::ready); + + auto response = result.response_future->get(); + REQUIRE(response.response_type == polymath::sygnal::SygnalControlCommandResponseType::ENABLE); + REQUIRE(response.bus_id == bus_id); + REQUIRE(response.interface_id == interface_id); + REQUIRE(response.value == 1.0); + } + + SECTION("Send control command and receive response") + { + constexpr uint8_t bus_id = 1; + constexpr uint8_t interface_id = 3; + constexpr uint8_t subsystem_id = 0; + constexpr double control_value = 0.75; + + std::string error_message; + auto result = + sygnal_interface->sendControlCommand(bus_id, interface_id, subsystem_id, control_value, true, error_message); + + REQUIRE(result.success); + REQUIRE(result.response_future.has_value()); + + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + auto response_frame = createControlResponseFrame(bus_id, interface_id, static_cast(control_value)); + auto send_err = simulator_adapter->send(response_frame); + REQUIRE_FALSE(send_err.has_value()); + + auto status = result.response_future->wait_for(std::chrono::seconds(2)); + REQUIRE(status == std::future_status::ready); + + auto response = result.response_future->get(); + REQUIRE(response.response_type == polymath::sygnal::SygnalControlCommandResponseType::CONTROL); + REQUIRE(response.bus_id == bus_id); + REQUIRE(response.interface_id == interface_id); + REQUIRE(response.value == Approx(control_value).margin(0.01)); + } + + SECTION("Send relay command and receive response") + { + constexpr uint8_t bus_id = 1; + constexpr uint8_t subsystem_id = 0; + constexpr bool relay_state = true; + + std::string error_message; + auto result = sygnal_interface->sendRelayCommand(bus_id, subsystem_id, relay_state, true, error_message); + + REQUIRE(result.success); + REQUIRE(result.response_future.has_value()); + + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + + auto response_frame = createRelayResponseFrame(bus_id, subsystem_id, 1); + auto send_err = simulator_adapter->send(response_frame); + REQUIRE_FALSE(send_err.has_value()); + + auto status = result.response_future->wait_for(std::chrono::seconds(2)); + REQUIRE(status == std::future_status::ready); + + auto response = result.response_future->get(); + REQUIRE(response.response_type == polymath::sygnal::SygnalControlCommandResponseType::RELAY); + REQUIRE(response.bus_id == bus_id); + REQUIRE(response.interface_id == subsystem_id); + REQUIRE(response.value == 1.0); + } + + SECTION("Parse MCM heartbeat for subsystem 0") + { + constexpr uint8_t bus_id = 1; + constexpr uint8_t subsystem_id = 0; + constexpr uint8_t system_state = static_cast(polymath::sygnal::SygnalSystemState::MCM_CONTROL); + std::array interface_states = {1, 0, 1, 0, 1}; + + auto heartbeat_frame = createHeartbeatFrame(bus_id, subsystem_id, system_state, interface_states); + auto send_err = simulator_adapter->send(heartbeat_frame); + REQUIRE_FALSE(send_err.has_value()); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + auto mcm0_state = sygnal_interface->get_sygnal_mcm0_state(); + REQUIRE(mcm0_state == polymath::sygnal::SygnalSystemState::MCM_CONTROL); + + auto interface_states_result = sygnal_interface->get_interface_states_0(); + 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); + REQUIRE(interface_states_result[3] == polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); + REQUIRE(interface_states_result[4] == polymath::sygnal::SygnalSystemState::MCM_CONTROL); + } + + SECTION("Parse MCM heartbeat for subsystem 1") + { + constexpr uint8_t bus_id = 1; + constexpr uint8_t subsystem_id = 1; + constexpr uint8_t system_state = static_cast(polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); + std::array interface_states = {0, 1, 0, 1, 0}; + + auto heartbeat_frame = createHeartbeatFrame(bus_id, subsystem_id, system_state, interface_states); + auto send_err = simulator_adapter->send(heartbeat_frame); + REQUIRE_FALSE(send_err.has_value()); + + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + auto mcm1_state = sygnal_interface->get_sygnal_mcm1_state(); + REQUIRE(mcm1_state == polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); + + auto interface_states_result = sygnal_interface->get_interface_states_1(); + 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); + REQUIRE(interface_states_result[3] == polymath::sygnal::SygnalSystemState::MCM_CONTROL); + REQUIRE(interface_states_result[4] == polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); + } + + SECTION("Fire and forget command (no response expected)") + { + constexpr uint8_t bus_id = 1; + constexpr uint8_t interface_id = 0; + constexpr uint8_t subsystem_id = 0; + + std::string error_message; + auto result = sygnal_interface->sendControlStateCommand( + bus_id, interface_id, subsystem_id, polymath::sygnal::SygnalControlState::HUMAN_CONTROL, false, error_message); + + REQUIRE(result.success); + REQUIRE_FALSE(result.response_future.has_value()); + } + + SECTION("Invalid interface ID returns failure") + { + constexpr uint8_t bus_id = 1; + constexpr uint8_t invalid_interface_id = 10; + constexpr uint8_t subsystem_id = 0; + + std::string error_message; + auto result = sygnal_interface->sendControlStateCommand( + bus_id, + invalid_interface_id, + subsystem_id, + polymath::sygnal::SygnalControlState::MCM_CONTROL, + true, + error_message); + + REQUIRE_FALSE(result.success); + REQUIRE_FALSE(error_message.empty()); + } + + SECTION("Invalid subsystem ID returns failure") + { + constexpr uint8_t bus_id = 1; + constexpr uint8_t interface_id = 0; + constexpr uint8_t invalid_subsystem_id = 5; + + std::string error_message; + auto result = sygnal_interface->sendControlStateCommand( + bus_id, + interface_id, + invalid_subsystem_id, + polymath::sygnal::SygnalControlState::MCM_CONTROL, + true, + error_message); + + REQUIRE_FALSE(result.success); + REQUIRE_FALSE(error_message.empty()); + } + + interface_adapter->joinReceptionThread(); + interface_adapter->closeSocket(); + simulator_adapter->closeSocket(); +} From 1d4ca17a5ac7ab432f29075724491acaa932e47c Mon Sep 17 00:00:00 2001 From: Zeerek Date: Thu, 29 Jan 2026 23:46:46 +0000 Subject: [PATCH 14/15] Update tests to use futures and add timestamps to mcm for now --- .../sygnal_interface_socketcan.hpp | 2 ++ .../sygnal_mcm_interface.hpp | 6 ++++++ .../src/sygnal_interface_socketcan.cpp | 12 ++++++++++++ .../src/sygnal_mcm_interface.cpp | 4 ++++ .../test/sygnal_interface_socketcan_test.cpp | 19 +++++++++++++------ 5 files changed, 37 insertions(+), 6 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 44a9740..b435892 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,8 @@ struct SendCommandResult std::optional> response_future; }; +constexpr uint32_t MAX_PROMISE_QUEUE_LENGTH = 100; + /// @brief Combined Sygnal MCM and Control interface with SocketCAN communication /// Provides thread-safe async communication with promise/future pattern for responses class SygnalInterfaceSocketcan 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 0b67e75..ae20d44 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 @@ -74,12 +74,18 @@ class SygnalMcmInterface return sygnal_mcm_state_; } + std::chrono::system_clock::time_point get_last_heartbeat_timestamp() const + { + return last_heartbeat_timestamp_; + } + bool parseMcmHeartbeatFrame(const socketcan::CanFrame & frame); private: uint8_t subsystem_id_; SygnalSystemState sygnal_mcm_state_; std::array sygnal_interface_states_; + std::chrono::system_clock::time_point last_heartbeat_timestamp_; }; } // namespace polymath::sygnal 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 328f3ae..24c7cab 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 @@ -118,6 +118,10 @@ SendCommandResult SygnalInterfaceSocketcan::sendControlStateCommand( std::promise promise; future_opt = promise.get_future(); std::lock_guard lock(promises_mutex_); + if (enable_response_promises_.size() >= MAX_PROMISE_QUEUE_LENGTH) { + // Really old promises are dropped silently for now. We should warn in the future + enable_response_promises_.pop(); + } enable_response_promises_.push(std::move(promise)); } @@ -150,6 +154,10 @@ SendCommandResult SygnalInterfaceSocketcan::sendControlCommand( std::promise promise; future_opt = promise.get_future(); std::lock_guard lock(promises_mutex_); + if (control_response_promises_.size() >= MAX_PROMISE_QUEUE_LENGTH) { + // Really old promises are dropped silently for now. We should warn in the future + control_response_promises_.pop(); + } control_response_promises_.push(std::move(promise)); } @@ -176,6 +184,10 @@ SendCommandResult SygnalInterfaceSocketcan::sendRelayCommand( std::promise promise; future_opt = promise.get_future(); std::lock_guard lock(promises_mutex_); + if (relay_response_promises_.size() >= MAX_PROMISE_QUEUE_LENGTH) { + // Really old promises are dropped silently for now. We should warn in the future + relay_response_promises_.pop(); + } relay_response_promises_.push(std::move(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 828b2bb..0677392 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 @@ -14,6 +14,8 @@ #include "sygnal_can_interface_lib/sygnal_mcm_interface.hpp" +#include + #include "sygnal_can_interface_lib/crc8.hpp" #include "sygnal_dbc/mcm_heartbeat.h" @@ -53,6 +55,8 @@ bool SygnalMcmInterface::parseMcmHeartbeatFrame(const socketcan::CanFrame & fram return false; } + last_heartbeat_timestamp_ = std::chrono::system_clock::now(); + // Set the MCM state sygnal_mcm_state_ = SygnalSystemState(unpacked_heartbeat_t.system_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 3a3907e..79a3a1e 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 @@ -24,6 +24,7 @@ #endif #include +#include #include #include "socketcan_adapter/socketcan_adapter.hpp" @@ -156,8 +157,12 @@ TEST_CASE("SygnalInterfaceSocketcan vcan0 integration tests", "[vcan]") auto sygnal_interface = std::make_unique(interface_adapter); + std::promise frame_received_promise; + std::future frame_received_future = frame_received_promise.get_future(); + REQUIRE(interface_adapter->setOnReceiveCallback( - [&sygnal_interface](std::unique_ptr frame) { + [&sygnal_interface, &frame_received_promise](std::unique_ptr frame) { + frame_received_promise.set_value(); sygnal_interface->parse(*frame); })); @@ -176,7 +181,7 @@ TEST_CASE("SygnalInterfaceSocketcan vcan0 integration tests", "[vcan]") REQUIRE(result.success); REQUIRE(result.response_future.has_value()); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + // std::this_thread::sleep_for(std::chrono::milliseconds(10)); auto response_frame = createEnableResponseFrame(bus_id, interface_id, 1); auto send_err = simulator_adapter->send(response_frame); @@ -206,7 +211,7 @@ TEST_CASE("SygnalInterfaceSocketcan vcan0 integration tests", "[vcan]") REQUIRE(result.success); REQUIRE(result.response_future.has_value()); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + // std::this_thread::sleep_for(std::chrono::milliseconds(10)); auto response_frame = createControlResponseFrame(bus_id, interface_id, static_cast(control_value)); auto send_err = simulator_adapter->send(response_frame); @@ -234,7 +239,7 @@ TEST_CASE("SygnalInterfaceSocketcan vcan0 integration tests", "[vcan]") REQUIRE(result.success); REQUIRE(result.response_future.has_value()); - std::this_thread::sleep_for(std::chrono::milliseconds(10)); + // std::this_thread::sleep_for(std::chrono::milliseconds(10)); auto response_frame = createRelayResponseFrame(bus_id, subsystem_id, 1); auto send_err = simulator_adapter->send(response_frame); @@ -261,7 +266,9 @@ TEST_CASE("SygnalInterfaceSocketcan vcan0 integration tests", "[vcan]") auto send_err = simulator_adapter->send(heartbeat_frame); REQUIRE_FALSE(send_err.has_value()); - std::this_thread::sleep_for(std::chrono::milliseconds(50)); + // std::this_thread::sleep_for(std::chrono::milliseconds(50)); + + 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); @@ -285,7 +292,7 @@ TEST_CASE("SygnalInterfaceSocketcan vcan0 integration tests", "[vcan]") auto send_err = simulator_adapter->send(heartbeat_frame); REQUIRE_FALSE(send_err.has_value()); - std::this_thread::sleep_for(std::chrono::milliseconds(50)); + 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); From fc82a873686e38c0d83c2a3a3d760d7995a83701 Mon Sep 17 00:00:00 2001 From: Zeerek Date: Fri, 30 Jan 2026 00:14:27 +0000 Subject: [PATCH 15/15] Add in generate parameter library --- .../sygnal_can_interface_ros2/CMakeLists.txt | 9 +++++- .../sygnal_can_interface_node.hpp | 7 +++-- .../sygnal_can_interface_ros2/package.xml | 1 + .../src/sygnal_can_interface_node.cpp | 28 ++++++++----------- .../src/sygnal_can_interface_params.yaml | 17 +++++++++++ 5 files changed, 42 insertions(+), 20 deletions(-) create mode 100644 sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_params.yaml diff --git a/sygnal_can_interface/sygnal_can_interface_ros2/CMakeLists.txt b/sygnal_can_interface/sygnal_can_interface_ros2/CMakeLists.txt index 39ddf0d..9c6138f 100644 --- a/sygnal_can_interface/sygnal_can_interface_ros2/CMakeLists.txt +++ b/sygnal_can_interface/sygnal_can_interface_ros2/CMakeLists.txt @@ -26,8 +26,14 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() find_package(ament_cmake_auto REQUIRED) +find_package(generate_parameter_library REQUIRED) ament_auto_find_build_dependencies() +generate_parameter_library( + sygnal_can_interface_params + src/sygnal_can_interface_params.yaml +) + if(CMAKE_BUILD_TYPE STREQUAL "Debug") message(STATUS "Enabling AddressSanitizer (ASAN) for Debug build") set(ASAN_FLAGS "-fsanitize=address -fno-omit-frame-pointer") @@ -55,6 +61,7 @@ target_link_libraries(${PROJECT_NAME} PUBLIC ${sygnal_can_msgs_TARGETS} sygnal_can_interface_lib::sygnal_can_interface_lib socketcan_adapter::socketcan_adapter + sygnal_can_interface_params ) rclcpp_components_register_node(${PROJECT_NAME} @@ -65,7 +72,7 @@ rclcpp_components_register_node(${PROJECT_NAME} install(DIRECTORY include/ DESTINATION include) install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch) -install(TARGETS ${PROJECT_NAME} +install(TARGETS ${PROJECT_NAME} sygnal_can_interface_params EXPORT ${PROJECT_NAME}_TARGETS ARCHIVE DESTINATION lib LIBRARY DESTINATION lib 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 1dce894..32d801c 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 @@ -21,6 +21,8 @@ #include #include +#include + #include "diagnostic_msgs/msg/diagnostic_array.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_components/register_node_macro.hpp" @@ -95,9 +97,8 @@ class SygnalCanInterfaceNode : public rclcpp_lifecycle::LifecycleNode diagnostic_msgs::msg::DiagnosticArray createDiagnosticsMessage(); // Parameters - std::string can_interface_; - double publish_rate_; - std::chrono::milliseconds timeout_ms_; + std::shared_ptr param_listener_; + sygnal_can_interface_ros2::Params params_; // SocketCAN and Sygnal components std::shared_ptr socketcan_adapter_; diff --git a/sygnal_can_interface/sygnal_can_interface_ros2/package.xml b/sygnal_can_interface/sygnal_can_interface_ros2/package.xml index 9e085f9..d310d64 100644 --- a/sygnal_can_interface/sygnal_can_interface_ros2/package.xml +++ b/sygnal_can_interface/sygnal_can_interface_ros2/package.xml @@ -9,6 +9,7 @@ ament_cmake ament_cmake_auto + generate_parameter_library rclcpp rclcpp_lifecycle 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 312299b..06b23e0 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 @@ -33,10 +33,8 @@ namespace polymath::sygnal SygnalCanInterfaceNode::SygnalCanInterfaceNode(const rclcpp::NodeOptions & options) : rclcpp_lifecycle::LifecycleNode("sygnal_can_interface_node", "", options) { - // Declare parameters - declare_parameter("can_interface", std::string("can0")); - declare_parameter("publish_rate", 3.0); // Hz - declare_parameter("timeout_ms", 500); // ms + param_listener_ = std::make_shared(get_node_parameters_interface()); + params_ = param_listener_->get_params(); } SygnalCanInterfaceNode::~SygnalCanInterfaceNode() @@ -50,20 +48,18 @@ SygnalCanInterfaceNode::~SygnalCanInterfaceNode() rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SygnalCanInterfaceNode::on_configure( const rclcpp_lifecycle::State &) { - // Get parameters - can_interface_ = get_parameter("can_interface").as_string(); - publish_rate_ = get_parameter("publish_rate").as_double(); - auto timeout_ms = get_parameter("timeout_ms").as_int(); - timeout_ms_ = std::chrono::milliseconds(timeout_ms); + // Get parameters (refresh in case they changed) + params_ = param_listener_->get_params(); - RCLCPP_INFO(get_logger(), "Configuring Sygnal CAN Interface node with CAN interface: %s", can_interface_.c_str()); + RCLCPP_INFO( + get_logger(), "Configuring Sygnal CAN Interface node with CAN interface: %s", params_.can_interface.c_str()); try { // Initialize SocketCAN adapter - socketcan_adapter_ = std::make_shared(can_interface_); + socketcan_adapter_ = std::make_shared(params_.can_interface); if (!socketcan_adapter_->openSocket()) { - RCLCPP_ERROR(get_logger(), "Failed to open SocketCAN interface: %s", can_interface_.c_str()); + RCLCPP_ERROR(get_logger(), "Failed to open SocketCAN interface: %s", params_.can_interface.c_str()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; } @@ -100,7 +96,7 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Sygnal std::bind(&SygnalCanInterfaceNode::sendRelayCommandCallback, this, std::placeholders::_1, std::placeholders::_2)); // Create timer for publishing - auto period = std::chrono::duration(1.0 / publish_rate_); + auto period = std::chrono::duration(1.0 / params_.publish_rate); timer_ = create_wall_timer( std::chrono::duration_cast(period), std::bind(&SygnalCanInterfaceNode::timerCallback, this)); @@ -242,7 +238,7 @@ void SygnalCanInterfaceNode::setControlStateCallback( // If we expect a reply, wait for response with timeout if (request->expect_reply && result.response_future.has_value()) { auto future = std::move(result.response_future.value()); - auto status = future.wait_for(timeout_ms_); + auto status = future.wait_for(std::chrono::milliseconds(params_.timeout_ms)); if (status == std::future_status::ready) { auto command_response = future.get(); @@ -297,7 +293,7 @@ void SygnalCanInterfaceNode::sendControlCommandCallback( // If we expect a reply, wait for response with timeout if (request->expect_reply && result.response_future.has_value()) { auto future = std::move(result.response_future.value()); - auto status = future.wait_for(timeout_ms_); + auto status = future.wait_for(std::chrono::milliseconds(params_.timeout_ms)); if (status == std::future_status::ready) { auto command_response = future.get(); @@ -378,7 +374,7 @@ void SygnalCanInterfaceNode::sendRelayCommandCallback( // If we expect a reply, wait for response with timeout if (request->expect_reply && result.response_future.has_value()) { auto future = std::move(result.response_future.value()); - auto status = future.wait_for(timeout_ms_); + auto status = future.wait_for(std::chrono::milliseconds(params_.timeout_ms)); if (status == std::future_status::ready) { auto command_response = future.get(); 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 new file mode 100644 index 0000000..f6c4c3a --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_params.yaml @@ -0,0 +1,17 @@ +sygnal_can_interface_ros2: + can_interface: + type: string + default_value: "can0" + description: "CAN network interface name" + publish_rate: + type: double + default_value: 3.0 + description: "Publishing frequency in Hz" + validation: + gt<>: [0.0] + timeout_ms: + type: int + default_value: 500 + description: "Command response timeout in milliseconds" + validation: + gt<>: [0]