Initial import of FaRui Campus ADS v3.2

master/src/Driver/Vehicle/ros2_socketcan/.github/workflows/build_test.yml

@@ -0,0 +1,51 @@
+name: ROS2 CI
+
+on:
+  pull_request:
+    branches:
+      - 'develop'
+      - 'main'
+
+jobs:
+  test_environment:
+    runs-on: [ubuntu-latest]
+    strategy:
+      fail-fast: false
+      matrix:
+        ros_distribution:
+          - humble
+          - iron
+          - jazzy
+          - rolling
+        include:
+          # Humble Hawksbill (May 2022 - May 2027)
+          - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-humble-ros-base-latest
+            ros_distribution: humble
+            ros_version: 2
+          # Iron Irwini (May 2023 - November 2024)
+          - docker_image: rostooling/setup-ros-docker:ubuntu-jammy-ros-iron-ros-base-latest
+            ros_distribution: iron
+            ros_version: 2
+          # Jazzy Jalisco (May 2024 - May 2029)
+          - docker_image: rostooling/setup-ros-docker:ubuntu-noble-ros-jazzy-ros-base-latest
+            ros_distribution: jazzy
+            ros_version: 2
+          # Rolling Ridley  (June 2020 - Present)
+          - docker_image: rostooling/setup-ros-docker:ubuntu-noble-ros-rolling-ros-base-latest
+            ros_distribution: rolling
+            ros_version: 2
+    container:
+      image: ${{ matrix.docker_image }}
+    steps:
+      - name: setup directories
+        run: mkdir -p ros_ws/src
+      - name: checkout
+        uses: actions/checkout@v2
+        with:
+          path: ros_ws/src
+      - name: build and test
+        uses: ros-tooling/action-ros-ci@master
+        with:
+          package-name: ros2_socketcan ros2_socketcan_msgs
+          target-ros2-distro: ${{ matrix.ros_distribution }}
+          vcs-repo-file-url: ""

master/src/Driver/Vehicle/ros2_socketcan/.gitignore

@@ -0,0 +1,4 @@
+*.swp
+build/
+install/
+log/

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/CHANGELOG.rst

@@ -0,0 +1,76 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package ros2_socketcan
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.3.0 (2024-07-16)
+------------------
+* Jazzy release
+* fix: add missing header (`#42 <https://github.com/autowarefoundation/ros2_socketcan/issues/42>`_)
+* Allow remapping of the canbus topics (`#39 <https://github.com/autowarefoundation/ros2_socketcan/issues/39>`_)
+* Contributors: Joshua Whitley, Tim Clephas
+
+1.2.0 (2023-03-03)
+------------------
+* Add CAN FD Support (`#28 <https://github.com/autowarefoundation/ros2_socketcan/issues/28>`_)
+  * Add FD support to ROS2 interface.
+  * Add FD send/receive.
+  * Missed some fields in Frame msg.
+  * Make standard and FD mutually exclusive.
+  * Enable runtime checks for standard vs FD.
+  * Try to minimize API changes.
+  * Fix receive_id/fd_receive_id mix-up.
+  * Make new message FD-specific.
+  * Use FdFrame message.
+  * Remove unused functions.
+  * Always resize fd frame buffer to 64 before receive.
+  * Add enable_can_fd to socket_can_bridge.launch.xml.
+  ---------
+* SocketCAN filters (`#25 <https://github.com/autowarefoundation/ros2_socketcan/issues/25>`_)
+  * SocketCAN filters
+  Filters can be set using launch parameter.
+  A list of pairs (can id and mask) are fetched and applied to
+  socket filter.
+  Added unit test for filters application and fuctionality.
+  * Full support of SocketCAN filters
+  SocketCAN filters can be now
+  with string description used
+  by candump utility.
+  Added support for error masks
+  and joined CAN filters.
+  Instead of list of integers, receiver
+  node now uses string parameter
+  in order to receive filters. Filters will
+  now be parsed and setup during
+  configuration.
+  Added unit test for parsing and
+  updated filters unit test.
+  * Reference to man-pages docs of filters syntax
+  Added links referencing man-pages docs for candump,
+  containing more information about socketcan filters
+  syntax used. Links were added to doxygen documentation
+  of filters parsing method and to launch argument
+  description.
+* Fix unit conversion bug in to_timeval() (`#24 <https://github.com/autowarefoundation/ros2_socketcan/issues/24>`_)
+* Reorganize folders for adding ros2_socketcan_msgs (`#23 <https://github.com/autowarefoundation/ros2_socketcan/issues/23>`_)
+  Reorganize folders to permit adding a msgs package.
+* Contributors: Joshua Whitley, Marcel Dudek, ljuricic
+
+1.1.0 (2022-02-03)
+------------------
+* Added bus time (`#12 <https://github.com/autowarefoundation/ros2_socketcan/issues/12>`_)
+  * added the ability to get the bus time for the can packet, versus using ros time when received; packs bus time as part of the can id struct
+  * cleanup; cast fix
+  * chore: apply uncrustify
+  * chore: fix include order for cpplint
+  Co-authored-by: wep21 <border_goldenmarket@yahoo.co.jp>
+* Merge pull request `#10 <https://github.com/autowarefoundation/ros2_socketcan/issues/10>`_ from wep21/ci-galactic
+  Add galactic into action
+* Add galactic into action
+* Contributors: Andrew Saba, Daisuke Nishimatsu, Joshua Whitley
+
+1.0.0 (2021-04-01)
+------------------
+* Initial release
+* Initial port from Autoware.Auto
+* Initial commit
+* Contributors: Joshua Whitley, Kenji Miyake, wep21

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/CMakeLists.txt

@@ -0,0 +1,64 @@
+cmake_minimum_required(VERSION 3.5)
+project(ros2_socketcan)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+ament_auto_add_library(${PROJECT_NAME} SHARED
+  src/socket_can_common.cpp
+  src/socket_can_id.cpp
+  src/socket_can_receiver.cpp
+  src/socket_can_sender.cpp)
+
+ament_auto_add_library(socket_can_receiver_node SHARED
+  src/socket_can_receiver_node.cpp
+)
+
+target_link_libraries(socket_can_receiver_node
+  ${PROJECT_NAME}
+)
+
+rclcpp_components_register_node(socket_can_receiver_node
+  PLUGIN "drivers::socketcan::SocketCanReceiverNode"
+  EXECUTABLE socket_can_receiver_node_exe
+)
+
+ament_auto_add_library(socket_can_sender_node SHARED
+  src/socket_can_sender_node.cpp
+)
+
+target_link_libraries(socket_can_sender_node
+  ${PROJECT_NAME}
+)
+
+rclcpp_components_register_node(socket_can_sender_node
+  PLUGIN "drivers::socketcan::SocketCanSenderNode"
+  EXECUTABLE socket_can_sender_node_exe
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  # TODO(c.ho) Make this into a pytest
+  ament_add_gtest(${PROJECT_NAME}_test
+    test/gtest_main.cpp
+    test/receiver.cpp
+    test/sanity_checks.cpp)
+  target_include_directories(${PROJECT_NAME}_test PUBLIC include)
+  target_link_libraries(${PROJECT_NAME}_test ${PROJECT_NAME})
+endif()
+
+ament_auto_package(INSTALL_TO_SHARE
+  launch
+)

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/CONTRIBUTING.md

@@ -0,0 +1,13 @@
+Any contribution that you make to this repository will
+be under the Apache 2 License, as dictated by that
+[license](http://www.apache.org/licenses/LICENSE-2.0.html):
+
+~~~
+5. Submission of Contributions. Unless You explicitly state otherwise,
+   any Contribution intentionally submitted for inclusion in the Work
+   by You to the Licensor shall be under the terms and conditions of
+   this License, without any additional terms or conditions.
+   Notwithstanding the above, nothing herein shall supersede or modify
+   the terms of any separate license agreement you may have executed
+   with Licensor regarding such Contributions.
+~~~

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/LICENSE

@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   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.

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/design/README.md

@@ -0,0 +1,103 @@
+socket_can {#socket_can}
+===============
+
+# Purpose / Use cases
+<!-- Required -->
+<!-- Things to consider:
+    - Why did we implement this feature? -->
+
+CAN is the de-facto standard for communication between components on a vehicle.
+
+As such, to send commands to the vehicle, a mechanism to send messages via CAN is required.
+
+Similarly a mechanism to receive messages via CAN is required to receive data from the vehicle
+platform.
+
+# Design
+<!-- Required -->
+<!-- Things to consider:
+    - How does it work? -->
+
+These classes are a thin wrapper around C functions to manage some extra book-keeping.
+
+A typed interface for sending is also provided for compile-time checking of data sizes.
+
+A helper class following the named parameter idiom is provided to wrap the CAN ID.
+
+Sending and receiving are separate concerns and thus contained in separate classes.
+
+Finally, care is taken to avoid exposing C/POSIX headers.
+
+## Assumptions / Known limits
+<!-- Required -->
+
+The concern for sender is only simple book-keeping and sending of data.
+
+The same is true for the receiver.
+
+Any complex error handling which would require receiving data is outside the concern of this class,
+and should be a part of a higher level class which contains an instance of this class.
+
+# Inputs / Outputs / API
+<!-- Required -->
+<!-- Things to consider:
+    - How do you use the package / API? -->
+
+See the [sender API docs](@ref drivers::socketcan::SocketCanSender).
+and the [receiver API docs](@ref drivers::socketcan::SocketCanReceiver).
+
+# Inner-workings / Algorithms
+<!-- If applicable -->
+
+These classes have no substantive logic.
+
+Unix's select() function was used to wait for resource availability. On any error, an exception is
+thrown.
+
+# Error detection and handling
+<!-- Required -->
+
+Both the receiver and the sender classes throw exceptions in the following cases:
+1. On construction if the specified interface is invalid or cannot be bound
+2. If the file descriptor is unavailable within the timeout period for sending
+3. Any other Unix error is raised during the sending process
+
+Message-level error checking mechanisms a part of the CAN standard are outside the scope of this
+class.
+
+# Security considerations
+<!-- Required -->
+<!-- Things to consider:
+- Spoofing (How do you check for and handle fake input?)
+- Tampering (How do you check for and handle tampered input?)
+- Repudiation (How are you affected by the actions of external actors?).
+- Information Disclosure (Can data leak?).
+- Denial of Service (How do you handle spamming?).
+- Elevation of Privilege (Do you need to change permission levels during execution?) -->
+
+This component exposes any security concerns that CAN might have.
+
+# References / External links
+<!-- Optional -->
+
+API inspirations:
+1. [python-can](https://python-can.readthedocs.io/en/master/bus.html)
+2. [qtcanbus](https://doc.qt.io/qt-5.9/qcanbusdevice.html#writeFrame)
+3. [rust socketcan](https://docs.rs/socketcan/1.7.0/socketcan/struct.CANSocket.html)
+
+Implementation-specific references:
+1. [SocketCAN reference](https://www.kernel.org/doc/Documentation/networking/can.txt)
+2. [socket](http://man7.org/linux/man-pages/man2/socket.2.html)
+3. [bind](http://man7.org/linux/man-pages/man2/bind.2.html)
+4. [send](http://man7.org/linux/man-pages/man2/send.2.html)
+5. [ioctl](http://man7.org/linux/man-pages/man2/ioctl.2.html)
+6. [close](http://man7.org/linux/man-pages/man2/close.2.html)
+
+CAN-related references:
+1. [KVaser CAN Protocol Tour](https://www.kvaser.com/can-protocol-tutorial/)
+2. [Kvaser Higher Level Protocols](https://www.kvaser.com/about-can/higher-layer-protocols/)
+
+# Future extensions / Unimplemented parts
+<!-- Optional -->
+
+- AutoSAR/PCLint fixes around FDSET and select()

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/include/ros2_socketcan/socket_can_common.hpp

@@ -0,0 +1,69 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#ifndef ROS2_SOCKETCAN__SOCKET_CAN_COMMON_HPP_
+#define ROS2_SOCKETCAN__SOCKET_CAN_COMMON_HPP_
+
+#include <sys/select.h>
+#include <sys/time.h>
+#include <linux/can.h>
+
+#include <chrono>
+#include <cstdint>
+#include <string>
+#include <vector>
+
+namespace drivers
+{
+namespace socketcan
+{
+
+/// Bind a non-blocking CAN_RAW socket to the given interface
+/// \param[in] interface The name of the interface to bind, must be smaller than IFNAMSIZ
+/// \param[in] enable_fd Whether this socket uses CAN FD or not
+/// \return The file descriptor bound to the given interface
+/// \throw std::runtime_error If one of socket(), fnctl(), ioctl(), bind() failed
+/// \throw std::domain_error If the provided interface name is too long
+int32_t bind_can_socket(const std::string & interface, bool enable_fd);
+
+/// Set SocketCAN filters
+/// \param[in] fd File descriptor of the socket
+/// \param[in] f_list List of filters to be applied.
+/// \throw std::runtime_error If filters couldn't be applied
+void set_can_filter(int32_t fd, const std::vector<struct can_filter> & f_list);
+
+/// Set SocketCAN error filter
+/// \param[in] fd File descriptor of the socket
+/// \param[in] err_mask Error mask to be applied as a filter
+void set_can_err_filter(int32_t fd, can_err_mask_t err_mask);
+
+/// Set filters joining option for SocketCAN. If set, all filters
+/// must match for the frame to be passed.
+/// \param[in] fd File descriptor of the socket
+/// \param[in] join_filters Should the filters be joined?
+void set_can_filter_join(int32_t fd, bool join_filters);
+
+/// Convert std::chrono duration to timeval (with microsecond resolution)
+struct timeval to_timeval(const std::chrono::nanoseconds timeout) noexcept;
+/// Convert timeval to time in microseconds
+uint64_t from_timeval(const struct timeval tv) noexcept;
+/// Create a fd_set for use with select() that only contains the specified file descriptor
+fd_set single_set(int32_t file_descriptor) noexcept;
+
+}  // namespace socketcan
+}  // namespace drivers
+
+#endif  // ROS2_SOCKETCAN__SOCKET_CAN_COMMON_HPP_

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/include/ros2_socketcan/socket_can_id.hpp

@@ -0,0 +1,116 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+/// \file
+/// \brief This file defines a class a socket sender
+#ifndef ROS2_SOCKETCAN__SOCKET_CAN_ID_HPP_
+#define ROS2_SOCKETCAN__SOCKET_CAN_ID_HPP_
+
+#include <cstdint>
+#include <stdexcept>
+
+#include "ros2_socketcan/visibility_control.hpp"
+
+namespace drivers
+{
+namespace socketcan
+{
+
+constexpr std::size_t MAX_DATA_LENGTH = 8U;
+constexpr std::size_t MAX_FD_DATA_LENGTH = 64U;
+/// Special error for timeout
+class SOCKETCAN_PUBLIC SocketCanTimeout : public std::runtime_error
+{
+public:
+  explicit SocketCanTimeout(const char * const what)
+  : runtime_error{what} {}
+};  // class SocketCanTimeout
+
+enum class FrameType : uint32_t
+{
+  DATA,
+  ERROR,
+  REMOTE
+  // SocketCan doesn't support Overload frame directly?
+};  // enum class FrameType
+
+/// Tag for standard frame
+struct StandardFrame_ {};
+//lint -e{1502} NOLINT It's a tag
+constexpr StandardFrame_ StandardFrame;
+/// Tag for extended frame
+struct ExtendedFrame_ {};
+//lint -e{1502} NOLINT It's a tag
+constexpr ExtendedFrame_ ExtendedFrame;
+
+/// A wrapper around can_id_t to make it a little more C++-y
+/// WARNING: I'm assuming the 0th bit is the MSB aka the leftmost bit
+class SOCKETCAN_PUBLIC CanId
+{
+public:
+  using IdT = uint32_t;
+  using LengthT = uint32_t;
+  // Default constructor: standard data frame with id 0
+  CanId() = default;
+  /// Directly set id, blindly taking whatever bytes are given
+  explicit CanId(const IdT raw_id, const uint64_t bus_time, const LengthT data_length = 0U);
+  /// Sets ID
+  /// \throw std::domain_error if id would get truncated
+  CanId(const IdT id, const uint64_t bus_time, FrameType type, StandardFrame_);
+  /// Sets ID
+  /// \throw std::domain_error if id would get truncated
+  CanId(const IdT id, const uint64_t bus_time, FrameType type, ExtendedFrame_);
+
+  /// Sets bit 31 to 0
+  CanId & standard() noexcept;
+  /// Sets bit 31 to 1
+  CanId & extended() noexcept;
+  /// Sets bit 29 to 1, and bit 30 to 0
+  CanId & error_frame() noexcept;
+  /// Sets bit 29 to 0, and bit 30 to 1
+  CanId & remote_frame() noexcept;
+  /// Clears bits 29 and 30 (sets to 0)
+  CanId & data_frame() noexcept;
+  /// Sets the type accordingly
+  CanId & frame_type(const FrameType type);
+  /// Sets leading bits
+  /// \throw std::domain_error If id would get truncated, 11 bits for Standard, 29 bits for Extended
+  CanId & identifier(const IdT id);
+
+  /// Get just the can_id bits
+  IdT identifier() const noexcept;
+  /// Get the whole id value
+  IdT get() const noexcept;
+  /// Check if frame is extended
+  bool is_extended() const noexcept;
+  /// Check frame type
+  /// \throw std::domain_error If bits are in an inconsistent state
+  FrameType frame_type() const;
+  /// Get the length of the data; only nonzero on received data
+  LengthT length() const noexcept;
+
+  uint64_t get_bus_time() {return bus_time;}
+
+private:
+  SOCKETCAN_LOCAL CanId(const IdT id, const uint64_t bus_time, FrameType type, bool is_extended);
+
+  IdT m_id{};
+  LengthT m_data_length{};
+  uint64_t bus_time;
+};  // class CanId
+}  // namespace socketcan
+}  // namespace drivers
+
+#endif  // ROS2_SOCKETCAN__SOCKET_CAN_ID_HPP_

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/include/ros2_socketcan/socket_can_receiver.hpp

@@ -0,0 +1,171 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+/// \file
+/// \brief This file defines a class a socket sender
+#ifndef ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_HPP_
+#define ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_HPP_
+
+#include <linux/can.h>
+#include <array>
+#include <chrono>
+#include <cstdint>
+#include <cstring>
+#include <string>
+#include <vector>
+
+#include "ros2_socketcan/visibility_control.hpp"
+#include "ros2_socketcan/socket_can_id.hpp"
+
+namespace drivers
+{
+namespace socketcan
+{
+
+/// Simple RAII wrapper around a raw CAN receiver
+class SOCKETCAN_PUBLIC SocketCanReceiver
+{
+public:
+  /// Constructor
+  explicit SocketCanReceiver(const std::string & interface = "can0", const bool enable_fd = false);
+  /// Destructor
+  ~SocketCanReceiver() noexcept;
+
+  /// Structure containing possible CAN filter options.
+  struct CanFilterList
+  {
+    std::vector<struct can_filter> filters;
+    can_err_mask_t error_mask = 0;
+    bool join_filters = false;
+
+    /// Default constructor
+    CanFilterList() = default;
+
+
+    /// \copydoc ParseFilters(const std::string & str)
+    explicit CanFilterList(const char * str);
+
+
+    /// \copydoc ParseFilters(const std::string & str)
+    explicit CanFilterList(const std::string & str);
+
+    /// Parse CAN filters string:\n
+    /// Filters:\n
+    /// Comma separated filters can be specified for each given CAN interface.\n
+    /// <can_id>:<can_mask>\n
+    ///         (matches when <received_can_id> & mask == can_id & mask)\n
+    /// <can_id>~<can_mask>\n
+    ///         (matches when <received_can_id> & mask != can_id & mask)\n
+    /// #<error_mask>\n
+    ///         (set error frame filter, see include/linux/can/error.h)\n
+    /// [j|J]\n
+    ///         (join the given CAN filters - logical AND semantic)\n
+    ///
+    /// CAN IDs, masks and data content are given and expected in hexadecimal values.
+    /// When can_id and can_mask are both 8 digits, they are assumed to be 29 bit EFF.
+    /// \see https://manpages.ubuntu.com/manpages/jammy/man1/candump.1.html
+    /// \param[in] str Input to be parsed.
+    /// \return Populated CanFilterList structure.
+    /// \throw std::runtime_error if string couldn't be parsed.
+    static CanFilterList ParseFilters(const std::string & str);
+  };
+
+  /// Set SocketCAN filters
+  /// \param[in] filters List of filters to be applied.
+  /// \throw std::runtime_error If filters couldn't be applied
+  void SetCanFilters(const CanFilterList & filters);
+
+  /// Receive CAN data
+  /// \param[out] data A buffer to be written with data bytes. Must be at least 8 bytes in size
+  /// \param[in] timeout Maximum duration to wait for data on the file descriptor. Negative
+  ///                    durations are treated the same as zero timeout
+  /// \return The CanId for the received can_frame, with length appropriately populated
+  /// \throw SocketCanTimeout On timeout
+  /// \throw std::runtime_error on other errors
+  CanId receive(
+    void * const data,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
+  /// Receive typed CAN data. Slightly less efficient than untyped interface; has extra copy and
+  /// branches
+  /// \tparam Type of data to receive, must be 8 bytes or smaller
+  /// \param[out] data A buffer to be written with data bytes. Must be at least 8 bytes in size
+  /// \param[in] timeout Maximum duration to wait for data on the file descriptor. Negative
+  ///                    durations are treated the same as zero timeout
+  /// \return The CanId for the received can_frame, with length appropriately populated
+  /// \throw SocketCanTimeout On timeout
+  /// \throw std::runtime_error If received data would not fit into provided type
+  /// \throw std::runtime_error on other errors
+  template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
+  CanId receive(
+    T & data,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
+  {
+    static_assert(sizeof(data) <= MAX_DATA_LENGTH, "Data type too large for CAN");
+    std::array<uint8_t, MAX_DATA_LENGTH> data_raw{};
+    const auto ret = receive(&data_raw[0U], timeout);
+    if (ret.length() != sizeof(data)) {
+      throw std::runtime_error{"Received CAN data is of size incompatible with provided type!"};
+    }
+    (void)std::memcpy(&data, &data_raw[0U], ret.length());
+    return ret;
+  }
+
+  /// Receive CAN FD data
+  /// \param[out] data A buffer to be written with data bytes. Must be at least 64 bytes in size
+  /// \param[in] timeout Maximum duration to wait for data on the file descriptor. Negative
+  ///                    durations are treated the same as zero timeout
+  /// \return The CanId for the received canfd_frame, with length appropriately populated
+  /// \throw SocketCanTimeout On timeout
+  /// \throw std::runtime_error on other errors
+  CanId receive_fd(
+    void * const data,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
+  /// Receive typed CAN FD data. Slightly less efficient than untyped interface; has extra copy and
+  /// branches
+  /// \tparam Type of data to receive, must be 64 bytes or smaller
+  /// \param[out] data A buffer to be written with data bytes. Must be at least 64 bytes in size
+  /// \param[in] timeout Maximum duration to wait for data on the file descriptor. Negative
+  ///                    durations are treated the same as zero timeout
+  /// \return The CanId for the received canfd_frame, with length appropriately populated
+  /// \throw SocketCanTimeout On timeout
+  /// \throw std::runtime_error If received data would not fit into provided type
+  /// \throw std::runtime_error on other errors
+  template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
+  CanId receive_fd(
+    T & data,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
+  {
+    static_assert(sizeof(data) <= MAX_FD_DATA_LENGTH, "Data type too large for CAN FD");
+    std::array<uint8_t, MAX_FD_DATA_LENGTH> data_raw{};
+    const auto ret = receive_fd(&data_raw[0U], timeout);
+    if (ret.length() != sizeof(data)) {
+      throw std::runtime_error{"Received CAN FD data is of size incompatible with provided type!"};
+    }
+    (void)std::memcpy(&data, &data_raw[0U], ret.length());
+    return ret;
+  }
+
+private:
+  // Wait for file descriptor to be available to send data via select()
+  SOCKETCAN_LOCAL void wait(const std::chrono::nanoseconds timeout) const;
+
+  int32_t m_file_descriptor;
+  bool m_enable_fd;
+};  // class SocketCanReceiver
+
+}  // namespace socketcan
+}  // namespace drivers
+
+#endif  // ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_HPP_

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/include/ros2_socketcan/socket_can_receiver_node.hpp

@@ -0,0 +1,88 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#ifndef ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_NODE_HPP_
+#define ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_NODE_HPP_
+
+#include <memory>
+#include <thread>
+#include <string>
+#include <vector>
+
+#include "ros2_socketcan/visibility_control.hpp"
+#include "ros2_socketcan/socket_can_receiver.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rosidl_runtime_cpp/message_initialization.hpp"
+#include "can_msgs/msg/frame.hpp"
+#include "ros2_socketcan_msgs/msg/fd_frame.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+
+namespace lc = rclcpp_lifecycle;
+using LNI = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface;
+
+namespace drivers
+{
+namespace socketcan
+{
+/// \brief SocketCanReceiverNode class which can pass messages
+/// from CAN hardware or virtual channels
+class SOCKETCAN_PUBLIC SocketCanReceiverNode final
+  : public lc::LifecycleNode
+{
+public:
+  /// \brief Default constructor
+  explicit SocketCanReceiverNode(rclcpp::NodeOptions options);
+
+  /// \brief Callback from transition to "configuring" state.
+  /// \param[in] state The current state that the node is in.
+  LNI::CallbackReturn on_configure(const lc::State & state) override;
+
+  /// \brief Callback from transition to "activating" state.
+  /// \param[in] state The current state that the node is in.
+  LNI::CallbackReturn on_activate(const lc::State & state) override;
+
+  /// \brief Callback from transition to "deactivating" state.
+  /// \param[in] state The current state that the node is in.
+  LNI::CallbackReturn on_deactivate(const lc::State & state) override;
+
+  /// \brief Callback from transition to "unconfigured" state.
+  /// \param[in] state The current state that the node is in.
+  LNI::CallbackReturn on_cleanup(const lc::State & state) override;
+
+  /// \brief Callback from transition to "shutdown" state.
+  /// \param[in] state The current state that the node is in.
+  LNI::CallbackReturn on_shutdown(const lc::State & state) override;
+
+  /// \brief Callback for reading from hardware interface on timer tick.
+  void receive();
+
+private:
+  std::string interface_;
+  std::shared_ptr<lc::LifecyclePublisher<can_msgs::msg::Frame>> frames_pub_;
+  std::shared_ptr<lc::LifecyclePublisher<ros2_socketcan_msgs::msg::FdFrame>> fd_frames_pub_;
+  std::unique_ptr<SocketCanReceiver> receiver_;
+  std::unique_ptr<std::thread> receiver_thread_;
+  std::chrono::nanoseconds interval_ns_;
+  bool enable_fd_;
+  bool use_bus_time_;
+};
+}  // namespace socketcan
+}  // namespace drivers
+
+#endif  // ROS2_SOCKETCAN__SOCKET_CAN_RECEIVER_NODE_HPP_

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/include/ros2_socketcan/socket_can_sender.hpp

@@ -0,0 +1,196 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+/// \file
+/// \brief This file defines a class a socket sender
+#ifndef ROS2_SOCKETCAN__SOCKET_CAN_SENDER_HPP_
+#define ROS2_SOCKETCAN__SOCKET_CAN_SENDER_HPP_
+
+#include <chrono>
+#include <cstdint>
+#include <string>
+
+#include "ros2_socketcan/visibility_control.hpp"
+#include "ros2_socketcan/socket_can_id.hpp"
+
+namespace drivers
+{
+namespace socketcan
+{
+
+/// Simple RAII wrapper around a raw CAN sender
+class SOCKETCAN_PUBLIC SocketCanSender
+{
+public:
+  /// Constructor
+  explicit SocketCanSender(
+    const std::string & interface = "can0",
+    const bool enable_fd = false,
+    const CanId & default_id = CanId{});
+  /// Destructor
+  ~SocketCanSender() noexcept;
+
+  /// Send raw data with the default id
+  /// \param[in] data A pointer to the beginning of the data to send
+  /// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
+  ///                    durations are treated the same as zero timeout
+  /// \param[in] length The amount of data to send starting from the data pointer
+  /// \throw std::domain_error If length is > 8
+  /// \throw SocketCanTimeout On timeout
+  /// \throw std::runtime_error on other errors
+  void send(
+    const void * const data,
+    const std::size_t length,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
+  /// Send raw data with an explicit CAN id
+  /// \param[in] data A pointer to the beginning of the data to send
+  /// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
+  ///                    durations are treated the same as zero timeout
+  /// \param[in] id The id field for the CAN frame
+  /// \param[in] length The amount of data to send starting from the data pointer
+  /// \throw std::domain_error If length is > 8
+  /// \throw SocketCanTimeout On timeout
+  /// \throw std::runtime_error on other errors
+  void send(
+    const void * const data,
+    const std::size_t length,
+    const CanId id,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
+  /// Send typed data with the default id
+  /// \tparam Type of data to send, must be 8 bytes or smaller
+  /// \param[in] data The data to send
+  /// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
+  ///                    durations are treated the same as zero timeout
+  /// \throw SocketCanTimeout On timeout
+  /// \throw std::runtime_error on other errors
+  template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
+  void send(
+    const T & data,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
+  {
+    send(data, m_default_id, timeout);
+  }
+
+  /// Send typed data with an explicit CAN Id
+  /// \tparam Type of data to send, must be 8 bytes or smaller
+  /// \param[in] data The data to send
+  /// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
+  ///                    durations are treated the same as zero timeout
+  /// \param[in] id The id field for the CAN frame
+  /// \throw SocketCanTimeout On timeout
+  /// \throw std::runtime_error on other errors
+  template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
+  void send(
+    const T & data,
+    const CanId id,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
+  {
+    static_assert(sizeof(data) <= MAX_DATA_LENGTH, "Data type too large for CAN");
+    //lint -e586 I have to use reinterpret cast because I'm operating on bytes, see below NOLINT
+    send_impl(reinterpret_cast<const char *>(&data), sizeof(data), id, timeout);
+    // reinterpret_cast to byte, or (unsigned) char is well defined;
+    // all pointers can implicitly convert to void *
+  }
+
+  /// Send raw data with the default id
+  /// \param[in] data A pointer to the beginning of the data to send
+  /// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
+  ///                    durations are treated the same as zero timeout
+  /// \param[in] length The amount of data to send starting from the data pointer
+  /// \throw std::domain_error If length is > 64
+  /// \throw SocketCanTimeout On timeout
+  /// \throw std::runtime_error on other errors
+  void send_fd(
+    const void * const data,
+    const std::size_t length,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
+  /// Send raw data with an explicit CAN id
+  /// \param[in] data A pointer to the beginning of the data to send
+  /// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
+  ///                    durations are treated the same as zero timeout
+  /// \param[in] id The id field for the CAN frame
+  /// \param[in] length The amount of data to send starting from the data pointer
+  /// \throw std::domain_error If length is > 64
+  /// \throw SocketCanTimeout On timeout
+  /// \throw std::runtime_error on other errors
+  void send_fd(
+    const void * const data,
+    const std::size_t length,
+    const CanId id,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const;
+  /// Send typed data with the default id
+  /// \tparam Type of data to send, must be 8 bytes or smaller
+  /// \param[in] data The data to send
+  /// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
+  ///                    durations are treated the same as zero timeout
+  /// \throw SocketCanTimeout On timeout
+  /// \throw std::runtime_error on other errors
+  template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
+  void send_fd(
+    const T & data,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
+  {
+    send_fd(data, m_default_id, timeout);
+  }
+
+  /// Send typed data with an explicit CAN Id
+  /// \tparam Type of data to send, must be 8 bytes or smaller
+  /// \param[in] data The data to send
+  /// \param[in] timeout Maximum duration to wait for file descriptor to be free for write. Negative
+  ///                    durations are treated the same as zero timeout
+  /// \param[in] id The id field for the CAN frame
+  /// \throw SocketCanTimeout On timeout
+  /// \throw std::runtime_error on other errors
+  template<typename T, typename = std::enable_if_t<!std::is_pointer<T>::value>>
+  void send_fd(
+    const T & data,
+    const CanId id,
+    const std::chrono::nanoseconds timeout = std::chrono::nanoseconds::zero()) const
+  {
+    static_assert(sizeof(data) <= MAX_FD_DATA_LENGTH, "Data type too large for CAN FD");
+    //lint -e586 I have to use reinterpret cast because I'm operating on bytes, see below NOLINT
+    send_fd_impl(reinterpret_cast<const char *>(&data), sizeof(data), id, timeout);
+    // reinterpret_cast to byte, or (unsigned) char is well defined;
+    // all pointers can implicitly convert to void *
+  }
+
+  /// Get the default CAN id
+  CanId default_id() const noexcept;
+
+private:
+  // Underlying implementation of sending, data is assumed to be of an appropriate length
+  void send_impl(
+    const void * const data,
+    const std::size_t length,
+    const CanId id,
+    const std::chrono::nanoseconds timeout) const;
+  // Underlying implementation of FD sending, data is assumed to be of an appropriate length
+  void send_fd_impl(
+    const void * const data,
+    const std::size_t length,
+    const CanId id,
+    const std::chrono::nanoseconds timeout) const;
+  // Wait for file descriptor to be available to send data via select()
+  SOCKETCAN_LOCAL void wait(const std::chrono::nanoseconds timeout) const;
+
+  bool m_enable_fd;
+  int32_t m_file_descriptor{};
+  CanId m_default_id;
+};  // class SocketCanSender
+
+}  // namespace socketcan
+}  // namespace drivers
+
+#endif  // ROS2_SOCKETCAN__SOCKET_CAN_SENDER_HPP_

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/include/ros2_socketcan/socket_can_sender_node.hpp

@@ -0,0 +1,87 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#ifndef ROS2_SOCKETCAN__SOCKET_CAN_SENDER_NODE_HPP_
+#define ROS2_SOCKETCAN__SOCKET_CAN_SENDER_NODE_HPP_
+
+#include <memory>
+#include <string>
+
+#include "ros2_socketcan/visibility_control.hpp"
+#include "ros2_socketcan/socket_can_sender.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_components/register_node_macro.hpp"
+#include "rclcpp_lifecycle/lifecycle_node.hpp"
+#include "rosidl_runtime_cpp/message_initialization.hpp"
+#include "can_msgs/msg/frame.hpp"
+#include "ros2_socketcan_msgs/msg/fd_frame.hpp"
+#include "lifecycle_msgs/msg/state.hpp"
+
+namespace lc = rclcpp_lifecycle;
+using LNI = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface;
+
+namespace drivers
+{
+namespace socketcan
+{
+/// \brief SocketCanSenderNode class which can pass messages
+/// from CAN hardware or virtual channels
+class SOCKETCAN_PUBLIC SocketCanSenderNode final
+  : public lc::LifecycleNode
+{
+public:
+  /// \brief Default constructor
+  explicit SocketCanSenderNode(rclcpp::NodeOptions options);
+
+  /// \brief Callback from transition to "configuring" state.
+  /// \param[in] state The current state that the node is in.
+  LNI::CallbackReturn on_configure(const lc::State & state) override;
+
+  /// \brief Callback from transition to "activating" state.
+  /// \param[in] state The current state that the node is in.
+  LNI::CallbackReturn on_activate(const lc::State & state) override;
+
+  /// \brief Callback from transition to "deactivating" state.
+  /// \param[in] state The current state that the node is in.
+  LNI::CallbackReturn on_deactivate(const lc::State & state) override;
+
+  /// \brief Callback from transition to "unconfigured" state.
+  /// \param[in] state The current state that the node is in.
+  LNI::CallbackReturn on_cleanup(const lc::State & state) override;
+
+  /// \brief Callback from transition to "shutdown" state.
+  /// \param[in] state The current state that the node is in.
+  LNI::CallbackReturn on_shutdown(const lc::State & state) override;
+
+  /// \brief Callback for ros can frame.
+  void on_frame(const can_msgs::msg::Frame::SharedPtr msg);
+
+  /// \brief Callback for ros can fd frame.
+  void on_fd_frame(const ros2_socketcan_msgs::msg::FdFrame::SharedPtr msg);
+
+private:
+  std::string interface_;
+  bool enable_fd_;
+  rclcpp::Subscription<can_msgs::msg::Frame>::SharedPtr frames_sub_;
+  rclcpp::Subscription<ros2_socketcan_msgs::msg::FdFrame>::SharedPtr fd_frames_sub_;
+  std::unique_ptr<SocketCanSender> sender_;
+  std::chrono::nanoseconds timeout_ns_;
+};
+}  // namespace socketcan
+}  // namespace drivers
+
+#endif  // ROS2_SOCKETCAN__SOCKET_CAN_SENDER_NODE_HPP_

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/include/ros2_socketcan/visibility_control.hpp

@@ -0,0 +1,50 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+#ifndef ROS2_SOCKETCAN__VISIBILITY_CONTROL_HPP_
+#define ROS2_SOCKETCAN__VISIBILITY_CONTROL_HPP_
+
+// 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_EXPORT __attribute__ ((dllexport))
+    #define SOCKETCAN_IMPORT __attribute__ ((dllimport))
+  #else
+    #define SOCKETCAN_EXPORT __declspec(dllexport)
+    #define SOCKETCAN_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef SOCKETCAN_BUILDING_LIBRARY
+    #define SOCKETCAN_PUBLIC SOCKETCAN_EXPORT
+  #else
+    #define SOCKETCAN_PUBLIC SOCKETCAN_IMPORT
+  #endif
+  #define SOCKETCAN_PUBLIC_TYPE SOCKETCAN_PUBLIC
+  #define SOCKETCAN_LOCAL
+#else
+  #define SOCKETCAN_EXPORT __attribute__ ((visibility("default")))
+  #define SOCKETCAN_IMPORT
+  #if __GNUC__ >= 4
+    #define SOCKETCAN_PUBLIC __attribute__ ((visibility("default")))
+    #define SOCKETCAN_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define SOCKETCAN_PUBLIC
+    #define SOCKETCAN_LOCAL
+  #endif
+  #define SOCKETCAN_PUBLIC_TYPE
+#endif
+
+#endif  // ROS2_SOCKETCAN__VISIBILITY_CONTROL_HPP_

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/launch/socket_can_bridge.launch.xml

@@ -0,0 +1,27 @@
+<launch>
+
+  <arg name="interface" default="can0" />
+  <arg name="receiver_interval_sec" default="0.01" />
+  <arg name="sender_timeout_sec" default="0.01" />
+  <arg name="enable_can_fd" default="false" />
+  <arg name="from_can_bus_topic" default="/socket_can/from_can_bus" />
+  <arg name="to_can_bus_topic" default="/socket_can/to_can_bus" />
+  <arg name="use_bus_time" default="true" />
+  
+  <include file="$(find-pkg-share ros2_socketcan)/launch/socket_can_receiver.launch.py">
+    <arg name="interface" value="$(var interface)" />
+    <arg name="interval_sec" value="$(var receiver_interval_sec)" />
+    <arg name="enable_can_fd" value="$(var enable_can_fd)" />
+    <arg name="from_can_bus_topic" value="$(var from_can_bus_topic)" />
+    <arg name="use_bus_time" value="$(var use_bus_time)" />
+  </include>
+
+  <include file="$(find-pkg-share ros2_socketcan)/launch/socket_can_sender.launch.py">
+    <arg name="interface" value="$(var interface)" />
+    <arg name="timeout_sec" value="$(var sender_timeout_sec)" />
+    <arg name="enable_can_fd" value="$(var enable_can_fd)" />
+    <arg name="to_can_bus_topic" value="$(var to_can_bus_topic)" />
+  </include>
+
+</launch>
+

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/launch/socket_can_receiver.launch.py

@@ -0,0 +1,114 @@
+# Copyright 2021 the Autoware Foundation
+#
+# 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.
+#
+# Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+
+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, TextSubstitution
+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():
+    socket_can_receiver_node = LifecycleNode(
+        package='ros2_socketcan',
+        executable='socket_can_receiver_node_exe',
+        name='socket_can_receiver',
+        namespace=TextSubstitution(text=''),
+        parameters=[{
+            'interface': LaunchConfiguration('interface'),
+            'enable_can_fd': LaunchConfiguration('enable_can_fd'),
+            'interval_sec':
+            LaunchConfiguration('interval_sec'),
+            'filters': LaunchConfiguration('filters'),
+            'use_bus_time': LaunchConfiguration('use_bus_time'),
+        }],
+        remappings=[('from_can_bus', LaunchConfiguration('from_can_bus_topic'))],
+        output='screen')
+
+    socket_can_receiver_configure_event_handler = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=socket_can_receiver_node,
+            on_start=[
+                EmitEvent(
+                    event=ChangeState(
+                        lifecycle_node_matcher=matches_action(socket_can_receiver_node),
+                        transition_id=Transition.TRANSITION_CONFIGURE,
+                    ),
+                ),
+            ],
+        ),
+        condition=IfCondition(LaunchConfiguration('auto_configure')),
+    )
+
+    socket_can_receiver_activate_event_handler = RegisterEventHandler(
+        event_handler=OnStateTransition(
+            target_lifecycle_node=socket_can_receiver_node,
+            start_state='configuring',
+            goal_state='inactive',
+            entities=[
+                EmitEvent(
+                    event=ChangeState(
+                        lifecycle_node_matcher=matches_action(socket_can_receiver_node),
+                        transition_id=Transition.TRANSITION_ACTIVATE,
+                    ),
+                ),
+            ],
+        ),
+        condition=IfCondition(LaunchConfiguration('auto_activate')),
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument('interface', default_value='can0'),
+        DeclareLaunchArgument('enable_can_fd', default_value='false'),
+        DeclareLaunchArgument('interval_sec', default_value='0.01'),
+        DeclareLaunchArgument('use_bus_time', default_value='false'),
+        DeclareLaunchArgument('filters', default_value='0:0',
+                              description='Comma separated filters can be specified for each given'
+                                          ' CAN interface.\n'
+                                          '\t<can_id>:<can_mask>\n'
+                                          '\t\t(matches when <received_can_id> & mask == can_id & '
+                                          'mask)\n'
+                                          '\t<can_id>~<can_mask>\n'
+                                          '\t\t(matches when <received_can_id> & mask != can_id & '
+                                          'mask)\n'
+                                          '\t#<error_mask>\n'
+                                          '\t\t(set error frame filter, see include/linux/can/'
+                                          'error.h)\n'
+                                          '\t[j|J]\n'
+                                          '\t\t(join the given CAN filters - logical AND '
+                                          'semantic)\n\n'
+                                          '\tCAN IDs, masks and data content are given and '
+                                          'expected in hexadecimal values. When can_id and '
+                                          'can_mask are both 8 digits, they are assumed to '
+                                          "be 29 bit EFF. '0:0' default filter will accept "
+                                          'all data frames.\n'
+                                          '\tFor more information about syntax check: '
+                                          'https://manpages.ubuntu.com/manpages/jammy/'
+                                          'man1/candump.1.html'),
+        DeclareLaunchArgument('auto_configure', default_value='true'),
+        DeclareLaunchArgument('auto_activate', default_value='true'),
+        DeclareLaunchArgument('from_can_bus_topic', default_value='from_can_bus'),
+        socket_can_receiver_node,
+        socket_can_receiver_configure_event_handler,
+        socket_can_receiver_activate_event_handler,
+    ])

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/launch/socket_can_sender.launch.py

@@ -0,0 +1,88 @@
+# Copyright 2021 the Autoware Foundation
+#
+# 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.
+#
+# Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+
+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, TextSubstitution
+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():
+    socket_can_sender_node = LifecycleNode(
+        package='ros2_socketcan',
+        executable='socket_can_sender_node_exe',
+        name='socket_can_sender',
+        namespace=TextSubstitution(text=''),
+        parameters=[{
+            'interface': LaunchConfiguration('interface'),
+            'enable_can_fd': LaunchConfiguration('enable_can_fd'),
+            'timeout_sec':
+            LaunchConfiguration('timeout_sec'),
+        }],
+        remappings=[('to_can_bus', LaunchConfiguration('to_can_bus_topic'))],
+        output='screen')
+
+    socket_can_sender_configure_event_handler = RegisterEventHandler(
+        event_handler=OnProcessStart(
+            target_action=socket_can_sender_node,
+            on_start=[
+                EmitEvent(
+                    event=ChangeState(
+                        lifecycle_node_matcher=matches_action(socket_can_sender_node),
+                        transition_id=Transition.TRANSITION_CONFIGURE,
+                    ),
+                ),
+            ],
+        ),
+        condition=IfCondition(LaunchConfiguration('auto_configure')),
+    )
+
+    socket_can_sender_activate_event_handler = RegisterEventHandler(
+        event_handler=OnStateTransition(
+            target_lifecycle_node=socket_can_sender_node,
+            start_state='configuring',
+            goal_state='inactive',
+            entities=[
+                EmitEvent(
+                    event=ChangeState(
+                        lifecycle_node_matcher=matches_action(socket_can_sender_node),
+                        transition_id=Transition.TRANSITION_ACTIVATE,
+                    ),
+                ),
+            ],
+        ),
+        condition=IfCondition(LaunchConfiguration('auto_activate')),
+    )
+
+    return LaunchDescription([
+        DeclareLaunchArgument('interface', default_value='can0'),
+        DeclareLaunchArgument('enable_can_fd', default_value='false'),
+        DeclareLaunchArgument('timeout_sec', default_value='0.01'),
+        DeclareLaunchArgument('auto_configure', default_value='true'),
+        DeclareLaunchArgument('auto_activate', default_value='true'),
+        DeclareLaunchArgument('to_can_bus_topic', default_value='to_can_bus'),
+        socket_can_sender_node,
+        socket_can_sender_configure_event_handler,
+        socket_can_sender_activate_event_handler,
+    ])

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ros2_socketcan</name>
+  <version>1.3.0</version>
+  <description>Simple wrapper around SocketCAN</description>
+  <maintainer email="whitleysoftwareservices@gmail.com">Josh Whitley</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclcpp_components</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>lifecycle_msgs</depend>
+  <depend>can_msgs</depend>
+  <depend>ros2_socketcan_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_cmake_gtest</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/src/socket_can_common.cpp

@@ -0,0 +1,161 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#include "ros2_socketcan/socket_can_common.hpp"
+
+#include <fcntl.h>
+#include <net/if.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <linux/can/raw.h>
+
+#include <unistd.h>
+#include <linux/can.h>
+
+#include <cstring>
+#include <stdexcept>
+#include <string>
+#include <vector>
+
+namespace drivers
+{
+namespace socketcan
+{
+
+////////////////////////////////////////////////////////////////////////////////
+int32_t bind_can_socket(const std::string & interface, bool enable_fd)
+{
+  if (interface.length() >= static_cast<std::string::size_type>(IFNAMSIZ)) {
+    throw std::domain_error{"CAN interface name too long"};
+  }
+
+  // Create file descriptor
+  const auto file_descriptor = socket(PF_CAN, static_cast<int32_t>(SOCK_RAW), CAN_RAW);
+  if (0 > file_descriptor) {
+    throw std::runtime_error{"Failed to open CAN socket"};
+  }
+  // Make it non-blocking so we can use timeouts
+  //lint -e{9001} NOLINT I can't do anything about using this third party octal constant...
+  if (0 != fcntl(file_descriptor, F_SETFL, O_NONBLOCK)) {
+    throw std::runtime_error{"Failed to set CAN socket to nonblocking"};
+  }
+
+  // Set up address/interface name
+  struct ifreq ifr;
+  // The destination struct is local; don't need address
+  (void)strncpy(&ifr.ifr_name[0U], interface.c_str(), interface.length() + 1U);
+  if (0 != ioctl(file_descriptor, static_cast<uint32_t>(SIOCGIFINDEX), &ifr)) {
+    throw std::runtime_error{"Failed to set CAN socket name via ioctl()"};
+  }
+
+  struct sockaddr_can addr;
+  addr.can_family = static_cast<decltype(addr.can_family)>(AF_CAN);
+  addr.can_ifindex = ifr.ifr_ifindex;
+
+  // Bind address
+  //lint -save -e586 NOLINT This (c-style casts actually) is the idiomatic way to use sockaddr
+  if (0 > bind(file_descriptor, reinterpret_cast<struct sockaddr *>(&addr), sizeof(addr))) {
+    throw std::runtime_error{"Failed to bind CAN socket"};
+  }
+  //lint -restore NOLINT
+
+  // Enable CAN FD support
+  const int32_t enable_canfd = enable_fd ? 1 : 0;
+  if (0 !=
+    setsockopt(
+      file_descriptor, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &enable_canfd,
+      sizeof(enable_canfd)))
+  {
+    throw std::runtime_error{"Failed to enable CAN FD support"};
+  }
+
+  return file_descriptor;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void set_can_filter(int32_t fd, const std::vector<struct can_filter> & f_list)
+{
+  if (0 !=
+    setsockopt(
+      fd, SOL_CAN_RAW, CAN_RAW_FILTER, f_list.empty() ? NULL : f_list.data(),
+      sizeof(can_filter) * f_list.size()))
+  {
+    throw std::runtime_error{"Failed to set up CAN filters: " + std::string{strerror(errno)}};
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void set_can_err_filter(int32_t fd, can_err_mask_t err_mask)
+{
+  if (0 !=
+    setsockopt(
+      fd, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &err_mask,
+      sizeof(err_mask)))
+  {
+    throw std::runtime_error{"Failed to set up CAN error filters: " +
+            std::string{strerror(errno)}};
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void set_can_filter_join(int32_t fd, bool join_filters)
+{
+  auto join = static_cast<int>(join_filters);
+  if (0 !=
+    setsockopt(
+      fd, SOL_CAN_RAW, CAN_RAW_JOIN_FILTERS, &join,
+      sizeof(join)))
+  {
+    throw std::runtime_error{"Failed to set up joined CAN filters: " +
+            std::string{strerror(errno)}};
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+struct timeval to_timeval(const std::chrono::nanoseconds timeout) noexcept
+{
+  const auto count = timeout.count();
+  constexpr auto BILLION = 1'000'000'000LL;
+  struct timeval c_timeout;
+  c_timeout.tv_sec = static_cast<decltype(c_timeout.tv_sec)>(count / BILLION);
+  c_timeout.tv_usec = static_cast<decltype(c_timeout.tv_usec)>((count % BILLION) / 1000LL);
+
+  return c_timeout;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+uint64_t from_timeval(const struct timeval tv) noexcept
+{
+  return static_cast<uint64_t>(tv.tv_sec) * 1e6 + tv.tv_usec;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+fd_set single_set(int32_t file_descriptor) noexcept
+{
+  fd_set descriptor_set;
+  // TODO(c.ho) sort through all these MISRA errors...
+  //lint -save -e9146 NOLINT
+  //lint --e{9063, 9036, 9084, 9027, 9033, 550, 717, 9001, 9093, 953} NOLINT
+  FD_ZERO(&descriptor_set);
+  //lint --e{9063, 9036, 9084, 9027, 9033, 550, 9123, 9125, 9126, 1924, 9130} NOLINT
+  FD_SET(file_descriptor, &descriptor_set);
+  //lint -restore NOLINT
+
+  return descriptor_set;
+}
+}  // namespace socketcan
+}  // namespace drivers

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/src/socket_can_id.cpp

@@ -0,0 +1,194 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#include <linux/can.h>  // for CAN typedef so I can static_assert it
+
+#include <utility>
+
+#include "ros2_socketcan/socket_can_id.hpp"
+
+namespace drivers
+{
+namespace socketcan
+{
+
+//lint -e{9006} NOLINT false positive: this expression is compile time evaluated
+static_assert(
+  MAX_DATA_LENGTH == sizeof(std::declval<struct can_frame>().data),
+  "Unexpected CAN frame data size");
+static_assert(
+  MAX_FD_DATA_LENGTH == sizeof(std::declval<struct canfd_frame>().data),
+  "Unexpected CAN FD frame data size");
+static_assert(std::is_same<CanId::IdT, canid_t>::value, "Underlying type of CanId is incorrect");
+constexpr CanId::IdT EXTENDED_MASK = CAN_EFF_FLAG;
+constexpr CanId::IdT REMOTE_MASK = CAN_RTR_FLAG;
+constexpr CanId::IdT ERROR_MASK = CAN_ERR_FLAG;
+constexpr CanId::IdT EXTENDED_ID_MASK = CAN_EFF_MASK;
+constexpr CanId::IdT STANDARD_ID_MASK = CAN_SFF_MASK;
+
+////////////////////////////////////////////////////////////////////////////////
+CanId::CanId(const IdT raw_id, const uint64_t bus_time, const LengthT data_length)
+: m_id{raw_id},
+  m_data_length{data_length},
+  bus_time(bus_time)
+{
+  (void)frame_type();  // just to throw
+}
+CanId::CanId(const IdT id, const uint64_t bus_time, FrameType type, StandardFrame_)
+: CanId{id, bus_time, type, false} {}
+
+CanId::CanId(const IdT id, const uint64_t bus_time, FrameType type, ExtendedFrame_)
+: CanId{id, bus_time, type, true} {}
+
+////////////////////////////////////////////////////////////////////////////////
+CanId::CanId(const IdT id, const uint64_t bus_time, FrameType type, bool is_extended)
+: bus_time(bus_time)
+{
+  // Set extended bit
+  if (is_extended) {
+    (void)extended();
+  }
+  (void)frame_type(type);
+  (void)identifier(id);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+CanId & CanId::standard() noexcept
+{
+  //lint -e{9126} NOLINT false positive: underlying type is unsigned long, and same as m_id
+  m_id = m_id & (~EXTENDED_MASK);
+  return *this;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+CanId & CanId::extended() noexcept
+{
+  m_id = m_id | EXTENDED_MASK;
+  return *this;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+CanId & CanId::error_frame() noexcept
+{
+  //lint -e{9126} NOLINT false positive: underlying type is unsigned long, and same as m_id
+  m_id = m_id & (~REMOTE_MASK);
+  m_id = m_id | ERROR_MASK;
+  return *this;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+CanId & CanId::remote_frame() noexcept
+{
+  //lint -e{9126} NOLINT false positive: underlying type is unsigned long, and same as m_id
+  m_id = m_id & (~ERROR_MASK);
+  m_id = m_id | REMOTE_MASK;
+  return *this;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+CanId & CanId::data_frame() noexcept
+{
+  //lint -e{9126} NOLINT false positive: underlying type is unsigned long, and same as m_id
+  m_id = m_id & (~ERROR_MASK);
+  //lint -e{9126} NOLINT false positive: underlying type is unsigned long, and same as m_id
+  m_id = m_id & (~REMOTE_MASK);
+  return *this;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+CanId & CanId::frame_type(const FrameType type)
+{
+  switch (type) {
+    case FrameType::DATA:
+      (void)data_frame();
+      break;
+    case FrameType::ERROR:
+      (void)error_frame();
+      break;
+    case FrameType::REMOTE:
+      (void)remote_frame();
+      break;
+    default:
+      throw std::logic_error{"CanId: No such type"};
+  }
+  return *this;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+CanId & CanId::identifier(const IdT id)
+{
+  // Can specification: http://esd.cs.ucr.edu/webres/can20.pdf
+  // says "The 7 most significant bits cannot all be recessive (value of 1)", pg 11
+  constexpr auto MAX_EXTENDED = 0x1FBF'FFFFU;
+  constexpr auto MAX_STANDARD = 0x07EFU;
+  static_assert(MAX_EXTENDED <= EXTENDED_ID_MASK, "Max extended id value is wrong");
+  static_assert(MAX_STANDARD <= STANDARD_ID_MASK, "Max extended id value is wrong");
+  auto max_id = MAX_STANDARD;
+  auto unmasked_id = id;
+  if (is_extended()) {
+    max_id = MAX_EXTENDED;
+    unmasked_id = id & ~(EXTENDED_MASK);
+  }
+  if (max_id < unmasked_id) {
+    throw std::domain_error{"CanId would be truncated!"};
+  }
+  // Clear and set
+  //lint -e{9126} NOLINT false positive: underlying type is unsigned long, and same as m_id
+  m_id = m_id & (~EXTENDED_ID_MASK);  // clear ALL ID bits, not just standard bits
+  m_id = m_id | id;
+  return *this;
+}
+////////////////////////////////////////////////////////////////////////////////
+CanId::IdT CanId::get() const noexcept
+{
+  return m_id;
+}
+////////////////////////////////////////////////////////////////////////////////
+bool CanId::is_extended() const noexcept
+{
+  return (m_id & EXTENDED_MASK) == EXTENDED_MASK;
+}
+////////////////////////////////////////////////////////////////////////////////
+CanId::IdT CanId::identifier() const noexcept
+{
+  const auto mask = is_extended() ? EXTENDED_ID_MASK : STANDARD_ID_MASK;
+  return m_id & mask;
+}
+////////////////////////////////////////////////////////////////////////////////
+CanId::LengthT CanId::length() const noexcept
+{
+  return m_data_length;
+}
+////////////////////////////////////////////////////////////////////////////////
+FrameType CanId::frame_type() const
+{
+  const auto is_error = (m_id & ERROR_MASK) == ERROR_MASK;
+  const auto is_remote = (m_id & REMOTE_MASK) == REMOTE_MASK;
+  if (is_error && is_remote) {
+    throw std::domain_error{"CanId has both bits 29 and 30 set! Inconsistent!"};
+  }
+
+  if (is_error) {
+    return FrameType::ERROR;
+  }
+  if (is_remote) {
+    return FrameType::REMOTE;
+  }
+  return FrameType::DATA;
+}
+
+}  // namespace socketcan
+}  // namespace drivers

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/src/socket_can_receiver.cpp

@@ -0,0 +1,207 @@
+// Copyright 2019 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#include "ros2_socketcan/socket_can_common.hpp"
+#include "ros2_socketcan/socket_can_receiver.hpp"
+
+#include <unistd.h>  // for close()
+#include <sys/ioctl.h>
+#include <sys/types.h>
+#include <sys/select.h>
+#include <sys/socket.h>
+#include <linux/can.h>
+#include <linux/sockios.h>
+
+#include <cstring>
+#include <string>
+#include <sstream>
+#include <vector>
+#include <cstdio>
+
+namespace drivers
+{
+namespace socketcan
+{
+
+////////////////////////////////////////////////////////////////////////////////
+SocketCanReceiver::SocketCanReceiver(const std::string & interface, const bool enable_fd)
+: m_file_descriptor{bind_can_socket(interface, enable_fd)},
+  m_enable_fd(enable_fd)
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+SocketCanReceiver::~SocketCanReceiver() noexcept
+{
+  // Can't do anything on error; in fact generally shouldn't on close() error
+  (void)close(m_file_descriptor);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+SocketCanReceiver::CanFilterList::CanFilterList(const char * str)
+{
+  *this = ParseFilters(str);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+SocketCanReceiver::CanFilterList::CanFilterList(const std::string & str)
+{
+  *this = ParseFilters(str);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+SocketCanReceiver::CanFilterList SocketCanReceiver::CanFilterList::ParseFilters(
+  const std::string & str)
+{
+  CanFilterList filter_list;
+  filter_list.error_mask = 0;
+  filter_list.join_filters = false;
+
+  std::istringstream input(str);
+  std::string fstr;
+
+  while (getline(input, fstr, ',')) {
+    // trim leading and trailing whitespaces
+    fstr = fstr.substr(
+      fstr.find_first_not_of(" \t"),
+      fstr.find_last_not_of(" \t") - fstr.find_first_not_of(" \t") + 1);
+
+    struct can_filter filter;
+    if (std::sscanf(fstr.c_str(), "%x:%x", &filter.can_id, &filter.can_mask) == 2) {
+      filter.can_mask &= ~CAN_ERR_FLAG;
+      if (fstr.size() > 8 && fstr[8] == ':') {
+        filter.can_id |= CAN_EFF_FLAG;
+      }
+      filter_list.filters.push_back(filter);
+    } else if (std::sscanf(fstr.c_str(), "%x~%x", &filter.can_id, &filter.can_mask) == 2) {
+      filter.can_id |= CAN_INV_FILTER;
+      filter.can_mask &= ~CAN_ERR_FLAG;
+      if (fstr.size() > 8 && fstr[8] == '~') {
+        filter.can_id |= CAN_EFF_FLAG;
+      }
+      filter_list.filters.push_back(filter);
+    } else if (fstr == "j" || fstr == "J") {
+      filter_list.join_filters = true;
+    } else if (std::sscanf(fstr.c_str(), "#%x", &filter_list.error_mask) != 1) {
+      throw std::runtime_error("Error during filter parsing: " + fstr);
+    }
+  }
+  return filter_list;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void SocketCanReceiver::SetCanFilters(const CanFilterList & filters)
+{
+  set_can_filter(m_file_descriptor, filters.filters);
+  set_can_err_filter(m_file_descriptor, filters.error_mask);
+  set_can_filter_join(m_file_descriptor, filters.join_filters);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void SocketCanReceiver::wait(const std::chrono::nanoseconds timeout) const
+{
+  if (decltype(timeout)::zero() < timeout) {
+    auto c_timeout = to_timeval(timeout);
+    auto read_set = single_set(m_file_descriptor);
+    // Wait
+    if (0 == select(m_file_descriptor + 1, &read_set, NULL, NULL, &c_timeout)) {
+      throw SocketCanTimeout{"CAN Receive Timeout"};
+    }
+    //lint --e{9130, 1924, 9123, 9125, 1924, 9126} NOLINT
+    if (!FD_ISSET(m_file_descriptor, &read_set)) {
+      throw SocketCanTimeout{"CAN Receive timeout"};
+    }
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+CanId SocketCanReceiver::receive(void * const data, const std::chrono::nanoseconds timeout) const
+{
+  if (m_enable_fd) {
+    throw std::runtime_error{"attempted to read standard frame from FD socket"};
+  }
+
+  wait(timeout);
+  // Read
+  struct can_frame frame;
+  const auto nbytes = read(m_file_descriptor, &frame, sizeof(frame));
+  // Checks
+  if (nbytes < 0) {
+    throw std::runtime_error{strerror(errno)};
+  }
+  if (static_cast<std::size_t>(nbytes) < sizeof(frame)) {
+    throw std::runtime_error{"read: incomplete CAN frame"};
+  }
+  if (static_cast<std::size_t>(nbytes) != sizeof(frame)) {
+    throw std::logic_error{"Message was wrong size"};
+  }
+  // Write
+  const auto data_length = static_cast<CanId::LengthT>(frame.can_dlc);
+  (void)std::memcpy(data, static_cast<void *>(&frame.data[0U]), data_length);
+
+  // get bus timestamp
+  struct timeval tv;
+  ioctl(m_file_descriptor, SIOCGSTAMP, &tv);
+  uint64_t bus_time = from_timeval(tv);
+
+  return CanId{frame.can_id, bus_time, data_length};
+}
+
+////////////////////////////////////////////////////////////////////////////////
+CanId SocketCanReceiver::receive_fd(void * const data, const std::chrono::nanoseconds timeout) const
+{
+  if (!m_enable_fd) {
+    throw std::runtime_error{"attempted to read FD frame from standard socket"};
+  }
+
+  wait(timeout);
+  // Read
+  struct canfd_frame frame;
+  const auto nbytes = read(m_file_descriptor, &frame, sizeof(frame));
+
+  // Checks
+  if (nbytes < 0) {
+    throw std::runtime_error{strerror(errno)};
+  }
+
+  if (static_cast<std::size_t>(nbytes) < sizeof(frame.can_id) + sizeof(frame.len)) {
+    throw std::runtime_error{"read: corrupted CAN frame"};
+  }
+
+  if (frame.len > CANFD_MAX_DLEN) {
+    throw std::runtime_error{"read: frame length is larger than max allowed CAN FD payload length"};
+  }
+
+  const auto data_length = static_cast<CanId::LengthT>(frame.len);
+  // some CAN FD frames are shorter than 64 bytes
+  const auto expected_length = sizeof(frame) - sizeof(frame.data) + data_length;
+
+  if (static_cast<std::size_t>(nbytes) < expected_length) {
+    throw std::runtime_error{"read: incomplete CAN FD frame"};
+  }
+  // Write
+  (void)std::memcpy(data, static_cast<void *>(&frame.data[0U]), data_length);
+
+  // get bus timestamp
+  struct timeval tv;
+  ioctl(m_file_descriptor, SIOCGSTAMP, &tv);
+  uint64_t bus_time = from_timeval(tv);
+
+  return CanId{frame.can_id, bus_time, data_length};
+}
+
+}  // namespace socketcan
+}  // namespace drivers

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/src/socket_can_receiver_node.cpp

@@ -0,0 +1,223 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#include "ros2_socketcan/socket_can_receiver_node.hpp"
+#include "ros2_socketcan/socket_can_common.hpp"
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <utility>
+#include <vector>
+
+namespace lc = rclcpp_lifecycle;
+using LNI = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface;
+using lifecycle_msgs::msg::State;
+using namespace std::chrono_literals;
+
+namespace drivers
+{
+namespace socketcan
+{
+SocketCanReceiverNode::SocketCanReceiverNode(rclcpp::NodeOptions options)
+: lc::LifecycleNode("socket_can_receiver_node", options)
+{
+  interface_ = this->declare_parameter("interface", "can0");
+  use_bus_time_ = this->declare_parameter<bool>("use_bus_time", false);
+  enable_fd_ = this->declare_parameter<bool>("enable_can_fd", false);
+  double interval_sec = this->declare_parameter("interval_sec", 0.01);
+  this->declare_parameter("filters", "0:0");
+  interval_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
+    std::chrono::duration<double>(interval_sec));
+
+  RCLCPP_INFO(this->get_logger(), "interface: %s", interface_.c_str());
+  RCLCPP_INFO(this->get_logger(), "use bus time: %d", use_bus_time_);
+  RCLCPP_INFO(this->get_logger(), "can fd enabled: %s", enable_fd_ ? "true" : "false");
+  RCLCPP_INFO(this->get_logger(), "interval(s): %f", interval_sec);
+}
+
+LNI::CallbackReturn SocketCanReceiverNode::on_configure(const lc::State & state)
+{
+  (void)state;
+
+  try {
+    receiver_ = std::make_unique<SocketCanReceiver>(interface_, enable_fd_);
+    // apply CAN filters
+    auto filters = get_parameter("filters").as_string();
+    receiver_->SetCanFilters(SocketCanReceiver::CanFilterList(filters));
+    RCLCPP_INFO(get_logger(), "applied filters: %s", filters.c_str());
+  } catch (const std::exception & ex) {
+    RCLCPP_ERROR(
+      this->get_logger(), "Error opening CAN receiver: %s - %s",
+      interface_.c_str(), ex.what());
+    return LNI::CallbackReturn::FAILURE;
+  }
+
+  RCLCPP_DEBUG(this->get_logger(), "Receiver successfully configured.");
+
+  if (!enable_fd_) {
+    frames_pub_ = this->create_publisher<can_msgs::msg::Frame>("from_can_bus", 500);
+  } else {
+    fd_frames_pub_ =
+      this->create_publisher<ros2_socketcan_msgs::msg::FdFrame>("from_can_bus_fd", 500);
+  }
+
+  receiver_thread_ = std::make_unique<std::thread>(&SocketCanReceiverNode::receive, this);
+
+  return LNI::CallbackReturn::SUCCESS;
+}
+
+LNI::CallbackReturn SocketCanReceiverNode::on_activate(const lc::State & state)
+{
+  (void)state;
+
+  // 检查当前状态，如果已经是 active，则忽略重复请求
+  if (this->get_current_state().id() == State::PRIMARY_STATE_ACTIVE) {
+    RCLCPP_DEBUG(this->get_logger(), "Receiver is already active, ignoring duplicate activate request.");
+    return LNI::CallbackReturn::SUCCESS;
+  }
+
+  if (!enable_fd_) {
+    frames_pub_->on_activate();
+  } else {
+    fd_frames_pub_->on_activate();
+  }
+
+  RCLCPP_DEBUG(this->get_logger(), "Receiver activated.");
+  return LNI::CallbackReturn::SUCCESS;
+}
+
+LNI::CallbackReturn SocketCanReceiverNode::on_deactivate(const lc::State & state)
+{
+  (void)state;
+
+  if (!enable_fd_) {
+    frames_pub_->on_deactivate();
+  } else {
+    fd_frames_pub_->on_deactivate();
+  }
+
+  RCLCPP_DEBUG(this->get_logger(), "Receiver deactivated.");
+  return LNI::CallbackReturn::SUCCESS;
+}
+
+LNI::CallbackReturn SocketCanReceiverNode::on_cleanup(const lc::State & state)
+{
+  (void)state;
+
+  if (!enable_fd_) {
+    frames_pub_.reset();
+  } else {
+    fd_frames_pub_.reset();
+  }
+
+  if (receiver_thread_->joinable()) {
+    receiver_thread_->join();
+  }
+  RCLCPP_DEBUG(this->get_logger(), "Receiver cleaned up.");
+  return LNI::CallbackReturn::SUCCESS;
+}
+
+LNI::CallbackReturn SocketCanReceiverNode::on_shutdown(const lc::State & state)
+{
+  (void)state;
+  RCLCPP_DEBUG(this->get_logger(), "Receiver shutting down.");
+  return LNI::CallbackReturn::SUCCESS;
+}
+
+void SocketCanReceiverNode::receive()
+{
+  CanId receive_id{};
+
+  if (!enable_fd_) {
+    can_msgs::msg::Frame frame_msg(rosidl_runtime_cpp::MessageInitialization::ZERO);
+    frame_msg.header.frame_id = "can";
+
+    while (rclcpp::ok()) {
+      if (this->get_current_state().id() != State::PRIMARY_STATE_ACTIVE) {
+        std::this_thread::sleep_for(100ms);
+        continue;
+      }
+
+      try {
+        receive_id = receiver_->receive(frame_msg.data.data(), interval_ns_);
+      } catch (const std::exception & ex) {
+        RCLCPP_WARN_THROTTLE(
+          this->get_logger(), *this->get_clock(), 1000,
+          "Error receiving CAN message: %s - %s",
+          interface_.c_str(), ex.what());
+        continue;
+      }
+
+      if (use_bus_time_) {
+        frame_msg.header.stamp =
+          rclcpp::Time(static_cast<int64_t>(receive_id.get_bus_time() * 1000U));
+      } else {
+        frame_msg.header.stamp = this->now();
+      }
+
+      frame_msg.id = receive_id.identifier();
+      frame_msg.is_rtr = (receive_id.frame_type() == FrameType::REMOTE);
+      frame_msg.is_extended = receive_id.is_extended();
+      frame_msg.is_error = (receive_id.frame_type() == FrameType::ERROR);
+      frame_msg.dlc = receive_id.length();
+      frames_pub_->publish(std::move(frame_msg));
+    }
+  } else {
+    ros2_socketcan_msgs::msg::FdFrame fd_frame_msg(rosidl_runtime_cpp::MessageInitialization::ZERO);
+    fd_frame_msg.header.frame_id = "can";
+
+    while (rclcpp::ok()) {
+      if (this->get_current_state().id() != State::PRIMARY_STATE_ACTIVE) {
+        std::this_thread::sleep_for(100ms);
+        continue;
+      }
+
+      fd_frame_msg.data.resize(64);
+
+      try {
+        receive_id = receiver_->receive_fd(fd_frame_msg.data.data<void>(), interval_ns_);
+      } catch (const std::exception & ex) {
+        RCLCPP_WARN_THROTTLE(
+          this->get_logger(), *this->get_clock(), 1000,
+          "Error receiving CAN FD message: %s - %s",
+          interface_.c_str(), ex.what());
+        continue;
+      }
+
+      fd_frame_msg.data.resize(receive_id.length());
+
+      if (use_bus_time_) {
+        fd_frame_msg.header.stamp =
+          rclcpp::Time(static_cast<int64_t>(receive_id.get_bus_time() * 1000U));
+      } else {
+        fd_frame_msg.header.stamp = this->now();
+      }
+
+      fd_frame_msg.id = receive_id.identifier();
+      fd_frame_msg.is_extended = receive_id.is_extended();
+      fd_frame_msg.is_error = (receive_id.frame_type() == FrameType::ERROR);
+      fd_frame_msg.len = receive_id.length();
+      fd_frames_pub_->publish(std::move(fd_frame_msg));
+    }
+  }
+}
+
+}  // namespace socketcan
+}  // namespace drivers
+
+RCLCPP_COMPONENTS_REGISTER_NODE(drivers::socketcan::SocketCanReceiverNode)
+

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/src/socket_can_sender.cpp

@@ -0,0 +1,175 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#include "ros2_socketcan/socket_can_common.hpp"
+#include "ros2_socketcan/socket_can_sender.hpp"
+
+#include <unistd.h>  // for close()
+#include <sys/select.h>
+#include <sys/socket.h>
+#include <linux/can.h>
+
+#include <cstring>
+#include <chrono>
+#include <stdexcept>
+#include <string>
+
+namespace drivers
+{
+namespace socketcan
+{
+
+////////////////////////////////////////////////////////////////////////////////
+SocketCanSender::SocketCanSender(
+  const std::string & interface,
+  const bool enable_fd,
+  const CanId & default_id)
+: m_enable_fd(enable_fd),
+  m_file_descriptor{bind_can_socket(interface, m_enable_fd)},
+  m_default_id{default_id}
+{
+}
+
+////////////////////////////////////////////////////////////////////////////////
+SocketCanSender::~SocketCanSender() noexcept
+{
+  (void)close(m_file_descriptor);
+  // I'm destructing--there's not much else I can do on an error
+}
+
+////////////////////////////////////////////////////////////////////////////////
+CanId SocketCanSender::default_id() const noexcept
+{
+  return m_default_id;
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void SocketCanSender::send(
+  const void * const data,
+  const std::size_t length,
+  const CanId id,
+  const std::chrono::nanoseconds timeout) const
+{
+  if (length > MAX_DATA_LENGTH) {
+    throw std::domain_error{"Size is too large to send via CAN"};
+  }
+  send_impl(data, length, id, timeout);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void SocketCanSender::send(
+  const void * const data,
+  const std::size_t length,
+  const std::chrono::nanoseconds timeout) const
+{
+  send(data, length, m_default_id, timeout);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void SocketCanSender::send_fd(
+  const void * const data,
+  const std::size_t length,
+  const CanId id,
+  const std::chrono::nanoseconds timeout) const
+{
+  if (length > MAX_FD_DATA_LENGTH) {
+    throw std::domain_error{"Size is too large to send via CAN FD"};
+  }
+  send_fd_impl(data, length, id, timeout);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void SocketCanSender::send_fd(
+  const void * const data,
+  const std::size_t length,
+  const std::chrono::nanoseconds timeout) const
+{
+  send_fd(data, length, m_default_id, timeout);
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void SocketCanSender::wait(const std::chrono::nanoseconds timeout) const
+{
+  if (decltype(timeout)::zero() < timeout) {
+    auto c_timeout = to_timeval(timeout);
+    auto write_set = single_set(m_file_descriptor);
+    // Wait
+    if (0 == select(m_file_descriptor + 1, NULL, &write_set, NULL, &c_timeout)) {
+      throw SocketCanTimeout{"CAN Send Timeout"};
+    }
+    //lint --e{9130, 9123, 9125, 1924, 9126} NOLINT
+    if (!FD_ISSET(m_file_descriptor, &write_set)) {
+      throw SocketCanTimeout{"CAN Send timeout"};
+    }
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void SocketCanSender::send_impl(
+  const void * const data,
+  const std::size_t length,
+  const CanId id,
+  const std::chrono::nanoseconds timeout) const
+{
+  if (m_enable_fd) {
+    throw std::runtime_error{"Tried to send standard frame from FD socket"};
+  }
+
+  // Use select call on positive timeout
+  wait(timeout);
+  // Actually send the data
+  constexpr int flags = 0;  // TODO(c.ho) not implemented
+  struct can_frame data_frame;
+  data_frame.can_id = id.get();
+  // User facing functions do check
+  data_frame.can_dlc = static_cast<decltype(data_frame.can_dlc)>(length);
+  //lint -e{586} NOLINT data_frame is a stack variable; guaranteed not to overlap
+  (void)std::memcpy(static_cast<void *>(&data_frame.data[0U]), data, length);
+  const auto bytes_sent = ::send(m_file_descriptor, &data_frame, sizeof(data_frame), flags);
+  if (0 > bytes_sent) {
+    throw std::runtime_error{strerror(errno)};
+  }
+}
+
+////////////////////////////////////////////////////////////////////////////////
+void SocketCanSender::send_fd_impl(
+  const void * const data,
+  const std::size_t length,
+  const CanId id,
+  const std::chrono::nanoseconds timeout) const
+{
+  if (!m_enable_fd) {
+    throw std::runtime_error{"Tried to send FD frame from standard socket"};
+  }
+
+  // Use select call on positive timeout
+  wait(timeout);
+  // Actually send the data
+  constexpr int flags = 0;  // TODO(c.ho) not implemented
+  struct canfd_frame data_frame;
+  data_frame.can_id = id.get();
+  // User facing functions do check
+  data_frame.len = static_cast<decltype(data_frame.len)>(length);
+  //lint -e{586} NOLINT data_frame is a stack variable; guaranteed not to overlap
+  (void)std::memcpy(static_cast<void *>(&data_frame.data[0U]), data, length);
+  const auto bytes_sent = ::send(m_file_descriptor, &data_frame, sizeof(data_frame), flags);
+  if (0 > bytes_sent) {
+    throw std::runtime_error{strerror(errno)};
+  }
+}
+
+}  // namespace socketcan
+}  // namespace drivers

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/src/socket_can_sender_node.cpp

@@ -0,0 +1,163 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#include "ros2_socketcan/socket_can_sender_node.hpp"
+#include "ros2_socketcan/socket_can_common.hpp"
+
+#include <chrono>
+#include <memory>
+#include <string>
+#include <utility>
+
+namespace lc = rclcpp_lifecycle;
+using LNI = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface;
+using lifecycle_msgs::msg::State;
+
+namespace drivers
+{
+namespace socketcan
+{
+SocketCanSenderNode::SocketCanSenderNode(rclcpp::NodeOptions options)
+: lc::LifecycleNode("socket_can_sender_node", options)
+{
+  interface_ = this->declare_parameter("interface", "can0");
+  enable_fd_ = this->declare_parameter("enable_can_fd", false);
+  double timeout_sec = this->declare_parameter("timeout_sec", 0.01);
+  timeout_ns_ = std::chrono::duration_cast<std::chrono::nanoseconds>(
+    std::chrono::duration<double>(timeout_sec));
+
+  RCLCPP_INFO(this->get_logger(), "interface: %s", interface_.c_str());
+  RCLCPP_INFO(this->get_logger(), "can fd enabled: %s", enable_fd_ ? "true" : "false");
+  RCLCPP_INFO(this->get_logger(), "timeout(s): %f", timeout_sec);
+}
+
+LNI::CallbackReturn SocketCanSenderNode::on_configure(const lc::State & state)
+{
+  (void)state;
+
+  try {
+    sender_ = std::make_unique<SocketCanSender>(interface_, enable_fd_);
+  } catch (const std::exception & ex) {
+    RCLCPP_ERROR(
+      this->get_logger(), "Error opening CAN sender: %s - %s",
+      interface_.c_str(), ex.what());
+    return LNI::CallbackReturn::FAILURE;
+  }
+
+  RCLCPP_DEBUG(this->get_logger(), "Sender successfully configured.");
+
+  if (!enable_fd_) {
+    frames_sub_ = this->create_subscription<can_msgs::msg::Frame>(
+      "to_can_bus", 500, std::bind(&SocketCanSenderNode::on_frame, this, std::placeholders::_1));
+  } else {
+    fd_frames_sub_ = this->create_subscription<ros2_socketcan_msgs::msg::FdFrame>(
+      "to_can_bus_fd", 500, std::bind(
+        &SocketCanSenderNode::on_fd_frame, this,
+        std::placeholders::_1));
+  }
+
+  return LNI::CallbackReturn::SUCCESS;
+}
+
+LNI::CallbackReturn SocketCanSenderNode::on_activate(const lc::State & state)
+{
+  (void)state;
+  RCLCPP_DEBUG(this->get_logger(), "Sender activated.");
+  return LNI::CallbackReturn::SUCCESS;
+}
+
+LNI::CallbackReturn SocketCanSenderNode::on_deactivate(const lc::State & state)
+{
+  (void)state;
+  RCLCPP_DEBUG(this->get_logger(), "Sender deactivated.");
+  return LNI::CallbackReturn::SUCCESS;
+}
+
+LNI::CallbackReturn SocketCanSenderNode::on_cleanup(const lc::State & state)
+{
+  (void)state;
+
+  if (!enable_fd_) {
+    frames_sub_.reset();
+  } else {
+    fd_frames_sub_.reset();
+  }
+
+  RCLCPP_DEBUG(this->get_logger(), "Sender cleaned up.");
+  return LNI::CallbackReturn::SUCCESS;
+}
+
+LNI::CallbackReturn SocketCanSenderNode::on_shutdown(const lc::State & state)
+{
+  (void)state;
+  RCLCPP_DEBUG(this->get_logger(), "Sender shutting down.");
+  return LNI::CallbackReturn::SUCCESS;
+}
+
+void SocketCanSenderNode::on_frame(const can_msgs::msg::Frame::SharedPtr msg)
+{
+  if (this->get_current_state().id() == State::PRIMARY_STATE_ACTIVE) {
+    FrameType type;
+    if (msg->is_rtr) {
+      type = FrameType::REMOTE;
+    } else if (msg->is_error) {
+      type = FrameType::ERROR;
+    } else {
+      type = FrameType::DATA;
+    }
+
+    CanId send_id = msg->is_extended ? CanId(msg->id, 0, type, ExtendedFrame) :
+      CanId(msg->id, 0, type, StandardFrame);
+    try {
+      sender_->send(msg->data.data(), msg->dlc, send_id, timeout_ns_);
+    } catch (const std::exception & ex) {
+      RCLCPP_WARN_THROTTLE(
+        this->get_logger(), *this->get_clock(), 1000,
+        "Error sending CAN message: %s - %s",
+        interface_.c_str(), ex.what());
+      return;
+    }
+  }
+}
+
+void SocketCanSenderNode::on_fd_frame(const ros2_socketcan_msgs::msg::FdFrame::SharedPtr msg)
+{
+  if (this->get_current_state().id() == State::PRIMARY_STATE_ACTIVE) {
+    FrameType type;
+    if (msg->is_error) {
+      type = FrameType::ERROR;
+    } else {
+      type = FrameType::DATA;
+    }
+
+    CanId send_id = msg->is_extended ? CanId(msg->id, 0, type, ExtendedFrame) :
+      CanId(msg->id, 0, type, StandardFrame);
+    try {
+      sender_->send_fd(msg->data.data<void>(), msg->len, send_id, timeout_ns_);
+    } catch (const std::exception & ex) {
+      RCLCPP_WARN_THROTTLE(
+        this->get_logger(), *this->get_clock(), 1000,
+        "Error sending CAN message: %s - %s",
+        interface_.c_str(), ex.what());
+      return;
+    }
+  }
+}
+
+}  // namespace socketcan
+}  // namespace drivers
+
+RCLCPP_COMPONENTS_REGISTER_NODE(drivers::socketcan::SocketCanSenderNode)

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/test/gtest_main.cpp

@@ -0,0 +1,22 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+#include <gtest/gtest.h>
+
+int32_t main(int32_t argc, char ** argv)
+{
+  ::testing::InitGoogleTest(&argc, argv);
+  return RUN_ALL_TESTS();
+}

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/test/receiver.cpp

@@ -0,0 +1,387 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#include <gtest/gtest.h>
+#include <linux/can/error.h>
+
+#include <chrono>
+#include <memory>
+#include <string>
+
+#include "ros2_socketcan/socket_can_receiver.hpp"
+#include "ros2_socketcan/socket_can_sender.hpp"
+
+using drivers::socketcan::SocketCanReceiver;
+using drivers::socketcan::SocketCanSender;
+using drivers::socketcan::CanId;
+using drivers::socketcan::StandardFrame;
+using drivers::socketcan::ExtendedFrame;
+using drivers::socketcan::FrameType;
+
+// Requires elevated kernel permissions normal containers can't provide
+class DISABLED_receiver : public ::testing::Test
+{
+protected:
+  void SetUp()
+  {
+    constexpr auto test_interface = "vcan0";
+    receiver_ = std::make_unique<SocketCanReceiver>(test_interface);
+    sender_ = std::make_unique<SocketCanSender>(test_interface);
+  }
+
+  std::unique_ptr<SocketCanReceiver> receiver_{};
+  std::unique_ptr<SocketCanSender> sender_{};
+  std::chrono::milliseconds send_timeout_{1LL};
+  std::chrono::milliseconds receive_timeout_{10LL};
+};  // class receiver
+
+TEST_F(DISABLED_receiver, basic_typed)
+{
+  constexpr uint32_t send_msg = 0x5A'5A'5A'5AU;
+  const CanId send_id{};
+  sender_->send(send_msg, send_id, send_timeout_);
+  {
+    uint32_t receive_msg{};
+    CanId receive_id{};
+    EXPECT_NO_THROW(receive_id = receiver_->receive(receive_msg, receive_timeout_));
+    EXPECT_EQ(receive_msg, send_msg);
+    EXPECT_EQ(receive_id.length(), sizeof(send_msg));
+    EXPECT_EQ(send_id.get(), receive_id.get());
+  }
+}
+
+TEST_F(DISABLED_receiver, ping_pong)
+{
+  for (uint64_t idx = 0U; idx < 100U; ++idx) {
+    CanId send_id{};
+    {
+      send_id.identifier(static_cast<CanId::IdT>(idx));
+      // Switch between standard and extended
+      if (idx % 2U == 0U) {
+        (void)send_id.extended();
+      } else {
+        (void)send_id.standard();
+      }
+      // Switch between remote and data; error frame is not picked up by socketCan?
+      if (idx % 3U == 0U) {
+        (void)send_id.data_frame();
+      } else {
+        (void)send_id.remote_frame();
+      }
+    }
+    EXPECT_NO_THROW(sender_->send(idx, send_id, send_timeout_)) << idx;
+    {
+      decltype(idx) receive_msg{};
+      CanId receive_id{};
+      receive_id = receiver_->receive(receive_msg, receive_timeout_);
+      EXPECT_EQ(receive_msg, idx);
+      EXPECT_EQ(receive_id.length(), sizeof(idx));
+      EXPECT_EQ(send_id.get(), receive_id.get());
+    }
+  }
+}
+
+TEST_F(DISABLED_receiver, can_filters_parser)
+{
+  typedef SocketCanReceiver::CanFilterList CanFilterList;
+
+  auto filter_list = CanFilterList("101:7FF,333:ab,404:1,92345678:DFFFFFFF");
+  ASSERT_EQ(filter_list.filters.size(), 4U);
+  EXPECT_EQ(filter_list.filters[0].can_id, 0x101U);
+  EXPECT_EQ(filter_list.filters[0].can_mask, 0x7FFU);
+  EXPECT_EQ(filter_list.filters[1].can_id, 0x333U);
+  EXPECT_EQ(filter_list.filters[1].can_mask, 0xABU);
+  EXPECT_EQ(filter_list.filters[2].can_id, 0x404U);
+  EXPECT_EQ(filter_list.filters[2].can_mask, 0x1U);
+  EXPECT_EQ(filter_list.filters[3].can_id, 0x92345678U);
+  EXPECT_EQ(filter_list.filters[3].can_mask, 0xDFFFFFFFU);
+  EXPECT_EQ(filter_list.error_mask, 0x0U);
+  EXPECT_FALSE(filter_list.join_filters);
+
+  filter_list = CanFilterList("");
+  EXPECT_TRUE(filter_list.filters.empty());
+  EXPECT_EQ(filter_list.error_mask, 0x0U);
+  EXPECT_FALSE(filter_list.join_filters);
+  filter_list = CanFilterList("#12345");
+  EXPECT_TRUE(filter_list.filters.empty());
+  EXPECT_EQ(filter_list.error_mask, 0x12345U);
+  EXPECT_FALSE(filter_list.join_filters);
+
+  filter_list = CanFilterList("j");
+  EXPECT_TRUE(filter_list.filters.empty());
+  EXPECT_EQ(filter_list.error_mask, 0x0U);
+  EXPECT_TRUE(filter_list.join_filters);
+
+
+  filter_list = CanFilterList("0~0,#FFFFFFFF");
+  ASSERT_EQ(filter_list.filters.size(), 1U);
+  EXPECT_EQ(filter_list.filters[0].can_id, 0x0U | CAN_INV_FILTER);
+  EXPECT_EQ(filter_list.filters[0].can_mask, 0x0U);
+  EXPECT_EQ(filter_list.error_mask, 0xFFFFFFFFU);
+  EXPECT_FALSE(filter_list.join_filters);
+
+  filter_list = CanFilterList("1:2,3~4,5:6,7~8,9:A,j");
+  ASSERT_EQ(filter_list.filters.size(), 5U);
+  EXPECT_EQ(filter_list.filters[0].can_id, 0x1U);
+  EXPECT_EQ(filter_list.filters[0].can_mask, 0x2U);
+  EXPECT_EQ(filter_list.filters[1].can_id, 0x3U | CAN_INV_FILTER);
+  EXPECT_EQ(filter_list.filters[1].can_mask, 0x4U);
+  EXPECT_EQ(filter_list.filters[2].can_id, 0x5U);
+  EXPECT_EQ(filter_list.filters[2].can_mask, 0x6U);
+  EXPECT_EQ(filter_list.filters[3].can_id, 0x7U | CAN_INV_FILTER);
+  EXPECT_EQ(filter_list.filters[3].can_mask, 0x8U);
+  EXPECT_EQ(filter_list.filters[4].can_id, 0x9U);
+  EXPECT_EQ(filter_list.filters[4].can_mask, 0xAU);
+  EXPECT_EQ(filter_list.error_mask, 0x0U);
+  EXPECT_TRUE(filter_list.join_filters);
+
+  filter_list = CanFilterList("ABC:DEF,123:C00007FF,J,#5");
+  ASSERT_EQ(filter_list.filters.size(), 2U);
+  EXPECT_EQ(filter_list.filters[0].can_id, 0xABCU);
+  EXPECT_EQ(filter_list.filters[0].can_mask, 0xDEFU);
+  EXPECT_EQ(filter_list.filters[1].can_id, 0x123U);
+  EXPECT_EQ(filter_list.filters[1].can_mask, 0xC00007FFU);
+  EXPECT_EQ(filter_list.error_mask, 0x5U);
+  EXPECT_TRUE(filter_list.join_filters);
+
+  // whitespace trimming test
+  filter_list = CanFilterList(
+    "        ABC:DEF             , 123:C00007FF       ,         J   ,    #5    ");
+  ASSERT_EQ(filter_list.filters.size(), 2U);
+  EXPECT_EQ(filter_list.filters[0].can_id, 0xABCU);
+  EXPECT_EQ(filter_list.filters[0].can_mask, 0xDEFU);
+  EXPECT_EQ(filter_list.filters[1].can_id, 0x123U);
+  EXPECT_EQ(filter_list.filters[1].can_mask, 0xC00007FFU);
+  EXPECT_EQ(filter_list.error_mask, 0x5U);
+  EXPECT_TRUE(filter_list.join_filters);
+
+  // test incorrect input
+  std::string str = "        ABC:DEF             , 123:C00007FF       ,         J   ,    #p5    ";
+  EXPECT_THROW(CanFilterList::ParseFilters(str), std::runtime_error);
+  str = "1:2,3~4,5:6,7~8,9:A,l";
+  EXPECT_THROW(CanFilterList::ParseFilters(str), std::runtime_error);
+  str = "1:2,3~4,5;6,7~8,9:A,j";
+  EXPECT_THROW(CanFilterList::ParseFilters(str), std::runtime_error);
+  str = "1:2,3 ~4,5:6,7~8,9:A,j";
+  EXPECT_THROW(CanFilterList::ParseFilters(str), std::runtime_error);
+  str = "not a correct string";
+  EXPECT_THROW(CanFilterList::ParseFilters(str), std::runtime_error);
+}
+
+TEST_F(DISABLED_receiver, can_filters)
+{
+  constexpr uint32_t send_msg = 0x5A'5A'5A'5AU;
+  SocketCanReceiver::CanFilterList filter_list;
+
+  ////////////////////////////////////////////////////////////////////////////////
+  // pass only ids: 0x100, 0x250, 0x555 of standard length
+  filter_list.filters = {{0x100, 0xC00007FF}, {0x250, 0xC00007FF}, {0x555, 0xC00007FF}};
+  receiver_->SetCanFilters(filter_list);
+  CanId send_id{};
+
+  // error frame should be blocked
+  send_id.error_frame();
+  send_id.identifier(static_cast<CanId::IdT>(0x100U));
+  sender_->send(send_msg, send_id, send_timeout_);
+
+  // RTR frame should be blocked
+  send_id.remote_frame();
+  send_id.standard();
+  send_id.identifier(static_cast<CanId::IdT>(0x100U));
+  sender_->send(send_msg, send_id, send_timeout_);
+
+  // extended data frame should be blocked
+  send_id.data_frame();
+  send_id.extended();
+  send_id.identifier(static_cast<CanId::IdT>(0x100U));
+  sender_->send(send_msg, send_id, send_timeout_);
+
+  // wrong ids - should be blocked
+  send_id.data_frame();
+  send_id.standard();
+  for (uint32_t idx = 0x50U; idx < 0x100U; idx++) {
+    send_id.identifier(static_cast<CanId::IdT>(idx));
+    sender_->send(send_msg, send_id, send_timeout_);
+  }
+
+  // should pass
+  send_id.identifier(static_cast<CanId::IdT>(0x100U));
+  sender_->send(send_msg, send_id, send_timeout_);
+
+  // wrong ids - should be blocked
+  for (uint32_t idx = 0x200U; idx < 0x250U; idx++) {
+    send_id.identifier(static_cast<CanId::IdT>(idx));
+    sender_->send(send_msg, send_id, send_timeout_);
+  }
+
+  // should pass
+  send_id.identifier(static_cast<CanId::IdT>(0x250U));
+  sender_->send(send_msg, send_id, send_timeout_);
+
+  // wrong ids - should be blocked
+  for (uint32_t idx = 0x500U; idx < 0x550U; idx++) {
+    send_id.identifier(static_cast<CanId::IdT>(idx));
+    sender_->send(send_msg, send_id, send_timeout_);
+  }
+
+  // should pass
+  send_id.identifier(static_cast<CanId::IdT>(0x555U));
+  sender_->send(send_msg, send_id, send_timeout_);
+
+  uint32_t receive_msg{};
+  CanId receive_id{};
+  receive_id = receiver_->receive(receive_msg, receive_timeout_);
+  EXPECT_EQ(0x100U, receive_id.get());
+  EXPECT_FALSE(receive_id.is_extended());
+  EXPECT_EQ(receive_id.frame_type(), FrameType::DATA);
+  receive_id = receiver_->receive(receive_msg, receive_timeout_);
+  EXPECT_EQ(0x250U, receive_id.get());
+  EXPECT_FALSE(receive_id.is_extended());
+  EXPECT_EQ(receive_id.frame_type(), FrameType::DATA);
+  receive_id = receiver_->receive(receive_msg, receive_timeout_);
+  EXPECT_EQ(0x555U, receive_id.get());
+  EXPECT_FALSE(receive_id.is_extended());
+  EXPECT_EQ(receive_id.frame_type(), FrameType::DATA);
+
+  ////////////////////////////////////////////////////////////////////////////////
+  // pass only even ids
+  filter_list.filters = {{0x0, 0x1}};
+  receiver_->SetCanFilters(filter_list);
+  send_id.extended();
+  for (uint32_t idx = 0x1000U; idx < 0x1050U; idx++) {
+    send_id.identifier(static_cast<CanId::IdT>(idx));
+    sender_->send(send_msg, send_id, send_timeout_);
+    if (idx % 2 == 0) {
+      receive_id = receiver_->receive(receive_msg, receive_timeout_);
+      EXPECT_EQ(send_id.get(), receive_id.get());
+      EXPECT_EQ(send_id.frame_type(), FrameType::DATA);
+    }
+  }
+
+  ////////////////////////////////////////////////////////////////////////////////
+  // pass none ids
+  filter_list.filters = {};
+  receiver_->SetCanFilters(filter_list);
+  send_id.standard();
+  for (uint32_t idx = 0x300U; idx < 0x330U; idx++) {
+    send_id.identifier(static_cast<CanId::IdT>(idx));
+    sender_->send(send_msg, send_id, send_timeout_);
+  }
+  EXPECT_THROW(receive_id = receiver_->receive(receive_msg, receive_timeout_), std::runtime_error);
+
+  ////////////////////////////////////////////////////////////////////////////////
+  // pass all frames (including errors and remotes)
+  filter_list.filters = {{0x0, 0x0}};
+  filter_list.error_mask = 0xFFFFFFFFU;
+  receiver_->SetCanFilters(filter_list);
+  send_id.standard();
+  for (uint32_t idx = 0x300U; idx < 0x330U; idx++) {
+    send_id.identifier(static_cast<CanId::IdT>(idx));
+    sender_->send(send_msg, send_id, send_timeout_);
+    receive_id = receiver_->receive(receive_msg, receive_timeout_);
+    EXPECT_EQ(send_id.get(), receive_id.get());
+    EXPECT_EQ(send_id.frame_type(), FrameType::DATA);
+  }
+  send_id.error_frame();
+  for (uint32_t idx = 0x200U; idx < 0x230U; idx++) {
+    send_id.identifier(static_cast<CanId::IdT>(idx));
+    sender_->send(send_msg, send_id, send_timeout_);
+    receive_id = receiver_->receive(receive_msg, receive_timeout_);
+    EXPECT_EQ(send_id.get(), receive_id.get());
+    EXPECT_EQ(send_id.frame_type(), FrameType::ERROR);
+  }
+  send_id.remote_frame();
+  for (uint32_t idx = 0x100U; idx < 0x130U; idx++) {
+    send_id.identifier(static_cast<CanId::IdT>(idx));
+    sender_->send(send_msg, send_id, send_timeout_);
+    receive_id = receiver_->receive(receive_msg, receive_timeout_);
+    EXPECT_EQ(send_id.get(), receive_id.get());
+    EXPECT_EQ(send_id.frame_type(), FrameType::REMOTE);
+  }
+
+
+  ////////////////////////////////////////////////////////////////////////////////
+  // JOIN FILTERS: pass only CAN_ERR_TX_TIMEOUT and CAN_ERR_BUSOFF error frames
+  // filter_list.filters = {{0x0, 0x0 | CAN_INV_FILTER}};
+  // filter_list.error_mask = (CAN_ERR_TX_TIMEOUT | CAN_ERR_BUSOFF);
+  // receiver_->SetCanFilters(filter_list);
+  receiver_->SetCanFilters(SocketCanReceiver::CanFilterList("0~0,#41"));  // same as above comment
+
+  // should be blocked
+  for (uint32_t idx = 0x300U; idx < 0x330U; idx++) {
+    send_id.identifier(static_cast<CanId::IdT>(idx));
+    sender_->send(send_msg, send_id, send_timeout_);
+  }
+
+  // should pass
+  send_id.error_frame();
+  send_id.identifier(static_cast<CanId::IdT>(CAN_ERR_TX_TIMEOUT));
+  sender_->send(send_msg, send_id, send_timeout_);
+  receive_id = receiver_->receive(receive_msg, receive_timeout_);
+  EXPECT_EQ(send_id.get(), receive_id.get());
+  EXPECT_EQ(send_id.frame_type(), FrameType::ERROR);
+
+  // should be blocked
+  send_id.identifier(static_cast<CanId::IdT>(CAN_ERR_ACK));
+  sender_->send(send_msg, send_id, send_timeout_);
+
+  // should pass
+  send_id.error_frame();
+  send_id.identifier(static_cast<CanId::IdT>(CAN_ERR_BUSOFF));
+  sender_->send(send_msg, send_id, send_timeout_);
+  receive_id = receiver_->receive(receive_msg, receive_timeout_);
+  EXPECT_EQ(send_id.get(), receive_id.get());
+  EXPECT_EQ(send_id.frame_type(), FrameType::ERROR);
+
+  ////////////////////////////////////////////////////////////////////////////////
+  // JOIN FILTERS: pass only even id data and remote frames from 0x400 to 0x499
+  filter_list.filters = {{0x0, 0x1}, {0x400, 0x700}};
+  filter_list.error_mask = 0;
+  filter_list.join_filters = true;
+  receiver_->SetCanFilters(filter_list);
+
+  send_id.data_frame();
+  send_id.standard();
+
+  // should be blocked
+  for (uint32_t idx = 0x300U; idx < 0x330U; idx++) {
+    send_id.identifier(static_cast<CanId::IdT>(idx));
+    sender_->send(send_msg, send_id, send_timeout_);
+  }
+
+  // only even should pass
+  for (uint32_t idx = 0x400U; idx < 0x500U; idx++) {
+    send_id.identifier(static_cast<CanId::IdT>(idx));
+    sender_->send(send_msg, send_id, send_timeout_);
+    if (idx % 2 == 0) {
+      receive_id = receiver_->receive(receive_msg, receive_timeout_);
+      EXPECT_EQ(send_id.get(), receive_id.get());
+      EXPECT_EQ(send_id.frame_type(), FrameType::DATA);
+    }
+  }
+
+  // only even should pass
+  send_id.remote_frame();
+  for (uint32_t idx = 0x400U; idx < 0x500U; idx++) {
+    send_id.identifier(static_cast<CanId::IdT>(idx));
+    sender_->send(send_msg, send_id, send_timeout_);
+    if (idx % 2 == 0) {
+      receive_id = receiver_->receive(receive_msg, receive_timeout_);
+      EXPECT_EQ(send_id.get(), receive_id.get());
+      EXPECT_EQ(send_id.frame_type(), FrameType::REMOTE);
+    }
+  }
+}

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/test/sanity_checks.cpp

@@ -0,0 +1,238 @@
+// Copyright 2021 the Autoware Foundation
+//
+// 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.
+//
+// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
+
+#include <unistd.h>
+#include <fcntl.h>
+#include <net/if.h>
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+
+#include <linux/can.h>
+#include <linux/can/raw.h>
+
+#include <gtest/gtest.h>
+#include <cstring>
+
+#include <memory>
+#include <string>
+
+#include "ros2_socketcan/socket_can_sender.hpp"
+#include "ros2_socketcan/socket_can_receiver.hpp"
+
+using drivers::socketcan::SocketCanSender;
+using drivers::socketcan::SocketCanReceiver;
+using drivers::socketcan::CanId;
+using drivers::socketcan::StandardFrame;
+using drivers::socketcan::ExtendedFrame;
+using drivers::socketcan::FrameType;
+using drivers::socketcan::MAX_DATA_LENGTH;
+
+
+// Exercise the CanId stuff
+TEST(socket_can_basics, id_bad)
+{
+  // Bad frame type
+  // had to re-write to use lambda to compile properly
+  const auto construct_bad_frame = []() -> auto {
+      constexpr CanId::IdT truncated_id = 0x6000'0000U;
+      return CanId{truncated_id, 0};
+    };
+  EXPECT_THROW(construct_bad_frame(), std::domain_error);
+
+  // Standard truncation
+  const auto construct = [](const auto frame) -> auto {
+      constexpr CanId::IdT truncated_id = 0xFFFF'FFFFU;
+      return CanId{truncated_id, 0, FrameType::DATA, frame};
+    };
+  EXPECT_THROW(construct(StandardFrame), std::domain_error);
+  EXPECT_THROW(construct(ExtendedFrame), std::domain_error);
+}
+
+TEST(socket_can_basics, id)
+{
+  // Default
+  {
+    CanId id{};
+    EXPECT_EQ(id.get(), 0U);
+    EXPECT_FALSE(id.is_extended());
+    EXPECT_EQ(id.frame_type(), FrameType::DATA);
+    // Set to extended
+    id = id.extended();
+    EXPECT_TRUE(id.is_extended());
+    EXPECT_EQ(id.get(), 0x8000'0000U);
+    // Change type to error
+    id = id.error_frame();
+    EXPECT_EQ(id.frame_type(), FrameType::ERROR);
+    EXPECT_EQ(id.get(), 0xA000'0000U);
+    // Change type to remote
+    id = id.remote_frame();
+    EXPECT_EQ(id.frame_type(), FrameType::REMOTE);
+    EXPECT_EQ(id.get(), 0xC000'0000U);
+    // Set to standard
+    id = id.standard();
+    EXPECT_FALSE(id.is_extended());
+    EXPECT_EQ(id.get(), 0x4000'0000U);
+    // Change type to data
+    id = id.data_frame();
+    EXPECT_EQ(id.frame_type(), FrameType::DATA);
+    EXPECT_EQ(id.get(), 0U);
+  }
+}
+
+// Sanity checks on constructor
+TEST(socket_can_basics, bad_constructor)
+{
+  {
+    const std::string long_name{"abcdefghijklmnopqrs"};
+    ASSERT_GE(long_name.size(), 14U);
+    EXPECT_THROW(SocketCanSender{long_name}, std::domain_error);
+    EXPECT_THROW(SocketCanReceiver{long_name}, std::domain_error);
+  }
+  {
+    constexpr auto nonexistent_interface = "foo";
+    EXPECT_THROW(SocketCanSender{nonexistent_interface}, std::runtime_error);
+    EXPECT_THROW(SocketCanReceiver{nonexistent_interface}, std::runtime_error);
+  }
+}
+
+// Requires elevated kernel permissions normal containers can't provide
+class DISABLED_sender_test : public ::testing::Test
+{
+public:
+  using MsgT = uint64_t;
+  static_assert(sizeof(MsgT) == 8U, "Data size is incorrect");
+
+protected:
+  void SetUp()
+  {
+    constexpr auto TEST_INTERFACE = "vcan0";
+    sender_ = std::make_unique<SocketCanSender>(TEST_INTERFACE);
+    // Set up file descriptor
+    file_descriptor_ = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+    fcntl(file_descriptor_, F_SETFL, O_NONBLOCK);
+    struct sockaddr_can addr;
+    struct ifreq ifr;
+
+    strcpy(ifr.ifr_name, TEST_INTERFACE);  // NOLINT literally just copying bytes
+    ioctl(file_descriptor_, SIOCGIFINDEX, &ifr);
+
+    addr.can_family = AF_CAN;
+    addr.can_ifindex = ifr.ifr_ifindex;
+
+    bind(file_descriptor_, (struct sockaddr *)&addr, sizeof(addr));
+  }
+
+  void TearDown()
+  {
+    close(file_descriptor_);
+  }
+
+  uint32_t receive(
+    MsgT & msg,
+    const std::chrono::nanoseconds timeout = std::chrono::milliseconds{1LL})
+  {
+    if (timeout < decltype(timeout)::zero()) {
+      throw std::domain_error{"Negative timeout"};
+    }
+    if (timeout >= std::chrono::seconds{1LL}) {
+      throw std::domain_error{"Timeout >= 1s, not dealing with this"};
+    }
+    // Set up selector
+    {
+      struct timeval c_timeout;
+      c_timeout.tv_sec = 0;
+      c_timeout.tv_usec = std::chrono::duration_cast<std::chrono::milliseconds>(timeout).count();
+
+      fd_set read_set;
+      FD_ZERO(&read_set);
+      FD_SET(file_descriptor_, &read_set);
+      // Wait
+      if (0 == select(file_descriptor_ + 1, &read_set, nullptr, nullptr, &c_timeout)) {
+        throw std::runtime_error{"Timeout"};
+      }
+      if (!FD_ISSET(file_descriptor_, &read_set)) {
+        throw std::runtime_error{"What?"};
+      }
+    }
+    // Read
+    struct can_frame frame;
+    const auto nbytes = read(file_descriptor_, &frame, sizeof(frame));
+    // Checks
+    if (nbytes < 0) {
+      throw std::runtime_error{"CAN raw socket read"};
+      perror("can raw socket read");
+    }
+    if (static_cast<std::size_t>(nbytes) < sizeof(frame)) {
+      throw std::runtime_error{"read: incomplete CAN frame"};
+    }
+    if (static_cast<std::size_t>(nbytes) != sizeof(frame)) {
+      throw std::logic_error{"Message was wrong size"};
+    }
+    // Write
+    (void)std::memcpy(&msg, frame.data, sizeof(msg));
+    return frame.can_id;
+  }
+
+  std::unique_ptr<SocketCanSender> sender_;
+  int file_descriptor_{};
+};  // class sender_test
+
+// Minimal usage
+TEST_F(DISABLED_sender_test, basic_untyped)
+{
+  constexpr MsgT data = 0xA5A5A5A5A5A5A5A5U;
+  // Use untyped interface
+  {
+    EXPECT_THROW(
+      sender_->send(&data, MAX_DATA_LENGTH + 1U, std::chrono::milliseconds{1LL}),
+      std::domain_error
+    );
+    sender_->send(&data, 8U, std::chrono::milliseconds{1LL});
+    MsgT msg{};
+    ASSERT_NE(msg, data);
+    const auto id = receive(msg);
+    EXPECT_EQ(data, msg);
+    EXPECT_EQ(id, sender_->default_id().get());
+  }
+}
+
+TEST_F(DISABLED_sender_test, basic_typed)
+{
+  constexpr MsgT data = 0xA5A5A5A5A5A5A5A5U;
+  // Use typed interface
+  {
+    sender_->send(data, std::chrono::milliseconds{1LL});
+    MsgT msg{};
+    ASSERT_NE(msg, data);
+    const auto id = receive(msg);
+    EXPECT_EQ(data, msg);
+    EXPECT_EQ(id, sender_->default_id().get());
+  }
+}
+
+// Ensure there's no funny stateful stuff happening
+TEST_F(DISABLED_sender_test, sequential)
+{
+  for (MsgT idx = 1UL; idx < 100UL; ++idx) {
+    sender_->send(idx, std::chrono::milliseconds{1LL});
+    MsgT msg{};
+    ASSERT_NE(msg, idx);
+    const auto id = receive(msg);
+    EXPECT_EQ(idx, msg);
+    EXPECT_EQ(id, sender_->default_id().get());
+  }
+}

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/test/vcan0_setup.sh

@@ -0,0 +1,5 @@
+# If running in docker/ade need the following arguments:
+# --privileged --cap-add=ALL -v /lib/modules:/lib/modules
+sudo modprobe vcan
+sudo ip link add dev vcan0 type vcan
+sudo ip link set vcan0 up

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan/test/vcan0_teardown.sh

@@ -0,0 +1 @@
+sudo ip link del vcan0

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan_msgs/CHANGELOG.rst

@@ -0,0 +1,49 @@
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Changelog for package ros2_socketcan_msgs
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+1.3.0 (2024-07-16)
+------------------
+* Jazzy release
+
+1.2.0 (2023-03-03)
+------------------
+* Add CAN FD Support (`#28 <https://github.com/autowarefoundation/ros2_socketcan/issues/28>`_)
+  * Add FD support to ROS2 interface.
+  * Add FD send/receive.
+  * Missed some fields in Frame msg.
+  * Make standard and FD mutually exclusive.
+  * Enable runtime checks for standard vs FD.
+  * Try to minimize API changes.
+  * Fix receive_id/fd_receive_id mix-up.
+  * Make new message FD-specific.
+  * Use FdFrame message.
+  * Remove unused functions.
+  * Always resize fd frame buffer to 64 before receive.
+  * Add enable_can_fd to socket_can_bridge.launch.xml.
+  ---------
+* Adding ros2_socketcan_msgs (`#26 <https://github.com/autowarefoundation/ros2_socketcan/issues/26>`_)
+* Contributors: Joshua Whitley
+
+* Add CAN FD Support (`#28 <https://github.com/autowarefoundation/ros2_socketcan/issues/28>`_)
+  * Add FD support to ROS2 interface.
+  * Add FD send/receive.
+  * Missed some fields in Frame msg.
+  * Make standard and FD mutually exclusive.
+  * Enable runtime checks for standard vs FD.
+  * Try to minimize API changes.
+  * Fix receive_id/fd_receive_id mix-up.
+  * Make new message FD-specific.
+  * Use FdFrame message.
+  * Remove unused functions.
+  * Always resize fd frame buffer to 64 before receive.
+  * Add enable_can_fd to socket_can_bridge.launch.xml.
+  ---------
+* Adding ros2_socketcan_msgs (`#26 <https://github.com/autowarefoundation/ros2_socketcan/issues/26>`_)
+* Contributors: Joshua Whitley
+
+1.1.0 (2022-02-03)
+------------------
+
+1.0.0 (2021-04-01)
+------------------

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan_msgs/CMakeLists.txt

@@ -0,0 +1,28 @@
+cmake_minimum_required(VERSION 3.5)
+project(ros2_socketcan_msgs)
+
+# Default to C++14
+if(NOT CMAKE_CXX_STANDARD)
+  set(CMAKE_CXX_STANDARD 14)
+endif()
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake_auto REQUIRED)
+ament_auto_find_build_dependencies()
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "msg/FdFrame.msg"
+  DEPENDENCIES std_msgs
+  ADD_LINTER_TESTS
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_auto_package()

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan_msgs/CONTRIBUTING.md

@@ -0,0 +1,13 @@
+Any contribution that you make to this repository will
+be under the Apache 2 License, as dictated by that
+[license](http://www.apache.org/licenses/LICENSE-2.0.html):
+
+~~~
+5. Submission of Contributions. Unless You explicitly state otherwise,
+   any Contribution intentionally submitted for inclusion in the Work
+   by You to the Licensor shall be under the terms and conditions of
+   this License, without any additional terms or conditions.
+   Notwithstanding the above, nothing herein shall supersede or modify
+   the terms of any separate license agreement you may have executed
+   with Licensor regarding such Contributions.
+~~~

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan_msgs/LICENSE

@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   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.

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan_msgs/msg/FdFrame.msg

@@ -0,0 +1,6 @@
+std_msgs/Header header
+uint32 id
+bool is_extended
+bool is_error
+uint8 len
+uint8[<=64] data

master/src/Driver/Vehicle/ros2_socketcan/ros2_socketcan_msgs/package.xml

@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>ros2_socketcan_msgs</name>
+  <version>1.3.0</version>
+  <description>Messages for SocketCAN</description>
+  <maintainer email="josh@electrifiedautonomy.com">Josh Whitley</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake_auto</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+
+  <depend>std_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>