Add Interface Library and ROS2 Implementation for the MCM and MCM Interfaces

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
+  "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
+  "$<INSTALL_INTERFACE:include>"
+)
+
+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()

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.

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 <cstdint>
+
+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_

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 <optional>
+#include <string>
+
+#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<polymath::socketcan::CanFrame> 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<polymath::socketcan::CanFrame> 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<polymath::socketcan::CanFrame> 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<SygnalControlCommandResponse> parseCommandResponseFrame(const socketcan::CanFrame & frame);
+};
+
+}  // namespace polymath::sygnal
+
+#endif  // SYGNAL_CAN_INTERFACE_LIB__SYGNAL_COMMAND_INTERFACE_HPP_

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 <future>
+#include <memory>
+#include <mutex>
+#include <optional>
+#include <queue>
+#include <string>
+
+#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<std::future<SygnalControlCommandResponse>> 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::SocketcanAdapter> 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<SygnalSystemState, 5> get_interface_states_0() const;
+
+  /// @brief Get interface states array from MCM 1
+  std::array<SygnalSystemState, 5> 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::SocketcanAdapter> socketcan_adapter_;
+  SygnalMcmInterface mcm_interface_0_;
+  SygnalMcmInterface mcm_interface_1_;
+  SygnalControlInterface control_interface_;
+
+  // Promise queues for each response type
+  std::queue<std::promise<SygnalControlCommandResponse>> enable_response_promises_;
+  std::queue<std::promise<SygnalControlCommandResponse>> control_response_promises_;
+  std::queue<std::promise<SygnalControlCommandResponse>> relay_response_promises_;
+
+  std::mutex promises_mutex_;
+};
+
+}  // namespace polymath::sygnal
+
+#endif  // SYGNAL_CAN_INTERFACE_LIB__SYGNAL_INTERFACE_SOCKETCAN_HPP_