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..34c114a --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/CMakeLists.txt @@ -0,0 +1,108 @@ +# 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_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/crc8.cpp + src/sygnal_mcm_interface.cpp + src/sygnal_command_interface.cpp + src/sygnal_interface_socketcan.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 +) + +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) + + 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/README.md b/sygnal_can_interface/sygnal_can_interface_lib/README.md new file mode 100644 index 0000000..ad8743b --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/README.md @@ -0,0 +1,3 @@ +# Sygnal Can Interface Lib + +C++ Libarary for interfacing with Sygnal's CAN Interfaces. Currently only supports MCM command and feedback. 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_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..ef5b924 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_command_interface.hpp @@ -0,0 +1,107 @@ +// 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_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 +}; + +constexpr uint32_t MAX_SYGNAL_INTERFACES = 6; +constexpr uint32_t MAX_SYGNAL_SUBSYSTEMS = 1; + +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; +}; + +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 uint8_t subsystem_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 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 + /// @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_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..b435892 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_interface_socketcan.hpp @@ -0,0 +1,122 @@ +// 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; +}; + +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 +{ +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 + bool parse(const socketcan::CanFrame & frame); + + /// @brief Get interface states array from MCM 0 + std::array get_interface_states_0() const; + + /// @brief Get interface states array from MCM 1 + std::array get_interface_states_1() const; + + /// @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, + uint8_t subsystem_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, + uint8_t subsystem_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_0_; + SygnalMcmInterface mcm_interface_1_; + 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/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..ae20d44 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/include/sygnal_can_interface_lib/sygnal_mcm_interface.hpp @@ -0,0 +1,93 @@ +// 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_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: + explicit SygnalMcmInterface(uint8_t subsystem_id); + ~SygnalMcmInterface() = default; + + std::array get_interface_states() const + { + return sygnal_interface_states_; + } + + SygnalSystemState get_mcm_state() const + { + 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 + +#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/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 new file mode 100644 index 0000000..b84069e --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_command_interface.cpp @@ -0,0 +1,255 @@ +// 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 "sygnal_can_interface_lib/crc8.hpp" +#include "sygnal_dbc/mcm_control.h" +#include "sygnal_dbc/mcm_relay.h" + +namespace polymath::sygnal +{ + +SygnalControlInterface::SygnalControlInterface() +{} + +std::optional SygnalControlInterface::createControlStateCommandFrame( + const uint8_t bus_id, + const uint8_t interface_id, + const uint8_t subsystem_id, + 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; + + 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 uint8_t subsystem_id, + 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; + + 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_interface_socketcan.cpp b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp new file mode 100644 index 0000000..24c7cab --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_interface_socketcan.cpp @@ -0,0 +1,203 @@ +// 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_0_(0) +, mcm_interface_1_(1) +, control_interface_() +{} + +bool SygnalInterfaceSocketcan::parse(const socketcan::CanFrame & frame) +{ + // Try parsing as MCM heartbeat (both interfaces will check subsystem_id) + if (mcm_interface_0_.parseMcmHeartbeatFrame(frame)) { + return true; + }; + + if (mcm_interface_1_.parseMcmHeartbeatFrame(frame)) { + return true; + }; + + // Try parsing as command response + auto response = control_interface_.parseCommandResponseFrame(frame); + if (!response.has_value()) { + return false; + } + + // 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; + } + } + + return true; +} + +std::array SygnalInterfaceSocketcan::get_interface_states_0() const +{ + return mcm_interface_0_.get_interface_states(); +} + +std::array SygnalInterfaceSocketcan::get_interface_states_1() const +{ + return mcm_interface_1_.get_interface_states(); +} + +SygnalSystemState SygnalInterfaceSocketcan::get_sygnal_mcm0_state() const +{ + return mcm_interface_0_.get_mcm_state(); +} + +SygnalSystemState SygnalInterfaceSocketcan::get_sygnal_mcm1_state() const +{ + return mcm_interface_1_.get_mcm_state(); +} + +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, subsystem_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_); + 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)); + } + + 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, + uint8_t subsystem_id, + double value, + bool expect_reply, + std::string & 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}; + } + + std::optional> future_opt; + if (expect_reply) { + 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)); + } + + 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_); + 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)); + } + + 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 new file mode 100644 index 0000000..0677392 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/src/sygnal_mcm_interface.cpp @@ -0,0 +1,73 @@ +// 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 "sygnal_can_interface_lib/crc8.hpp" +#include "sygnal_dbc/mcm_heartbeat.h" + +namespace polymath::sygnal +{ + +SygnalMcmInterface::SygnalMcmInterface(uint8_t subsystem_id) +: subsystem_id_(subsystem_id) +, sygnal_mcm_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; + } + + // Only parse if this frame is for our subsystem + if (unpacked_heartbeat_t.subsystem_id != subsystem_id_) { + return false; + } + + last_heartbeat_timestamp_ = std::chrono::system_clock::now(); + + // 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); + 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 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..a6ebbb8 --- /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, 0, 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, 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_interface_socketcan_test.cpp b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_test.cpp new file mode 100644 index 0000000..79a3a1e --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_interface_socketcan_test.cpp @@ -0,0 +1,363 @@ +// 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 + +#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); + + std::promise frame_received_promise; + std::future frame_received_future = frame_received_promise.get_future(); + + REQUIRE(interface_adapter->setOnReceiveCallback( + [&sygnal_interface, &frame_received_promise](std::unique_ptr frame) { + frame_received_promise.set_value(); + 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)); + + frame_received_future.wait_for(std::chrono::seconds(2)); + + auto mcm0_state = sygnal_interface->get_sygnal_mcm0_state(); + REQUIRE(mcm0_state == polymath::sygnal::SygnalSystemState::MCM_CONTROL); + + auto 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()); + + frame_received_future.wait_for(std::chrono::seconds(2)); + + auto mcm1_state = sygnal_interface->get_sygnal_mcm1_state(); + REQUIRE(mcm1_state == polymath::sygnal::SygnalSystemState::HUMAN_CONTROL); + + auto 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(); +} 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..427ecc8 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_lib/test/sygnal_mcm_interface_test.cpp @@ -0,0 +1,159 @@ +// 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 for subsystem 0", "[sygnal_mcm_interface]") +{ + polymath::sygnal::SygnalMcmInterface interface(0); + + 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) { + REQUIRE(state == polymath::sygnal::SygnalSystemState::FAIL_HARD); + } +} + +TEST_CASE("SygnalMcmInterface parses MCM_CONTROL heartbeat", "[sygnal_mcm_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_mcm_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(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_mcm_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(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_mcm_state() == polymath::sygnal::SygnalSystemState::FAIL_HARD); +} + +TEST_CASE("SygnalMcmInterface rejects bad CRC", "[sygnal_mcm_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_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]") +{ + 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"); +} 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..9c6138f --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_ros2/CMakeLists.txt @@ -0,0 +1,88 @@ +# 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) +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") + 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 + sygnal_can_interface_params +) + +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "polymath::sygnal::SygnalCanInterfaceNode" + EXECUTABLE ${PROJECT_NAME}_node) + +# Installs +install(DIRECTORY include/ DESTINATION include) +install(DIRECTORY launch/ DESTINATION share/${PROJECT_NAME}/launch) + +install(TARGETS ${PROJECT_NAME} sygnal_can_interface_params + 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..32d801c --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_ros2/include/sygnal_can_interface_ros2/sygnal_can_interface_node.hpp @@ -0,0 +1,129 @@ +// 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 + +#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 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(); + + // Parameters + std::shared_ptr param_listener_; + sygnal_can_interface_ros2::Params params_; + + // 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 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_; +}; + +} // 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..d310d64 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_ros2/package.xml @@ -0,0 +1,28 @@ + + + + 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 + generate_parameter_library + + 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..06b23e0 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_interface_ros2/src/sygnal_can_interface_node.cpp @@ -0,0 +1,523 @@ +// 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 + +#include "sygnal_can_interface_lib/sygnal_command_interface.hpp" +#include "sygnal_can_interface_lib/sygnal_mcm_interface.hpp" + +namespace polymath::sygnal +{ + +SygnalCanInterfaceNode::SygnalCanInterfaceNode(const rclcpp::NodeOptions & options) +: rclcpp_lifecycle::LifecycleNode("sygnal_can_interface_node", "", options) +{ + param_listener_ = std::make_shared(get_node_parameters_interface()); + params_ = param_listener_->get_params(); +} + +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 (refresh in case they changed) + params_ = param_listener_->get_params(); + + 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(params_.can_interface); + + if (!socketcan_adapter_->openSocket()) { + RCLCPP_ERROR(get_logger(), "Failed to open SocketCAN interface: %s", params_.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)); + 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( + "~/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 / params_.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(); + 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; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn SygnalCanInterfaceNode::on_deactivate( + const rclcpp_lifecycle::State &) +{ + // Deactivate publishers + diagnostics_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; +} + +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(); + mcm0_heartbeat_pub_.reset(); + mcm1_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() +{ + // Get MCM states from sygnal_interface_ + auto mcm0_state = sygnal_interface_->get_sygnal_mcm0_state(); + auto mcm1_state = sygnal_interface_->get_sygnal_mcm1_state(); + auto interface_states_0 = sygnal_interface_->get_interface_states_0(); + auto interface_states_1 = sygnal_interface_->get_interface_states_1(); + + // Create and populate MCM 0 heartbeat message + sygnal_can_msgs::msg::McmHeartbeat mcm0_msg; + mcm0_msg.header.stamp = now(); + mcm0_msg.state = static_cast(mcm0_state); + + // Convert interface states array to vector for MCM 0 + mcm0_msg.interface_states.clear(); + for (const auto & state : interface_states_0) { + mcm0_msg.interface_states.push_back(static_cast(state)); + } + + // 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 0 and MCM 1 heartbeats and diagnostics"); +} + +void SygnalCanInterfaceNode::setControlStateCallback( + const std::shared_ptr request, + std::shared_ptr response) +{ + RCLCPP_INFO( + get_logger(), + "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 + 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, request->subsystem_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(std::chrono::milliseconds(params_.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) +{ + RCLCPP_INFO( + get_logger(), + "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 + std::string error_message; + 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); + + 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(std::chrono::milliseconds(params_.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::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, 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->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()); + } 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) +{ + 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); + + // 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(std::chrono::milliseconds(params_.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() +{ + 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_0 = sygnal_interface_->get_interface_states_0(); + auto interface_states_1 = sygnal_interface_->get_interface_states_1(); + + // Create MCM 0 diagnostics + { + diagnostic_msgs::msg::DiagnosticStatus mcm0_diag; + mcm0_diag.name = "sygnal_mcm0"; + mcm0_diag.hardware_id = "sygnal_can_interface"; + + // Determine diagnostic level based on state + if (mcm0_state == SygnalSystemState::MCM_CONTROL || mcm0_state == SygnalSystemState::HUMAN_CONTROL) { + mcm0_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + } else if ( + mcm0_state == SygnalSystemState::FAIL_OPERATIONAL_1 || mcm0_state == SygnalSystemState::FAIL_OPERATIONAL_2) + { + mcm0_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } else { + mcm0_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + + mcm0_diag.message = "MCM0 state: " + 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; + } else if ( + mcm1_state == SygnalSystemState::FAIL_OPERATIONAL_1 || mcm1_state == SygnalSystemState::FAIL_OPERATIONAL_2) + { + mcm1_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } else { + mcm1_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + + mcm1_diag.message = "MCM1 state: " + 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) from both MCM subsystems + for (size_t i = 0; i < interface_states_0.size(); ++i) { + auto state_0 = interface_states_0[i]; + auto state_1 = interface_states_1[i]; + + // Create diagnostic for this interface + diagnostic_msgs::msg::DiagnosticStatus interface_diag; + interface_diag.name = "sygnal_interface_" + std::to_string(i); + interface_diag.hardware_id = "sygnal_can_interface"; + + // Determine worst-case diagnostic level from both MCMs + auto worst_state = state_0; + if (static_cast(state_1) > static_cast(state_0)) { + worst_state = state_1; + } + + if (worst_state == SygnalSystemState::MCM_CONTROL || worst_state == SygnalSystemState::HUMAN_CONTROL) { + interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::OK; + } else if ( + worst_state == SygnalSystemState::FAIL_OPERATIONAL_1 || worst_state == SygnalSystemState::FAIL_OPERATIONAL_2) + { + interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::WARN; + } else { + interface_diag.level = diagnostic_msgs::msg::DiagnosticStatus::ERROR; + } + + // Build message showing states from both MCMs + interface_diag.message = "Interface " + std::to_string(i) + " - MCM0: " + sygnalSystemStateToString(state_0) + + ", MCM1: " + sygnalSystemStateToString(state_1); + + // Add state values from both MCMs + diagnostic_msgs::msg::KeyValue kv_mcm0; + kv_mcm0.key = "mcm0_state"; + kv_mcm0.value = sygnalSystemStateToString(state_0); + interface_diag.values.push_back(kv_mcm0); + + diagnostic_msgs::msg::KeyValue kv_mcm1; + kv_mcm1.key = "mcm1_state"; + kv_mcm1.value = sygnalSystemStateToString(state_1); + interface_diag.values.push_back(kv_mcm1); + + // Flag if states differ + if (state_0 != state_1) { + diagnostic_msgs::msg::KeyValue kv_mismatch; + kv_mismatch.key = "state_mismatch"; + kv_mismatch.value = "true"; + interface_diag.values.push_back(kv_mismatch); + } + + array_msg.status.push_back(interface_diag); + } + + return array_msg; +} + +} // namespace polymath::sygnal 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] 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..bd7e46a --- /dev/null +++ b/sygnal_can_interface/sygnal_can_msgs/CMakeLists.txt @@ -0,0 +1,49 @@ +# 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" + "msg/ControlCommand.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/ControlCommand.msg b/sygnal_can_interface/sygnal_can_msgs/msg/ControlCommand.msg new file mode 100644 index 0000000..2f90c29 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_msgs/msg/ControlCommand.msg @@ -0,0 +1,13 @@ +# Command to forward to Sygnal's Systems + +# Bus ID (0-127) +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/msg/McmHeartbeat.msg b/sygnal_can_interface/sygnal_can_msgs/msg/McmHeartbeat.msg new file mode 100644 index 0000000..fb35345 --- /dev/null +++ b/sygnal_can_interface/sygnal_can_msgs/msg/McmHeartbeat.msg @@ -0,0 +1,17 @@ +# MCM heartbeat message containing system state information +# Header for timestamp +std_msgs/Header header + +# 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 this MCM subsystem (0-255) +uint8 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..43c858d --- /dev/null +++ b/sygnal_can_interface/sygnal_can_msgs/srv/SendControlCommand.srv @@ -0,0 +1,16 @@ +# Service to send a control command with a value +# Request + +sygnal_can_msgs/ControlCommand command + +# 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..f38b5ac --- /dev/null +++ b/sygnal_can_interface/sygnal_can_msgs/srv/SetControlState.srv @@ -0,0 +1,29 @@ +# 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 + +# 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