diff --git a/.config/clang-format b/.config/clang-format new file mode 100644 index 0000000..587bc1b --- /dev/null +++ b/.config/clang-format @@ -0,0 +1,68 @@ +--- + +# See https://releases.llvm.org/14.0.0/tools/clang/docs/ClangFormatStyleOptions.html for documentation of these options +BasedOnStyle: Google +IndentWidth: 2 +ColumnLimit: 120 + +AccessModifierOffset: -2 +AlignAfterOpenBracket: AlwaysBreak +AlignConsecutiveAssignments: None +AlignConsecutiveDeclarations: None +AlignEscapedNewlines: Left +AlignTrailingComments: false +AllowAllArgumentsOnNextLine: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: Empty +AllowShortFunctionsOnASingleLine: false +BinPackArguments: false +BinPackParameters: false +BraceWrapping: + AfterClass: true + AfterControlStatement: MultiLine + AfterEnum: true + AfterFunction: true + AfterNamespace: true + AfterStruct: true + AfterUnion: true + AfterExternBlock: true + BeforeCatch: false + BeforeElse: false + BeforeLambdaBody: false + BeforeWhile: false + IndentBraces: false + SplitEmptyFunction: false + SplitEmptyRecord: false + SplitEmptyNamespace: false +BreakBeforeBraces: Custom +BreakConstructorInitializers: BeforeComma +CompactNamespaces: false +ContinuationIndentWidth: 2 +ConstructorInitializerIndentWidth: 0 +DerivePointerAlignment: false +EmptyLineAfterAccessModifier: Never +EmptyLineBeforeAccessModifier: LogicalBlock +FixNamespaceComments: true +IncludeBlocks: Regroup +IncludeCategories: + # Headers in <> with .h extension (best guess at C system headers) + - Regex: '<([A-Za-z0-9\Q/-_\E])+\.h>' + Priority: 1 + # Headers in <> without extension (C++ system headers) + - Regex: '<([A-Za-z0-9\Q/-_\E])+>' + Priority: 2 + # Headers in <> with other extensions. + - Regex: '<([A-Za-z0-9.\Q/-_\E])+>' + Priority: 3 + # Headers in "" + - Regex: '"([A-Za-z0-9.\Q/-_\E])+"' + Priority: 4 +IndentAccessModifiers: false +IndentPPDirectives: BeforeHash +PackConstructorInitializers: Never +PointerAlignment: Middle +ReferenceAlignment: Middle +ReflowComments: false +SeparateDefinitionBlocks: Always +SortIncludes: CaseInsensitive +SpacesBeforeTrailingComments: 2 diff --git a/.config/copyright.txt b/.config/copyright.txt new file mode 100644 index 0000000..a617e4a --- /dev/null +++ b/.config/copyright.txt @@ -0,0 +1,13 @@ +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. diff --git a/.cpplint.cfg b/.cpplint.cfg new file mode 100644 index 0000000..8f355ec --- /dev/null +++ b/.cpplint.cfg @@ -0,0 +1,19 @@ +# Because of cpplint's config file assumptions, this can't be contained in .config/ directory +linelength=256 + +# TODO(emerson) we need to apply a copyright check, maybe not via this tool though +filter=-legal/copyright +# TODO(emerson) we want these, but the style as enforced here is probably not quite right for us +filter=-build/header_guard +filter=-build/c++17 + +# Per our style guide, we want to allow the use of non-const reference passing for output parameters +filter=-runtime/references + +# Disable all formatting checks, this is handled by clang-format +filter=-readability/braces +filter=-whitespace/braces +filter=-whitespace/indent +filter=-whitespace/newline +filter=-whitespace/parens +filter=-whitespace/semicolon diff --git a/.github/workflows/build.yml b/.github/workflows/build.yml new file mode 100644 index 0000000..0bb56a4 --- /dev/null +++ b/.github/workflows/build.yml @@ -0,0 +1,37 @@ +--- + +name: Build and test +"on": + pull_request: + push: + branches: + - main + +jobs: + build_and_test: + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + include: + - ros: humble + ubuntu: jammy + - ros: jazzy + ubuntu: noble + - ros: rolling + ubuntu: noble + name: ROS 2 ${{ matrix.ros }} + container: + image: ghcr.io/ros-tooling/setup-ros-docker/setup-ros-docker-ubuntu-${{ matrix.ubuntu }}:latest + env: + ROS_DISTRO: ${{ matrix.ros }} + PIP_BREAK_SYSTEM_PACKAGES: 1 + steps: + - uses: actions/checkout@v4 + - uses: ros-tooling/action-ros-ci@v0.4 + with: + target-ros2-distro: ${{ matrix.ros }} + - uses: actions/upload-artifact@v4 + with: + name: colcon-logs-${{ matrix.ros }} + path: ros_ws/log diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml new file mode 100644 index 0000000..093bf31 --- /dev/null +++ b/.github/workflows/pre-commit.yml @@ -0,0 +1,18 @@ +--- +name: pre-commit + +"on": + pull_request: + push: + branches: [main] + +jobs: + pre-commit: + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: "3.10" + - run: sudo apt-get update && sudo apt-get install libxml2-utils + - uses: pre-commit/action@v3.0.1 diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ce0d030 --- /dev/null +++ b/.gitignore @@ -0,0 +1,4 @@ +# Colcon +/build/ +/install/ +/log/ diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml deleted file mode 100644 index ddcdddc..0000000 --- a/.gitlab-ci.yml +++ /dev/null @@ -1,25 +0,0 @@ -include: - - project: "polymathrobotics/ci/ci_templates" - ref: main - file: "/ros/ros2_package.impl.yml" - # Enables checking out the current branch on build and test jobs - - project: "polymathrobotics/ci/ci_templates" - ref: main - file: "/common/rules.yml" - -build_and_test_socketcan_adapter: - variables: - PACKAGE_NAME: socketcan_adapter - PACKAGE_DIRECTORY: $PACKAGE_NAME - extends: .ros2_build_and_test - -eval_socketcan_adapter: - extends: .ros2_evaluate - variables: - PACKAGE_NAME: socketcan_adapter - needs: - - job: build_and_test_socketcan_adapter - artifacts: true - artifacts: - reports: - junit: $ARTIFACTS_PATH/test_results/test_results/$PACKAGE_NAME/*.xml diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..267768c --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,81 @@ +--- +# See https://pre-commit.com for more information on these settings +repos: + # Generally useful checks provided by pre-commit + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v5.0.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-merge-conflict + - id: check-shebang-scripts-are-executable + - id: check-symlinks + - id: check-toml + - id: check-xml + - id: end-of-file-fixer + - id: forbid-submodules + - id: mixed-line-ending + - id: trailing-whitespace + # Python + - repo: https://github.com/astral-sh/ruff-pre-commit + rev: v0.11.5 + hooks: + - id: ruff-format + - id: ruff + args: [--fix] + # C++ formatting + - repo: https://github.com/pre-commit/mirrors-clang-format + rev: v19.1.7 + hooks: + - id: clang-format + args: ["--style=file:.config/clang-format"] + # C++ linting + - repo: https://github.com/cpplint/cpplint + rev: 2.0.0 + hooks: + - id: cpplint + args: ["--config=.cpplint.cfg", --quiet, --output=sed] + # Markdown + - repo: https://github.com/jackdewinter/pymarkdown + rev: v0.9.28 + hooks: + - id: pymarkdown + args: [-d, MD013, fix] + # XML + - repo: https://github.com/emersonknapp/ament_xmllint + rev: v0.1 + hooks: + - id: ament_xmllint + # YAML + - repo: https://github.com/adrienverge/yamllint.git + rev: v1.29.0 + hooks: + - id: yamllint + args: [-d, "{extends: default, rules: {line-length: {max: 256}, commas: false}}"] + # CMake + - repo: https://github.com/cmake-lint/cmake-lint + rev: 1.4.3 + hooks: + - id: cmakelint + args: [--linelength=140] + - repo: https://github.com/Lucas-C/pre-commit-hooks + rev: v1.5.5 + hooks: + - id: insert-license + types_or: [python, cmake, shell] + name: Copyright headers for Python/CMake + args: [ + --license-filepath, .config/copyright.txt, + --comment-style, '#', + --allow-past-years, + --no-extra-eol, + ] + - id: insert-license + types_or: [c++, c] + name: Copyright headers for C/C++ + args: [ + --license-filepath, .config/copyright.txt, + --comment-style, '//', + --allow-past-years, + ] diff --git a/.ruff.toml b/.ruff.toml new file mode 100644 index 0000000..c39e694 --- /dev/null +++ b/.ruff.toml @@ -0,0 +1,18 @@ +line-length = 120 +indent-width = 4 + +[format] +preview = true +quote-style = "single" +indent-style = "space" +skip-magic-trailing-comma = false +line-ending = "lf" + +[lint] +select = ["E4", "E7", "E9", "F", "I"] +# Rules intended for future application +# select = ["N", "D", "C90", "PTH", "UP", "PERF", "RUF"] +ignore = [] +fixable = ["ALL"] +unfixable = [] +dummy-variable-rgx = "^(_+|(_+[a-zA-Z0-9_]*[a-zA-Z0-9]+?))$" diff --git a/CHANGELOG.md b/CHANGELOG.md index 10d0237..8c79851 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,6 +1,6 @@ # Release Notes -## v0.1.0 +## v0.0.0 __INITIAL RELEASE__ ### Features diff --git a/CMakeLists.txt b/CMakeLists.txt index 4d20aa7..15448d7 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,128 +1,134 @@ +# 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(socketcan_adapter) +# 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() + +# Enable ASAN in Debug builds +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() + +# Distro detection (override with -BUILD_HUMBLE=ON/OFF) +if(NOT DEFINED BUILD_HUMBLE) + if(DEFINED ENV{ROS_DISTRO} AND "$ENV{ROS_DISTRO}" STREQUAL "humble") + set(BUILD_HUMBLE TRUE) + message(STATUS "ROS_DISTRO=humble -> using Catch2 v2") + else() + set(BUILD_HUMBLE FALSE) + message(STATUS "ROS_DISTRO>humble -> Catch2 v3 (Jazzy/Rolling)") + endif() endif() -# find dependencies +# Dependencies find_package(ament_cmake REQUIRED) -find_package(ament_cmake_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) -find_package(rclcpp_components REQUIRED) find_package(can_msgs REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -set(dependencies - rclcpp - rclcpp_lifecycle - rclcpp_components - can_msgs -) +# Primary library add_library(socketcan_adapter SHARED - src/socketcan_adapter.cpp - src/socketcan_bridge_node.cpp - src/can_frame.cpp) - -target_compile_features(socketcan_adapter PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + src/socketcan_adapter.cpp + src/socketcan_bridge_node.cpp + src/can_frame.cpp +) target_include_directories(socketcan_adapter PUBLIC $ - $) - -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. + $ +) target_compile_definitions(socketcan_adapter PRIVATE "SOCKETCAN_ADAPTER_BUILDING_LIBRARY") -ament_target_dependencies(socketcan_adapter ${dependencies}) +# Executable using the library +add_executable(socketcan_bridge src/socketcan_bridge.cpp) +target_include_directories(socketcan_bridge PUBLIC + $ + $ +) -install( - DIRECTORY include/ - DESTINATION include +target_link_libraries(socketcan_adapter PUBLIC + ${can_msgs_TARGETS} + rclcpp::rclcpp + rclcpp_lifecycle::rclcpp_lifecycle ) -install( - TARGETS socketcan_adapter - EXPORT export_${PROJECT_NAME} +target_link_libraries(socketcan_bridge PUBLIC socketcan_adapter) + +# Installs +install(DIRECTORY include/ DESTINATION include) +install(TARGETS socketcan_adapter + EXPORT ${PROJECT_NAME}_TARGETS ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin ) +install(TARGETS socketcan_bridge DESTINATION lib/${PROJECT_NAME}) +install(EXPORT ${PROJECT_NAME}_TARGETS + NAMESPACE ${PROJECT_NAME}:: + DESTINATION share/${PROJECT_NAME}/cmake +) -add_executable(socketcan_bridge src/socketcan_bridge.cpp) -target_include_directories(socketcan_bridge PUBLIC - $ - $) -target_link_libraries(socketcan_bridge socketcan_adapter) +# Tests +if(BUILD_TESTING) + include(CTest) + # Find correct Catch2 major version per distro + if(BUILD_HUMBLE) + find_package(Catch2 2 REQUIRED) + else() + find_package(Catch2 3 REQUIRED) + endif() -ament_target_dependencies(socketcan_bridge ${dependencies}) + # Try Catch's CMake helpers if present; otherwise we’ll fall back + include(Catch OPTIONAL) -install(TARGETS socketcan_bridge - DESTINATION lib/${PROJECT_NAME}) + add_executable(can_frame_tests test/can_frame_test.cpp) + target_link_libraries(can_frame_tests PRIVATE socketcan_adapter Catch2::Catch2WithMain) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_gtest REQUIRED) - find_package(ament_cmake_test REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - ament_lint_auto_find_test_dependencies() - - find_package(Catch2 2 REQUIRED) - set(test_dependencies - Catch2 - ) - - add_executable(can_frame_test test/can_frame_test.cpp) - target_link_libraries(can_frame_test Catch2::Catch2 socketcan_adapter) - ament_target_dependencies( - can_frame_test - ${test_dependencies} - ${dependencies} - ) - - ament_add_test( - can_frame_test - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND ${CMAKE_CURRENT_BINARY_DIR}/can_frame_test -r junit -o test_results/${PROJECT_NAME}/can_frame_test_output.xml - ENV CATCH_CONFIG_CONSOLE_WIDTH=120 - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - ) - - # Don't try to run vcan-based tests in CI just yet - if(NOT DEFINED ENV{CI}) - add_executable(socketcan_adapter_test test/socketcan_adapter_test.cpp) - target_link_libraries(socketcan_adapter_test Catch2::Catch2 socketcan_adapter) - ament_target_dependencies(socketcan_adapter_test - ${test_dependencies} - ${dependencies} - ) - ament_add_test( - socketcan_adapter_test - GENERATE_RESULT_FOR_RETURN_CODE_ZERO - COMMAND ${CMAKE_CURRENT_BINARY_DIR}/socketcan_adapter_test -r junit -o test_results/${PROJECT_NAME}/socketcan_adapter_test_output.xml - ENV CATCH_CONFIG_CONSOLE_WIDTH=120 - WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" - ) + if(COMMAND catch_discover_tests) + catch_discover_tests(can_frame_tests) + else() + add_test(NAME can_frame_tests COMMAND can_frame_tests) endif() + if(DEFINED ENV{CAN_AVAILABLE}) + add_executable(socketcan_adapter_hardware_tests test/socketcan_adapter_test.cpp) + target_link_libraries(socketcan_adapter_hardware_tests PRIVATE socketcan_adapter Catch2::Catch2WithMain) + if(COMMAND catch_discover_tests) + catch_discover_tests(socketcan_adapter_hardware_tests) + else() + add_test(NAME socketcan_adapter_hardware_tests COMMAND socketcan_adapter_hardware_tests) + endif() + message(STATUS "CAN_AVAILABLE set - including hardware tests") + else() + message(STATUS "CAN_AVAILABLE not set - skipping hardware tests (set CAN_AVAILABLE=1 to enable)") + endif() endif() -ament_export_include_directories( - include -) -ament_export_libraries( - socketcan_adapter -) -ament_export_targets( - export_${PROJECT_NAME} -) - +# Export +ament_export_targets(${PROJECT_NAME}_TARGETS HAS_LIBRARY_TARGET) ament_package() diff --git a/README.md b/README.md index 067d307..2ce6e4e 100644 --- a/README.md +++ b/README.md @@ -5,11 +5,13 @@ Socketcan Driver Library for Linux based PCs and ROS2 nodes Socketcan adapter can be built with the ros2 ament toolchain. All requirements can be installed via rosdep Install the dependencies! + ```bash rosdep install -i -y --from-paths socketcan_adapter ``` Build it! + ```bash colcon build --packages-up-to socketcan_adapter ``` @@ -27,7 +29,7 @@ Example highlights: Does not implement CanFD yet. -### SocketcanAdapter +### SocketcanAdapter `SocketcanAdapter` Class - The `SocketcanAdapter` abstracts and manages socket operations for CAN communication. It initializes and configures the socket, applies filters, and handles CAN frame transmission and reception. The adapter offers error handling, thread-safe operations, and optional callback functions for asynchronous frame and error processing. Key features: @@ -83,7 +85,7 @@ int main() { adapter.closeSocket(); return -1; } - + // Step 4: Prepare a CAN frame to send canid_t raw_id = 0x123; std::array data = {0x11, 0x22, 0x33, 0x44}; @@ -113,6 +115,7 @@ int main() { To make usage even easier, this package comes with a ROS2 node with default settings! ## Launch + ```bash ros2 launch socketcan_adapter socketcan_bridge_launch.py ``` @@ -123,4 +126,4 @@ launch args: - `can_filter_list`: can filters (default: []) - `join_filters`: use joining logic for filters (default: false) - `auto_configure`: automatically configure the lifecycle node -- `auto_activate`: automatically activate the lifecycle node post configuration +- `auto_activate`: automatically activate the lifecycle node post configuration diff --git a/include/socketcan_adapter/can_frame.hpp b/include/socketcan_adapter/can_frame.hpp index 91c4839..911fd85 100644 --- a/include/socketcan_adapter/can_frame.hpp +++ b/include/socketcan_adapter/can_frame.hpp @@ -1,15 +1,28 @@ +// 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 SOCKETCAN_ADAPTER__CAN_FRAME_HPP_ #define SOCKETCAN_ADAPTER__CAN_FRAME_HPP_ #include -#include + #include +#include #include #include -namespace polymath -{ -namespace socketcan +namespace polymath::socketcan { /// @brief Can Frame Types to help passthrough into the CAN Frame @@ -18,14 +31,14 @@ enum class FrameType : uint8_t DATA = 0U, ERROR, REMOTE -}; // enum class FrameType +}; // enum class FrameType /// @brief Can Frame ID Type, Standard or Extended enum class IdType : uint8_t { STANDARD = 0U, EXTENDED -}; // enum class IdType +}; // enum class IdType /// /// @class polymath::socketcan::CanFrame @@ -47,8 +60,10 @@ class CanFrame /// @param data std::array enforces C++ std::array for it's core API /// @param timestamp uint64_t utc timestamp for the can frame to store CanFrame( - const canid_t raw_id, const std::array & data, - const uint64_t & timestamp, uint8_t len = CAN_MAX_DLC); + const canid_t raw_id, + const std::array & data, + const uint64_t & timestamp, + uint8_t len = CAN_MAX_DLC); /// @brief Initialize CanFrame with a partial ID and data defined in std::array /// @param raw_id Can ID already generated including error and data information @@ -57,8 +72,11 @@ class CanFrame /// @param frame_type DATA, ERROR or REMOTE /// @param frame_id_type EXTENDED or Standard CanFrame( - const canid_t id, const std::array & data, - const uint64_t & timestamp, FrameType & frame_type, IdType & frame_id_type, + const canid_t id, + const std::array & data, + const uint64_t & timestamp, + FrameType & frame_type, + IdType & frame_id_type, uint8_t len = CAN_MAX_DLC); /// @brief ~CanFrame is a default destructor for now @@ -137,13 +155,10 @@ class CanFrame /// https://gitlab.com/polymathrobotics/polymath_core/-/issues/5 private: - struct can_frame frame_ - { - }; + struct can_frame frame_{}; uint64_t timestamp_{}; }; -} // socketcan -} // polymath +} // namespace polymath::socketcan -#endif // SOCKETCAN_ADAPTER__CAN_FRAME_HPP_ +#endif // SOCKETCAN_ADAPTER__CAN_FRAME_HPP_ diff --git a/include/socketcan_adapter/socketcan_adapter.hpp b/include/socketcan_adapter/socketcan_adapter.hpp index 41961c8..62ced76 100644 --- a/include/socketcan_adapter/socketcan_adapter.hpp +++ b/include/socketcan_adapter/socketcan_adapter.hpp @@ -1,20 +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 SOCKETCAN_ADAPTER__SOCKETCAN_ADAPTER_HPP_ #define SOCKETCAN_ADAPTER__SOCKETCAN_ADAPTER_HPP_ -#include -#include #include +#include + #include #include +#include +#include #include +#include #include -#include -#include "socketcan_adapter/visibility_control.h" +#include + #include "socketcan_adapter/can_frame.hpp" -namespace polymath -{ -namespace socketcan +namespace polymath::socketcan { /// @brief State of socket, error, open or closed @@ -69,8 +84,7 @@ class SocketcanAdapter : public std::enable_shared_from_this /// @param filters reference to a vector of can filters to set for the socket /// @return optional error string filled with an error message if any std::optional setFilters( - const filter_vector_t & filters, - FilterMode mode = FilterMode::OVERWRITE); + const filter_vector_t & filters, FilterMode mode = FilterMode::OVERWRITE); /// Shared ptr to a vector is technically more efficient than a vector of shared_ptrs /// TODO: Vectors are harder to justify in MISRA, so might want to use Array @@ -79,8 +93,7 @@ class SocketcanAdapter : public std::enable_shared_from_this /// @param filters INPUT shared ptr to a vector of can filters to set for the socket /// @return optional error string filled with an error message if any std::optional setFilters( - const std::shared_ptr filters, - FilterMode mode = FilterMode::OVERWRITE); + const std::shared_ptr filters, FilterMode mode = FilterMode::OVERWRITE); /// @brief Set the error mask /// @param error_mask INPUT error maskfor the socket to pass through @@ -116,20 +129,17 @@ class SocketcanAdapter : public std::enable_shared_from_this /// @brief Stop and join reception thread /// @param timeout_s INPUT timeout in seconds, <=0 means no timeout /// @return success on closed and joined thread - bool joinReceptionThread( - const std::chrono::duration & timeout_s = JOIN_RECEPTION_TIMEOUT_S); + bool joinReceptionThread(const std::chrono::duration & timeout_s = JOIN_RECEPTION_TIMEOUT_S); /// @brief Set receive callback function if thread is used /// @param callback_function INPUT To be called on receipt of a can frame /// @return success on receive callback set - bool setOnReceiveCallback( - std::function frame)> && callback_function); + bool setOnReceiveCallback(std::function frame)> && callback_function); /// @brief Set receive callback function if thread is used /// @param callback_function INPUT To be called on receipt of a can frame /// @return success on error callback set - bool setOnErrorCallback( - std::function && callback_function); + bool setOnErrorCallback(std::function && callback_function); /// @brief Transmit a can frame via socket /// @param frame INPUT const reference to the frame @@ -194,7 +204,6 @@ class SocketcanAdapter : public std::enable_shared_from_this bool join_; }; -} // namespace socketcan -} // namespace polymath +} // namespace polymath::socketcan -#endif // SOCKETCAN_ADAPTER__SOCKETCAN_ADAPTER_HPP_ +#endif // SOCKETCAN_ADAPTER__SOCKETCAN_ADAPTER_HPP_ diff --git a/include/socketcan_adapter/socketcan_bridge_node.hpp b/include/socketcan_adapter/socketcan_bridge_node.hpp index f14282e..f80e264 100644 --- a/include/socketcan_adapter/socketcan_bridge_node.hpp +++ b/include/socketcan_adapter/socketcan_bridge_node.hpp @@ -1,20 +1,34 @@ +// 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 SOCKETCAN_ADAPTER__SOCKETCAN_BRIDGE_NODE_HPP_ #define SOCKETCAN_ADAPTER__SOCKETCAN_BRIDGE_NODE_HPP_ -#include "socketcan_adapter/socketcan_adapter.hpp" +#include +#include +#include + +#include "can_msgs/msg/frame.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "can_msgs/msg/frame.hpp" - +#include "socketcan_adapter/socketcan_adapter.hpp" -namespace polymath -{ -namespace socketcan +namespace polymath::socketcan { class SocketcanBridgeNode : public rclcpp_lifecycle::LifecycleNode { - public: /// @brief Construct the socketcan bridge node /// @param options @@ -24,8 +38,7 @@ class SocketcanBridgeNode : public rclcpp_lifecycle::LifecycleNode ~SocketcanBridgeNode(); protected: - using rclcpp_lifecycle_callback_return = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; + using rclcpp_lifecycle_callback_return = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; /// @brief Configure the Node and socket /// @param state @@ -56,8 +69,7 @@ class SocketcanBridgeNode : public rclcpp_lifecycle::LifecycleNode /// @brief Convert vector of strings to Filter Vector for socketcan /// @param filter_vector Vector of strings to convert /// @return filter_vector_t, a vector of filters - SocketcanAdapter::filter_vector_t stringVectorToFilterVector( - const std::vector & filter_vector); + SocketcanAdapter::filter_vector_t stringVectorToFilterVector(const std::vector & filter_vector); /// @brief Publish CanFrame, used as a callback for the socketcam adapter /// @param frame CanFrame const @@ -81,7 +93,6 @@ class SocketcanBridgeNode : public rclcpp_lifecycle::LifecycleNode rclcpp::Subscription::SharedPtr frame_subscriber_{nullptr}; }; -} // namespace socketcan -} // namespace polymath +} // namespace polymath::socketcan -#endif // SOCKETCAN_ADAPTER__SOCKETCAN_BRIDGE_NODE_HPP_ +#endif // SOCKETCAN_ADAPTER__SOCKETCAN_BRIDGE_NODE_HPP_ diff --git a/include/socketcan_adapter/visibility_control.h b/include/socketcan_adapter/visibility_control.h deleted file mode 100644 index 61ede55..0000000 --- a/include/socketcan_adapter/visibility_control.h +++ /dev/null @@ -1,35 +0,0 @@ -#ifndef SOCKETCAN_ADAPTER__VISIBILITY_CONTROL_H_ -#define SOCKETCAN_ADAPTER__VISIBILITY_CONTROL_H_ - -// This logic was borrowed (then namespaced) from the examples on the gcc wiki: -// https://gcc.gnu.org/wiki/Visibility - -#if defined _WIN32 || defined __CYGWIN__ - #ifdef __GNUC__ - #define SOCKETCAN_ADAPTER_EXPORT __attribute__ ((dllexport)) - #define SOCKETCAN_ADAPTER_IMPORT __attribute__ ((dllimport)) - #else - #define SOCKETCAN_ADAPTER_EXPORT __declspec(dllexport) - #define SOCKETCAN_ADAPTER_IMPORT __declspec(dllimport) - #endif - #ifdef SOCKETCAN_ADAPTER_BUILDING_LIBRARY - #define SOCKETCAN_ADAPTER_PUBLIC SOCKETCAN_ADAPTER_EXPORT - #else - #define SOCKETCAN_ADAPTER_PUBLIC SOCKETCAN_ADAPTER_IMPORT - #endif - #define SOCKETCAN_ADAPTER_PUBLIC_TYPE SOCKETCAN_ADAPTER_PUBLIC - #define SOCKETCAN_ADAPTER_LOCAL -#else - #define SOCKETCAN_ADAPTER_EXPORT __attribute__ ((visibility("default"))) - #define SOCKETCAN_ADAPTER_IMPORT - #if __GNUC__ >= 4 - #define SOCKETCAN_ADAPTER_PUBLIC __attribute__ ((visibility("default"))) - #define SOCKETCAN_ADAPTER_LOCAL __attribute__ ((visibility("hidden"))) - #else - #define SOCKETCAN_ADAPTER_PUBLIC - #define SOCKETCAN_ADAPTER_LOCAL - #endif - #define SOCKETCAN_ADAPTER_PUBLIC_TYPE -#endif - -#endif // SOCKETCAN_ADAPTER__VISIBILITY_CONTROL_H_ diff --git a/launch/socketcan_bridge_launch.py b/launch/socketcan_bridge_launch.py index b59c7ec..8c72920 100644 --- a/launch/socketcan_bridge_launch.py +++ b/launch/socketcan_bridge_launch.py @@ -1,3 +1,16 @@ +# Copyright (c) 2025-present Polymath Robotics, Inc. All rights reserved +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. # Copyright 2021 the Autoware Foundation # # Licensed under the Apache License, Version 2.0 (the "License"); @@ -15,41 +28,39 @@ # Co-developed by Tier IV, Inc. and Apex.AI, Inc. +from launch_ros.actions import LifecycleNode +from launch_ros.event_handlers import OnStateTransition +from launch_ros.events.lifecycle import ChangeState +from lifecycle_msgs.msg import Transition + from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, EmitEvent, RegisterEventHandler from launch.conditions import IfCondition from launch.event_handlers import OnProcessStart from launch.events import matches_action from launch.substitutions import LaunchConfiguration -from launch_ros.actions import LifecycleNode -from launch_ros.event_handlers import OnStateTransition -from launch_ros.events.lifecycle import ChangeState -from lifecycle_msgs.msg import Transition def generate_launch_description(): - # Define args - can_interface_arg = DeclareLaunchArgument("can_interface", default_value="can0") - can_error_mask_arg = DeclareLaunchArgument( - "can_error_mask", default_value="0x1FFFFFFF" - ) - can_filter_list_arg = DeclareLaunchArgument("can_filter_list", default_value=[]) - join_filters_arg = DeclareLaunchArgument("join_filters", default_value="false") + can_interface_arg = DeclareLaunchArgument('can_interface', default_value='can0') + can_error_mask_arg = DeclareLaunchArgument('can_error_mask', default_value='0x1FFFFFFF') + can_filter_list_arg = DeclareLaunchArgument('can_filter_list', default_value=[]) + join_filters_arg = DeclareLaunchArgument('join_filters', default_value='false') socketcan_bridge_node = LifecycleNode( - package="socketcan_adapter", - executable="socketcan_bridge", - name="socketcan_bridge", + package='socketcan_adapter', + executable='socketcan_bridge', + name='socketcan_bridge', parameters=[ { - "can_interface": LaunchConfiguration("can_interface"), - "can_error_mask": LaunchConfiguration("can_error_mask"), - "can_filter_list": LaunchConfiguration("can_filter_list"), - "join_filters": LaunchConfiguration("join_filters"), + 'can_interface': LaunchConfiguration('can_interface'), + 'can_error_mask': LaunchConfiguration('can_error_mask'), + 'can_filter_list': LaunchConfiguration('can_filter_list'), + 'join_filters': LaunchConfiguration('join_filters'), } ], - output="screen", + output='screen', ) socketcan_bridge_configure_event_handler = RegisterEventHandler( @@ -64,14 +75,14 @@ def generate_launch_description(): ), ], ), - condition=IfCondition(LaunchConfiguration("auto_configure")), + condition=IfCondition(LaunchConfiguration('auto_configure')), ) socketcan_bridge_activate_event_handler = RegisterEventHandler( event_handler=OnStateTransition( target_lifecycle_node=socketcan_bridge_node, - start_state="configuring", - goal_state="inactive", + start_state='configuring', + goal_state='inactive', entities=[ EmitEvent( event=ChangeState( @@ -81,19 +92,17 @@ def generate_launch_description(): ), ], ), - condition=IfCondition(LaunchConfiguration("auto_activate")), + condition=IfCondition(LaunchConfiguration('auto_activate')), ) - return LaunchDescription( - [ - can_interface_arg, - can_error_mask_arg, - can_filter_list_arg, - join_filters_arg, - DeclareLaunchArgument("auto_configure", default_value="true"), - DeclareLaunchArgument("auto_activate", default_value="true"), - socketcan_bridge_node, - socketcan_bridge_configure_event_handler, - socketcan_bridge_activate_event_handler, - ] - ) + return LaunchDescription([ + can_interface_arg, + can_error_mask_arg, + can_filter_list_arg, + join_filters_arg, + DeclareLaunchArgument('auto_configure', default_value='true'), + DeclareLaunchArgument('auto_activate', default_value='true'), + socketcan_bridge_node, + socketcan_bridge_configure_event_handler, + socketcan_bridge_activate_event_handler, + ]) diff --git a/package.xml b/package.xml index fa463f0..e945324 100644 --- a/package.xml +++ b/package.xml @@ -4,21 +4,16 @@ socketcan_adapter 0.1.0 An Adapter Library for Socketcan with ROS2 - zeerek + Polymath Engineering Apache-2.0 - - ament_cmake_ros - - ament_lint_auto - ament_lint_common + Zeerek Ahmad rclcpp rclcpp_lifecycle - rclcpp_components can_msgs ament_cmake_test - ament_cmake_gtest + catch2 ament_cmake diff --git a/src/can_frame.cpp b/src/can_frame.cpp index 3a470f3..57e3c0a 100644 --- a/src/can_frame.cpp +++ b/src/can_frame.cpp @@ -1,25 +1,38 @@ +// 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 "socketcan_adapter/can_frame.hpp" -#include + #include -namespace polymath -{ -namespace socketcan +#include +#include +#include + +namespace polymath::socketcan { CanFrame::CanFrame() : frame_{} -{ -} +{} CanFrame::CanFrame(const struct can_frame & frame) : frame_(frame) -{ -} +{} CanFrame::CanFrame( - const canid_t raw_id, const std::array & data, - const uint64_t & timestamp, uint8_t len) + const canid_t raw_id, const std::array & data, const uint64_t & timestamp, uint8_t len) : timestamp_(timestamp) { set_can_id(raw_id); @@ -28,8 +41,12 @@ CanFrame::CanFrame( } CanFrame::CanFrame( - const canid_t id, const std::array & data, - const uint64_t & timestamp, FrameType & frame_type, IdType & frame_id_type, uint8_t len) + const canid_t id, + const std::array & data, + const uint64_t & timestamp, + FrameType & frame_type, + IdType & frame_id_type, + uint8_t len) : timestamp_(timestamp) { set_can_id(id); @@ -196,5 +213,4 @@ const std::array CanFrame::get_data() const return data; } -} // socketcan -} // polymath +} // namespace polymath::socketcan diff --git a/src/socketcan_adapter.cpp b/src/socketcan_adapter.cpp index 67116ab..cfffb7f 100644 --- a/src/socketcan_adapter.cpp +++ b/src/socketcan_adapter.cpp @@ -1,31 +1,46 @@ +// 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 "socketcan_adapter/socketcan_adapter.hpp" + #include #include #include #include +#include +#include +#include #include + #include #include -#include -#include -#include +#include #include +#include +#include -namespace polymath -{ -namespace socketcan +namespace polymath::socketcan { SocketcanAdapter::SocketcanAdapter( - const std::string & interface_name, - const std::chrono::duration & receive_timeout_s) -: interface_name_(interface_name), - receive_timeout_s_(receive_timeout_s), - receive_callback_unique_ptr_([](std::unique_ptr/*frame*/) { /*do nothing*/}), - receive_error_callback_([](socket_error_string_t /*error*/) { /*do nothing*/}), - socket_state_(SocketState::CLOSED) -{ -} + const std::string & interface_name, const std::chrono::duration & receive_timeout_s) +: interface_name_(interface_name) +, receive_timeout_s_(receive_timeout_s) +, receive_callback_unique_ptr_([](std::unique_ptr /*frame*/) { /*do nothing*/ }) +, receive_error_callback_([](socket_error_string_t /*error*/) { /*do nothing*/ }) +, socket_state_(SocketState::CLOSED) +{} SocketcanAdapter::~SocketcanAdapter() { @@ -34,9 +49,7 @@ SocketcanAdapter::~SocketcanAdapter() bool SocketcanAdapter::openSocket() { - struct sockaddr_can addr - { - }; + struct sockaddr_can addr{}; socket_file_descriptor_ = socket(PF_CAN, SOCK_RAW, CAN_RAW); if (socket_file_descriptor_ < 0) { @@ -98,20 +111,16 @@ std::optional SocketcanAdapter::setErro return sendErrorMask(); } -std::optional SocketcanAdapter::setJoinOverwrite( - const bool & join) +std::optional SocketcanAdapter::setJoinOverwrite(const bool & join) { join_ = join; return sendJoin(); } -std::optional SocketcanAdapter::receive( - CanFrame & polymath_can_frame) +std::optional SocketcanAdapter::receive(CanFrame & polymath_can_frame) { struct pollfd fds[1]; - struct can_frame frame - { - }; + struct can_frame frame{}; fds[0].fd = socket_file_descriptor_; fds[0].events = POLLIN | POLLERR; @@ -119,14 +128,12 @@ std::optional SocketcanAdapter::receive /// TODO: We don't need to call duration cast every time we run this, we should store milliseconds instead /// https://gitlab.com/polymathrobotics/polymath_core/-/issues/8 int return_value = poll( - fds, NUM_SOCKETS_IN_ADAPTER, - std::chrono::duration_cast(receive_timeout_s_).count()); + fds, NUM_SOCKETS_IN_ADAPTER, std::chrono::duration_cast(receive_timeout_s_).count()); socket_error_string_t error_string; if (return_value < 0) { - socket_error_string_t err_string = socket_error_string_t("poll failed with error") + strerror( - errno); + socket_error_string_t err_string = socket_error_string_t("poll failed with error") + strerror(errno); return std::optional(err_string); } @@ -140,10 +147,8 @@ std::optional SocketcanAdapter::receive if (getsockopt(socket_file_descriptor_, SOL_SOCKET, SO_ERROR, &error, &len) < 0) { error_string += socket_error_string_t("socket errored but failed to get the error\n"); } else { - error_string += socket_error_string_t("Socket error: ") + socket_error_string_t( - strerror( - error) + - socket_error_string_t("\n")); + error_string += + socket_error_string_t("Socket error: ") + socket_error_string_t(strerror(error) + socket_error_string_t("\n")); } } @@ -180,8 +185,7 @@ std::optional SocketcanAdapter::receive() return !result ? std::optional(can_frame) : std::nullopt; } -std::optional SocketcanAdapter::receive( - std::shared_ptr frame) +std::optional SocketcanAdapter::receive(std::shared_ptr frame) { CanFrame can_frame = CanFrame(); auto result = receive(can_frame); @@ -191,8 +195,7 @@ std::optional SocketcanAdapter::receive return result; } -std::optional SocketcanAdapter::send( - const CanFrame & frame) +std::optional SocketcanAdapter::send(const CanFrame & frame) { struct can_frame raw_frame = frame.get_frame(); @@ -200,15 +203,11 @@ std::optional SocketcanAdapter::send( /// TODO: Add Timeout verification in case we need it for multithreading as in ros2_socketcan /// https://gitlab.com/polymathrobotics/polymath_core/-/issues/8 - const auto bytes_sent = ::send( - socket_file_descriptor_, &raw_frame, sizeof(struct can_frame), - flags); - - return (bytes_sent < 0) ? - std::optional( - std::string("socket send failed: ") + - std::strerror(errno)) : - std::nullopt; + const auto bytes_sent = ::send(socket_file_descriptor_, &raw_frame, sizeof(struct can_frame), flags); + + return (bytes_sent < 0) + ? std::optional(std::string("socket send failed: ") + std::strerror(errno)) + : std::nullopt; } std::optional SocketcanAdapter::send( @@ -220,31 +219,26 @@ std::optional SocketcanAdapter::send( /// TODO: Add Timeout verification in case we need it for multithreading as in ros2_socketcan /// https://gitlab.com/polymathrobotics/polymath_core/-/issues/8 - const auto bytes_sent = ::send( - socket_file_descriptor_, &raw_frame, sizeof(struct can_frame), - flags); - - return (bytes_sent < 0) ? - std::optional( - std::string("socket send failed: ") + - std::strerror(errno)) : - std::nullopt; + const auto bytes_sent = ::send(socket_file_descriptor_, &raw_frame, sizeof(struct can_frame), flags); + + return (bytes_sent < 0) + ? std::optional(std::string("socket send failed: ") + std::strerror(errno)) + : std::nullopt; } std::optional SocketcanAdapter::sendFilters() { socket_error_string_t error_output(""); - if (0 != setsockopt( - socket_file_descriptor_, - SOL_CAN_RAW, - CAN_RAW_FILTER, - filter_list_.empty() ? NULL : filter_list_.data(), - sizeof(struct can_filter) * filter_list_.size())) + if ( + 0 != setsockopt( + socket_file_descriptor_, + SOL_CAN_RAW, + CAN_RAW_FILTER, + filter_list_.empty() ? NULL : filter_list_.data(), + sizeof(struct can_filter) * filter_list_.size())) { - error_output += socket_error_string_t("Failed to send CAN filters: ") + socket_error_string_t( - strerror( - errno)); + error_output += socket_error_string_t("Failed to send CAN filters: ") + socket_error_string_t(strerror(errno)); } return error_output.empty() ? std::nullopt : std::optional(error_output); @@ -254,16 +248,8 @@ std::optional SocketcanAdapter::sendErr { socket_error_string_t error_output(""); - if (0 != setsockopt( - socket_file_descriptor_, - SOL_CAN_RAW, - CAN_RAW_ERR_FILTER, - &error_mask_, - sizeof(can_err_mask_t))) - { - error_output += socket_error_string_t("Failed to send Error Mask: ") + socket_error_string_t( - strerror( - errno)); + if (0 != setsockopt(socket_file_descriptor_, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &error_mask_, sizeof(can_err_mask_t))) { + error_output += socket_error_string_t("Failed to send Error Mask: ") + socket_error_string_t(strerror(errno)); } return error_output.empty() ? std::nullopt : std::optional(error_output); @@ -273,16 +259,8 @@ std::optional SocketcanAdapter::sendJoi { socket_error_string_t error_output(""); auto join = static_cast(join_); - if (0 != setsockopt( - socket_file_descriptor_, - SOL_CAN_RAW, - CAN_RAW_JOIN_FILTERS, - &join, - sizeof(int32_t))) - { - error_output += socket_error_string_t("Failed to set Join Filter ") + socket_error_string_t( - strerror( - errno)); + if (0 != setsockopt(socket_file_descriptor_, SOL_CAN_RAW, CAN_RAW_JOIN_FILTERS, &join, sizeof(int32_t))) { + error_output += socket_error_string_t("Failed to set Join Filter ") + socket_error_string_t(strerror(errno)); } return error_output.empty() ? std::nullopt : std::optional(error_output); @@ -311,8 +289,7 @@ bool SocketcanAdapter::setOnReceiveCallback( return true; } -bool SocketcanAdapter::setOnErrorCallback( - std::function && callback_function) +bool SocketcanAdapter::setOnErrorCallback(std::function && callback_function) { receive_error_callback_ = std::move(callback_function); return true; @@ -327,22 +304,20 @@ bool SocketcanAdapter::startReceptionThread() stop_thread_requested_ = false; thread_running_ = true; - can_receive_thread_ = std::thread( - [this]() - { - while (!stop_thread_requested_) { - CanFrame frame = CanFrame(); - std::optional error = receive(frame); - - if (!error) { - receive_callback_unique_ptr_(std::make_unique(frame)); - } else { - receive_error_callback_(*error); - } + can_receive_thread_ = std::thread([this]() { + while (!stop_thread_requested_) { + CanFrame frame = CanFrame(); + std::optional error = receive(frame); + + if (!error) { + receive_callback_unique_ptr_(std::make_unique(frame)); + } else { + receive_error_callback_(*error); } + } - thread_running_ = false; - }); + thread_running_ = false; + }); return true; } @@ -369,9 +344,7 @@ bool SocketcanAdapter::joinReceptionThread(const std::chrono::duration & std::string SocketcanAdapter::get_interface() { - return interface_name_; } -} // namespace socketcan -} // namespace polymath +} // namespace polymath::socketcan diff --git a/src/socketcan_bridge.cpp b/src/socketcan_bridge.cpp index 0a0407e..2529f2f 100644 --- a/src/socketcan_bridge.cpp +++ b/src/socketcan_bridge.cpp @@ -1,16 +1,30 @@ +// 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 "socketcan_adapter/socketcan_bridge_node.hpp" + #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +#include "socketcan_adapter/socketcan_bridge_node.hpp" #include "std_msgs/msg/string.hpp" int main(int argc, char ** argv) { rclcpp::init(argc, argv); - rclcpp_lifecycle::LifecycleNode::SharedPtr node = - std::make_shared(); + rclcpp_lifecycle::LifecycleNode::SharedPtr node = std::make_shared(); rclcpp::executors::SingleThreadedExecutor executor; executor.add_node(node->get_node_base_interface()); diff --git a/src/socketcan_bridge_node.cpp b/src/socketcan_bridge_node.cpp index 9fa4a65..b4162f9 100644 --- a/src/socketcan_bridge_node.cpp +++ b/src/socketcan_bridge_node.cpp @@ -1,5 +1,26 @@ -#include +// 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 "socketcan_adapter/socketcan_bridge_node.hpp" + +#include +#include +#include +#include +#include +#include + namespace polymath { @@ -10,8 +31,8 @@ SocketcanBridgeNode::SocketcanBridgeNode(const rclcpp::NodeOptions & options) : rclcpp_lifecycle::LifecycleNode("socketcan_bridge", "", options) { declare_parameter("can_interface", std::string("can0")); - declare_parameter("can_error_mask", int32_t(CAN_ERR_MASK)); - declare_parameter("can_filter_list", {}); // vector of strings + declare_parameter("can_error_mask", static_cast(CAN_ERR_MASK)); + declare_parameter("can_filter_list", {}); // vector of strings declare_parameter("join_filters", false); declare_parameter("receive_timeout_s", SOCKET_RECEIVE_TIMEOUT_S.count()); } @@ -33,17 +54,12 @@ SocketcanBridgeNode::rclcpp_lifecycle_callback_return SocketcanBridgeNode::on_co auto filter_vector = stringVectorToFilterVector(filter_list); - socketcan_adapter_ = std::make_unique( - interface_name, - std::chrono::duration(receive_timeout_s)); + socketcan_adapter_ = + std::make_unique(interface_name, std::chrono::duration(receive_timeout_s)); frame_publisher_ = create_publisher("can_rx", 10); frame_subscriber_ = create_subscription( - "can_tx", 10, std::bind( - &SocketcanBridgeNode::transmitCanFrame, - this, - std::placeholders::_1 - )); + "can_tx", 10, std::bind(&SocketcanBridgeNode::transmitCanFrame, this, std::placeholders::_1)); bool success; success = socketcan_adapter_->openSocket(); @@ -80,18 +96,10 @@ SocketcanBridgeNode::rclcpp_lifecycle_callback_return SocketcanBridgeNode::on_co } socketcan_adapter_->setOnReceiveCallback( - std::bind( - &SocketcanBridgeNode::publishCanFrame, - this, - std::placeholders::_1 - )); + std::bind(&SocketcanBridgeNode::publishCanFrame, this, std::placeholders::_1)); socketcan_adapter_->setOnErrorCallback( - std::bind( - &SocketcanBridgeNode::socketErrorCallback, - this, - std::placeholders::_1 - )); + std::bind(&SocketcanBridgeNode::socketErrorCallback, this, std::placeholders::_1)); return LifecycleNode::on_configure(state); } @@ -152,7 +160,7 @@ SocketcanAdapter::filter_vector_t SocketcanBridgeNode::stringVectorToFilterVecto { SocketcanAdapter::filter_vector_t filter_list; - struct can_filter can_filter {}; + struct can_filter can_filter{}; for (std::string filter : filter_vector) { if (std::sscanf(filter.c_str(), "%x:%x", &can_filter.can_id, &can_filter.can_mask) == 2) { @@ -161,10 +169,7 @@ SocketcanAdapter::filter_vector_t SocketcanBridgeNode::stringVectorToFilterVecto can_filter.can_id |= CAN_EFF_FLAG; } filter_list.emplace_back(can_filter); - } else if (std::sscanf( - filter.c_str(), "%x~%x", &can_filter.can_id, - &can_filter.can_mask) == 2) - { + } else if (std::sscanf(filter.c_str(), "%x~%x", &can_filter.can_id, &can_filter.can_mask) == 2) { can_filter.can_id |= CAN_INV_FILTER; can_filter.can_mask &= ~CAN_ERR_FLAG; if (filter.size() > 8 && filter[8] == '~') { @@ -181,8 +186,7 @@ SocketcanAdapter::filter_vector_t SocketcanBridgeNode::stringVectorToFilterVecto void SocketcanBridgeNode::publishCanFrame(std::unique_ptr frame) { - std::unique_ptr publishable_frame = - std::make_unique(); + std::unique_ptr publishable_frame = std::make_unique(); publishable_frame->header.stamp = get_clock()->now(); publishable_frame->id = frame->get_id(); @@ -230,5 +234,5 @@ void SocketcanBridgeNode::transmitCanFrame(can_msgs::msg::Frame::UniquePtr frame socketcan_adapter_->send(can_frame); } -} // namespace socketcan -} // namspace polymath +} // namespace socketcan +} // namespace polymath diff --git a/test/can_frame_test.cpp b/test/can_frame_test.cpp index ef12e4c..311a9d1 100644 --- a/test/can_frame_test.cpp +++ b/test/can_frame_test.cpp @@ -1,55 +1,76 @@ -#define CATCH_CONFIG_MAIN // this tells Catch to provide a main() - // only do this when all tests in 1 cpp file +// 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 // v3 +#else + #include // v2 +#endif -#include #include "socketcan_adapter/socketcan_adapter.hpp" -using namespace polymath::socketcan; - -TEST_CASE("CanFrame default constructor", "[CanFrame]") { - CanFrame frame; +TEST_CASE("CanFrame default constructor", "[CanFrame]") +{ + polymath::socketcan::CanFrame frame; REQUIRE(frame.get_id() == 0); REQUIRE(frame.get_len() == 0); - REQUIRE(frame.get_frame_type() == FrameType::DATA); - REQUIRE(frame.get_id_type() == IdType::STANDARD); + REQUIRE(frame.get_frame_type() == polymath::socketcan::FrameType::DATA); + REQUIRE(frame.get_id_type() == polymath::socketcan::IdType::STANDARD); } -TEST_CASE("CanFrame constructor with can_frame", "[CanFrame]") { +TEST_CASE("CanFrame constructor with can_frame", "[CanFrame]") +{ struct can_frame canFrame = {}; canFrame.can_id = 0x123; canFrame.can_dlc = 8; std::fill(canFrame.data, canFrame.data + 8, 0xFF); - CanFrame frame(canFrame); + polymath::socketcan::CanFrame frame(canFrame); REQUIRE(frame.get_id() == 0x123); REQUIRE(frame.get_len() == 8); - REQUIRE( - frame.get_data() == - std::array{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}); + REQUIRE(frame.get_data() == std::array{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}); } -TEST_CASE("CanFrame constructor with raw_id, data, and timestamp", "[CanFrame]") { +TEST_CASE("CanFrame constructor with raw_id, data, and timestamp", "[CanFrame]") +{ canid_t raw_id = 0x456; std::array data = {1, 2, 3, 4, 5, 6, 7, 8}; - uint64_t timestamp = 123456789; + std::uint64_t timestamp = 123456789; - CanFrame frame(raw_id, data, timestamp); + polymath::socketcan::CanFrame frame(raw_id, data, timestamp); REQUIRE(frame.get_id() == raw_id); REQUIRE(frame.get_len() == 8); REQUIRE(frame.get_data() == data); } -TEST_CASE("CanFrame constructor with additional parameters", "[CanFrame]") { +TEST_CASE("CanFrame constructor with additional parameters", "[CanFrame]") +{ canid_t raw_id = 0x789; std::array data = {9, 8, 7, 6, 5, 4, 3, 2}; - uint64_t timestamp = 987654321; - FrameType frame_type = FrameType::REMOTE; - IdType frame_id_type = IdType::EXTENDED; + std::uint64_t timestamp = 987654321; + auto frame_type = polymath::socketcan::FrameType::REMOTE; + auto frame_id_type = polymath::socketcan::IdType::EXTENDED; - CanFrame frame(raw_id, data, timestamp, frame_type, frame_id_type); + polymath::socketcan::CanFrame frame(raw_id, data, timestamp, frame_type, frame_id_type); REQUIRE(frame.get_id() == raw_id); REQUIRE(frame.get_len() == 8); @@ -58,8 +79,9 @@ TEST_CASE("CanFrame constructor with additional parameters", "[CanFrame]") { REQUIRE(frame.get_id_type() == frame_id_type); } -TEST_CASE("Set and get frame", "[CanFrame]") { - CanFrame frame; +TEST_CASE("Set and get frame", "[CanFrame]") +{ + polymath::socketcan::CanFrame frame; struct can_frame canFrame = {}; canFrame.can_id = 0xABC; canFrame.can_dlc = 6; @@ -68,79 +90,81 @@ TEST_CASE("Set and get frame", "[CanFrame]") { REQUIRE(frame.set_frame(canFrame)); REQUIRE(frame.get_id() == (0xABC & CAN_SFF_MASK)); REQUIRE(frame.get_len() == 6); - REQUIRE( - frame.get_data() == - std::array{0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0x00, 0x00}); + REQUIRE(frame.get_data() == std::array{0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0x00, 0x00}); } -TEST_CASE("Set and get ID types", "[CanFrame]") { - CanFrame frame; +TEST_CASE("Set and get ID types", "[CanFrame]") +{ + polymath::socketcan::CanFrame frame; frame.set_id_as_extended(); - REQUIRE(frame.get_id_type() == IdType::EXTENDED); + REQUIRE(frame.get_id_type() == polymath::socketcan::IdType::EXTENDED); frame.set_id_as_standard(); - REQUIRE(frame.get_id_type() == IdType::STANDARD); + REQUIRE(frame.get_id_type() == polymath::socketcan::IdType::STANDARD); } -TEST_CASE("Set and get frame types", "[CanFrame]") { - CanFrame frame; +TEST_CASE("Set and get frame types", "[CanFrame]") +{ + polymath::socketcan::CanFrame frame; frame.set_type_error(); - REQUIRE(frame.get_frame_type() == FrameType::ERROR); + REQUIRE(frame.get_frame_type() == polymath::socketcan::FrameType::ERROR); frame.set_type_remote(); - REQUIRE(frame.get_frame_type() == FrameType::REMOTE); + REQUIRE(frame.get_frame_type() == polymath::socketcan::FrameType::REMOTE); frame.set_type_data(); - REQUIRE(frame.get_frame_type() == FrameType::DATA); + REQUIRE(frame.get_frame_type() == polymath::socketcan::FrameType::DATA); } -TEST_CASE("Set and get length", "[CanFrame]") { - CanFrame frame; +TEST_CASE("Set and get length", "[CanFrame]") +{ + polymath::socketcan::CanFrame frame; frame.set_len(5); REQUIRE(frame.get_len() == 5); } -TEST_CASE("Set and get data", "[CanFrame]") { - CanFrame frame; +TEST_CASE("Set and get data", "[CanFrame]") +{ + polymath::socketcan::CanFrame frame; std::array data = {1, 2, 3, 4, 5, 6, 7, 8}; frame.set_data(data); REQUIRE(frame.get_data() == data); } -TEST_CASE("Get masked ID", "[CanFrame]") { +TEST_CASE("Get masked ID", "[CanFrame]") +{ canid_t id = 0x1FFFFFFF; std::array data = {}; - CanFrame frame(id, data, 0); + polymath::socketcan::CanFrame frame(id, data, 0); REQUIRE(frame.get_masked_id(12, 0) == (id & 0xFFF)); REQUIRE(frame.get_masked_id(8, 8) == ((id >> 8) & 0xFF) << 8); } -TEST_CASE("Get frame", "[CanFrame]") { +TEST_CASE("Get frame", "[CanFrame]") +{ struct can_frame canFrame = {}; canFrame.can_id = 0x123; canFrame.can_dlc = 4; std::fill(canFrame.data, canFrame.data + 4, 0xAA); - CanFrame frame(canFrame); + polymath::socketcan::CanFrame frame(canFrame); struct can_frame retrievedFrame = frame.get_frame(); REQUIRE(retrievedFrame.can_id == canFrame.can_id); REQUIRE(retrievedFrame.can_dlc == canFrame.can_dlc); - REQUIRE( - std::equal( - std::begin(retrievedFrame.data), std::end(retrievedFrame.data), - std::begin(canFrame.data))); + REQUIRE(std::equal(std::begin(retrievedFrame.data), std::end(retrievedFrame.data), std::begin(canFrame.data))); } -TEST_CASE("Get error as string", "[CanFrame]") { - CanFrame frame; +TEST_CASE("Get error as string", "[CanFrame]") +{ + polymath::socketcan::CanFrame frame; frame.set_type_error(); std::string error = frame.get_error(); - REQUIRE(!error.empty()); // This test can be expanded based on the actual implementation of get_error() + REQUIRE(!error.empty()); } diff --git a/test/socketcan_adapter_test.cpp b/test/socketcan_adapter_test.cpp index 2d4ad7f..8b0fc86 100644 --- a/test/socketcan_adapter_test.cpp +++ b/test/socketcan_adapter_test.cpp @@ -1,500 +1,170 @@ -#define CATCH_CONFIG_MAIN -#include -#include -#include +// 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 "socketcan_adapter/socketcan_adapter.hpp" -using namespace polymath::socketcan; +#include +#include +#include +#include + +#if __has_include() + #include // v3 +#else + #include // v2 +#endif + +TEST_CASE("CanFrame default constructor", "[CanFrame]") +{ + polymath::socketcan::CanFrame frame; + + REQUIRE(frame.get_id() == 0); + REQUIRE(frame.get_len() == 0); + REQUIRE(frame.get_frame_type() == polymath::socketcan::FrameType::DATA); + REQUIRE(frame.get_id_type() == polymath::socketcan::IdType::STANDARD); +} + +TEST_CASE("CanFrame constructor with can_frame", "[CanFrame]") +{ + struct can_frame canFrame = {}; + canFrame.can_id = 0x123; + canFrame.can_dlc = 8; + std::fill(canFrame.data, canFrame.data + 8, 0xFF); + + polymath::socketcan::CanFrame frame(canFrame); -TEST_CASE("SocketcanAdapter tests", "[SocketcanAdapter]") + REQUIRE(frame.get_id() == 0x123); + REQUIRE(frame.get_len() == 8); + REQUIRE(frame.get_data() == std::array{0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF}); +} + +TEST_CASE("CanFrame constructor with raw_id, data, and timestamp", "[CanFrame]") { + canid_t raw_id = 0x456; + std::array data = {1, 2, 3, 4, 5, 6, 7, 8}; + std::uint64_t timestamp = 123456789; + + polymath::socketcan::CanFrame frame(raw_id, data, timestamp); + + REQUIRE(frame.get_id() == raw_id); + REQUIRE(frame.get_len() == 8); + REQUIRE(frame.get_data() == data); +} + +TEST_CASE("CanFrame constructor with additional parameters", "[CanFrame]") +{ + canid_t raw_id = 0x789; + std::array data = {9, 8, 7, 6, 5, 4, 3, 2}; + std::uint64_t timestamp = 987654321; + auto frame_type = polymath::socketcan::FrameType::REMOTE; + auto frame_id_type = polymath::socketcan::IdType::EXTENDED; + + polymath::socketcan::CanFrame frame(raw_id, data, timestamp, frame_type, frame_id_type); + + REQUIRE(frame.get_id() == raw_id); + REQUIRE(frame.get_len() == 8); + REQUIRE(frame.get_data() == data); + REQUIRE(frame.get_frame_type() == frame_type); + REQUIRE(frame.get_id_type() == frame_id_type); +} + +TEST_CASE("Set and get frame", "[CanFrame]") +{ + polymath::socketcan::CanFrame frame; + struct can_frame canFrame = {}; + canFrame.can_id = 0xABC; + canFrame.can_dlc = 6; + std::fill(canFrame.data, canFrame.data + 6, 0xEE); + + REQUIRE(frame.set_frame(canFrame)); + REQUIRE(frame.get_id() == (0xABC & CAN_SFF_MASK)); + REQUIRE(frame.get_len() == 6); + REQUIRE(frame.get_data() == std::array{0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0xEE, 0x00, 0x00}); +} + +TEST_CASE("Set and get ID types", "[CanFrame]") +{ + polymath::socketcan::CanFrame frame; + + frame.set_id_as_extended(); + REQUIRE(frame.get_id_type() == polymath::socketcan::IdType::EXTENDED); - SECTION("Constructor and destructor") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - REQUIRE(adapter.get_socket_state() == SocketState::CLOSED); - } - - SECTION("Open and close socket") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - REQUIRE(adapter.openSocket()); - REQUIRE(adapter.get_socket_state() == SocketState::OPEN); - REQUIRE(adapter.closeSocket()); - REQUIRE(adapter.get_socket_state() == SocketState::CLOSED); - } - - SECTION("Receive and send CanFrame") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - SocketcanAdapter receipt_adapter(interface_name); - REQUIRE(adapter.openSocket()); - REQUIRE(receipt_adapter.openSocket()); - - CanFrame frame; - frame.set_can_id(0x123); - frame.set_len(4); - std::array data = {0x01, 0x02, 0x03, 0x04}; - frame.set_data(data); - - auto send_result = adapter.send(frame); - REQUIRE(!send_result.has_value()); - - std::optional received_frame_opt = receipt_adapter.receive(); - REQUIRE(received_frame_opt.has_value()); - - CanFrame received_frame = *received_frame_opt; - REQUIRE(received_frame.get_id() == 0x123); - REQUIRE(received_frame.get_len() == 4); - REQUIRE(received_frame.get_data() == data); - - REQUIRE(adapter.closeSocket()); - REQUIRE(receipt_adapter.closeSocket()); - } - - SECTION("Reception thread") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - SocketcanAdapter send_adapter(interface_name); - REQUIRE(adapter.openSocket()); - REQUIRE(send_adapter.openSocket()); - - bool callback_called = false; - adapter.setOnReceiveCallback( - [&callback_called](std::unique_ptr/*frame*/) - { - callback_called = true; - }); - REQUIRE(adapter.startReceptionThread()); - - CanFrame frame; - frame.set_can_id(0x123); - frame.set_len(4); - std::array data = {0x01, 0x02, 0x03, 0x04}; - frame.set_data(data); - - auto send_result = send_adapter.send(frame); - REQUIRE(!send_result.has_value()); - - std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Allow some time for the reception thread - REQUIRE(callback_called); - - REQUIRE(adapter.joinReceptionThread()); - REQUIRE(adapter.closeSocket()); - REQUIRE(send_adapter.closeSocket()); - } - - SECTION("Error handling") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - adapter.openSocket(); - - int32_t num_error_callbacks_called = 0; - std::string error_message = ""; - adapter.setOnErrorCallback( - [&num_error_callbacks_called, &error_message](std::string error) - { - num_error_callbacks_called++; - error_message = error; - }); - - adapter.startReceptionThread(); - - REQUIRE(adapter.closeSocket()); - - // Allow for poll to fail - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - - adapter.joinReceptionThread(); - - REQUIRE(num_error_callbacks_called > 0); - REQUIRE(!error_message.empty()); - } - - SECTION("Set receive timeout") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - std::chrono::duration new_timeout = std::chrono::milliseconds(100); - adapter.set_receive_timeout(new_timeout); - REQUIRE(adapter.openSocket()); - // Additional tests to verify the timeout effect can be added - REQUIRE(adapter.closeSocket()); - } - - SECTION("Check thread running state") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - REQUIRE(!adapter.is_thread_running()); - REQUIRE(adapter.openSocket()); - - adapter.setOnReceiveCallback([](std::unique_ptr/*frame*/) { /* No-op */}); - REQUIRE(adapter.startReceptionThread()); - REQUIRE(adapter.is_thread_running()); - REQUIRE(adapter.joinReceptionThread()); - REQUIRE(!adapter.is_thread_running()); - REQUIRE(adapter.closeSocket()); - } + frame.set_id_as_standard(); + REQUIRE(frame.get_id_type() == polymath::socketcan::IdType::STANDARD); } -TEST_CASE("socketcan filters", "[SocketcanAdapter]") +TEST_CASE("Set and get frame types", "[CanFrame]") { - SECTION("Set filters") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - REQUIRE(adapter.openSocket()); - - SocketcanAdapter::filter_vector_t filters = { - // Example filter - {0x123, 0xFFF}, - }; - auto result = adapter.setFilters(filters); - REQUIRE(!result.has_value()); - - REQUIRE(adapter.closeSocket()); - } - - SECTION("Verify Specific Standard Filters SFF") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - SocketcanAdapter send_adapter(interface_name); - REQUIRE(adapter.openSocket()); - REQUIRE(send_adapter.openSocket()); - - // Example filter, let's 12X through - SocketcanAdapter::filter_vector_t filters = { - {0x123, 0xFFF}, - }; - - auto result = adapter.setFilters(filters); - REQUIRE(!result.has_value()); - - // Send frame that should pass through the filter - CanFrame frame; - frame.set_can_id(0x123); - frame.set_len(4); - std::array data = {0x01, 0x02, 0x03, 0x04}; - frame.set_data(data); - - std::optional err_str_optional; - CanFrame receive_frame; - - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - - REQUIRE(!err_str_optional.has_value()); - REQUIRE(receive_frame.get_id() == frame.get_id()); - - // Now test something that shouldn't come through - frame.set_can_id(0x125); - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - REQUIRE(err_str_optional.has_value()); - - REQUIRE(adapter.closeSocket()); - REQUIRE(send_adapter.closeSocket()); - } - - SECTION("Verify Specific Inverse Filters SFF") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - SocketcanAdapter send_adapter(interface_name); - REQUIRE(adapter.openSocket()); - REQUIRE(send_adapter.openSocket()); - - // Example filter, let's 12X through - SocketcanAdapter::filter_vector_t filters = { - {0x123 | CAN_INV_FILTER, 0xFFF}, - }; - - auto result = adapter.setFilters(filters); - REQUIRE(!result.has_value()); - - // Send frame that should pass through the filter - CanFrame frame; - frame.set_can_id(0x123); - frame.set_len(4); - std::array data = {0x01, 0x02, 0x03, 0x04}; - frame.set_data(data); - - std::optional err_str_optional; - CanFrame receive_frame; - - // 0x123 should be blacklisted - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - REQUIRE(err_str_optional.has_value()); - - // Now test something that should come through - frame.set_can_id(0x125); - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - REQUIRE(!err_str_optional.has_value()); - REQUIRE(receive_frame.get_id() == frame.get_id()); - - REQUIRE(adapter.closeSocket()); - REQUIRE(send_adapter.closeSocket()); - } - - SECTION("Verify Groups of Standard Filters SFF") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - SocketcanAdapter send_adapter(interface_name); - REQUIRE(adapter.openSocket()); - REQUIRE(send_adapter.openSocket()); - - // Example filter, let's 12X through - SocketcanAdapter::filter_vector_t filters = { - {0x123, 0xFF0}, - }; - - auto result = adapter.setFilters(filters); - REQUIRE(!result.has_value()); - - // Send frame that should pass through the filter - CanFrame frame; - frame.set_can_id(0x123); - frame.set_len(4); - std::array data = {0x01, 0x02, 0x03, 0x04}; - frame.set_data(data); - - std::optional err_str_optional; - CanFrame receive_frame; - - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - - REQUIRE(!err_str_optional.has_value()); - REQUIRE(receive_frame.get_id() == frame.get_id()); - - // Now test something that should ALSO come through - frame.set_can_id(0x125); - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - REQUIRE(!err_str_optional.has_value()); - REQUIRE(receive_frame.get_id() == frame.get_id()); - - // Now test something that shouldn't come through - frame.set_can_id(0x135); - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - REQUIRE(err_str_optional.has_value()); - - REQUIRE(adapter.closeSocket()); - REQUIRE(send_adapter.closeSocket()); - } - - SECTION("Verify Groups of Inverse Filters SFF") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - SocketcanAdapter send_adapter(interface_name); - REQUIRE(adapter.openSocket()); - REQUIRE(send_adapter.openSocket()); - - // Example filter, let's 12X through - SocketcanAdapter::filter_vector_t filters = { - {0x123 | CAN_INV_FILTER, 0xFF0}, - }; - - auto result = adapter.setFilters(filters); - REQUIRE(!result.has_value()); - - // Send frame that should pass through the filter - CanFrame frame; - frame.set_can_id(0x123); - frame.set_len(4); - std::array data = {0x01, 0x02, 0x03, 0x04}; - frame.set_data(data); - - std::optional err_str_optional; - CanFrame receive_frame; - - // 0x123 shouldn't come through - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - REQUIRE(err_str_optional.has_value()); - - // Now test something that should ALSO not come through - frame.set_can_id(0x125); - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - REQUIRE(err_str_optional.has_value()); - - // Now test something that should come through - frame.set_can_id(0x135); - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - REQUIRE(!err_str_optional.has_value()); - REQUIRE(receive_frame.get_id() == frame.get_id()); - - REQUIRE(adapter.closeSocket()); - REQUIRE(send_adapter.closeSocket()); - } - - SECTION("Allow 0x123 and 0x125 ONLY") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - SocketcanAdapter send_adapter(interface_name); - REQUIRE(adapter.openSocket()); - REQUIRE(send_adapter.openSocket()); - - // Example filter, let's 12X through - SocketcanAdapter::filter_vector_t filters = { - {0x123, CAN_EFF_MASK}, // allow 0x123 ONLY, including EFF - {0x125, CAN_EFF_MASK}, // allow 0x125 ONLY, including EFF - }; - - auto result = adapter.setFilters(filters); - REQUIRE(!result.has_value()); - - // Send frame that should pass through the filter - CanFrame frame; - frame.set_can_id(0x123); - frame.set_len(4); - std::array data = {0x01, 0x02, 0x03, 0x04}; - frame.set_data(data); - - std::optional err_str_optional; - CanFrame receive_frame; - - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - - REQUIRE(!err_str_optional.has_value()); - REQUIRE(receive_frame.get_id() == frame.get_id()); - - // Now test something that should ALSO come through - frame.set_can_id(0x125); - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - REQUIRE(!err_str_optional.has_value()); - REQUIRE(receive_frame.get_id() == frame.get_id()); - - // Now test something that shouldn't come through - frame.set_can_id(0x135); - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - REQUIRE(err_str_optional.has_value()); - - REQUIRE(adapter.closeSocket()); - REQUIRE(send_adapter.closeSocket()); - } - - SECTION("Allow 0x120 - 0x12F but not 0x125") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - SocketcanAdapter send_adapter(interface_name); - REQUIRE(adapter.openSocket()); - REQUIRE(send_adapter.openSocket()); - - // Example filter, let's 12X through - SocketcanAdapter::filter_vector_t filters = { - {0x120, 0xFF0}, // allow 0x123 ONLY - {0x125 | CAN_INV_FILTER, 0xFFF}, // dis allow 0x125 ONLY - }; - - auto result = adapter.setFilters(filters); - REQUIRE(!result.has_value()); - - result = adapter.setJoinOverwrite(true); - REQUIRE(!result.has_value()); - - // Send frame that should pass through the filter - CanFrame frame; - frame.set_can_id(0x123); - frame.set_len(4); - std::array data = {0x01, 0x02, 0x03, 0x04}; - frame.set_data(data); - - std::optional err_str_optional; - CanFrame receive_frame; - - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - - REQUIRE(!err_str_optional.has_value()); - REQUIRE(receive_frame.get_id() == frame.get_id()); - - // Now test something that should ALSO come through - frame.set_can_id(0x12F); - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - REQUIRE(!err_str_optional.has_value()); - REQUIRE(receive_frame.get_id() == frame.get_id()); - - // Also make sure 0x125 doesn't come through - frame.set_can_id(0x125); - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - REQUIRE(err_str_optional.has_value()); - - // Now test something that shouldn't come through - frame.set_can_id(0x135); - err_str_optional = send_adapter.send(frame); - REQUIRE(!err_str_optional.has_value()); - - err_str_optional = adapter.receive(receive_frame); - REQUIRE(err_str_optional.has_value()); - - REQUIRE(adapter.closeSocket()); - REQUIRE(send_adapter.closeSocket()); - } + polymath::socketcan::CanFrame frame; - SECTION("Set error mask") - { - std::string interface_name = "vcan0"; - SocketcanAdapter adapter(interface_name); - REQUIRE(adapter.openSocket()); + frame.set_type_error(); + REQUIRE(frame.get_frame_type() == polymath::socketcan::FrameType::ERROR); + + frame.set_type_remote(); + REQUIRE(frame.get_frame_type() == polymath::socketcan::FrameType::REMOTE); + + frame.set_type_data(); + REQUIRE(frame.get_frame_type() == polymath::socketcan::FrameType::DATA); +} + +TEST_CASE("Set and get length", "[CanFrame]") +{ + polymath::socketcan::CanFrame frame; + frame.set_len(5); + + REQUIRE(frame.get_len() == 5); +} + +TEST_CASE("Set and get data", "[CanFrame]") +{ + polymath::socketcan::CanFrame frame; + std::array data = {1, 2, 3, 4, 5, 6, 7, 8}; + + frame.set_data(data); + REQUIRE(frame.get_data() == data); +} + +TEST_CASE("Get masked ID", "[CanFrame]") +{ + canid_t id = 0x1FFFFFFF; + std::array data = {}; + polymath::socketcan::CanFrame frame(id, data, 0); + + REQUIRE(frame.get_masked_id(12, 0) == (id & 0xFFF)); + REQUIRE(frame.get_masked_id(8, 8) == ((id >> 8) & 0xFF) << 8); +} + +TEST_CASE("Get frame", "[CanFrame]") +{ + struct can_frame canFrame = {}; + canFrame.can_id = 0x123; + canFrame.can_dlc = 4; + std::fill(canFrame.data, canFrame.data + 4, 0xAA); + + polymath::socketcan::CanFrame frame(canFrame); + struct can_frame retrievedFrame = frame.get_frame(); + + REQUIRE(retrievedFrame.can_id == canFrame.can_id); + REQUIRE(retrievedFrame.can_dlc == canFrame.can_dlc); + REQUIRE(std::equal(std::begin(retrievedFrame.data), std::end(retrievedFrame.data), std::begin(canFrame.data))); +} + +TEST_CASE("Get error as string", "[CanFrame]") +{ + polymath::socketcan::CanFrame frame; + frame.set_type_error(); - can_err_mask_t error_mask = CAN_ERR_MASK; - auto result = adapter.setErrorMaskOverwrite(error_mask); - REQUIRE(!result.has_value()); - - REQUIRE(adapter.closeSocket()); - } + std::string error = frame.get_error(); + REQUIRE(!error.empty()); }