add ethercat drive module

HiveCoreR0/cpu26.sh

@@ -0,0 +1,2 @@
+source install/setup.bash
+ros2 launch ethercat_motor_drive motor_drive.launch.py

HiveCoreR0/src/ethercat_driver_ros2/.pre-commit-config.yaml

@@ -0,0 +1,145 @@
+# To use:
+#
+#     pre-commit run -a
+#
+# Or:
+#
+#     pre-commit install  # (runs every time you commit in git)
+#
+# To update this file:
+#
+#     pre-commit autoupdate
+#
+# See https://github.com/pre-commit/pre-commit
+
+repos:
+  # Standard hooks
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.4.0
+    hooks:
+      - id: check-added-large-files
+      - id: check-ast
+      - id: check-case-conflict
+      - id: check-docstring-first
+      - id: check-merge-conflict
+      - id: check-symlinks
+      - id: check-xml
+      - id: check-yaml
+      - id: debug-statements
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+        exclude_types: [rst]
+      - id: fix-byte-order-marker
+
+
+  # Python hooks
+  - repo: https://github.com/asottile/pyupgrade
+    rev: v3.3.1
+    hooks:
+    -   id: pyupgrade
+        args: [--py36-plus]
+
+  # PyDocStyle
+  - repo: https://github.com/PyCQA/pydocstyle
+    rev: 6.3.0
+    hooks:
+    - id: pydocstyle
+      args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
+
+  - repo: https://github.com/pycqa/flake8
+    rev: 6.0.0
+    hooks:
+    - id: flake8
+      args: ["--extend-ignore=E501"]
+
+  # Uncrustify
+  - repo: local
+    hooks:
+      - id: ament_uncrustify
+        name: ament_uncrustify
+        description: Uncrustify.
+        stages: [commit]
+        entry: ament_uncrustify
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+        args: ["--reformat"]
+
+  # CPP hooks
+  - repo: local
+    hooks:
+      - id: clang-format
+        name: clang-format
+        description: Format files with ClangFormat.
+        entry: clang-format-14
+        language: system
+        files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
+        args: ['-fallback-style=none', '-i']
+
+  # - repo: local
+  #   hooks:
+  #     - id: ament_cppcheck
+  #       name: ament_cppcheck
+  #       description: Static code analysis of C/C++ files.
+  #       stages: [commit]
+  #       entry: ament_cppcheck
+  #       language: system
+  #       files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+
+  # Maybe use https://github.com/cpplint/cpplint instead
+  - repo: local
+    hooks:
+      - id: ament_cpplint
+        name: ament_cpplint
+        description: Static code analysis of C/C++ files.
+        stages: [commit]
+        entry: ament_cpplint
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+        args: ["--linelength=100", "--filter=-whitespace/newline"]
+
+  # Cmake hooks
+  - repo: local
+    hooks:
+      - id: ament_lint_cmake
+        name: ament_lint_cmake
+        description: Check format of CMakeLists.txt files.
+        stages: [commit]
+        entry: ament_lint_cmake
+        language: system
+        files: CMakeLists\.txt$
+
+  # Copyright
+  - repo: local
+    hooks:
+      - id: ament_copyright
+        name: ament_copyright
+        description: Check if copyright notice is available in all files.
+        stages: [commit]
+        entry: ament_copyright
+        language: system
+
+  # Docs - RestructuredText hooks
+  - repo: https://github.com/PyCQA/doc8
+    rev: v1.1.1
+    hooks:
+      - id: doc8
+        args: ['--max-line-length=100', '--ignore=D001']
+        exclude: CHANGELOG\.rst$
+
+  - repo: https://github.com/pre-commit/pygrep-hooks
+    rev: v1.10.0
+    hooks:
+      - id: rst-backticks
+        exclude: CHANGELOG\.rst$
+      - id: rst-directive-colons
+      - id: rst-inline-touching-normal
+
+  # Spellcheck in comments and docs
+  # skipping of *.svg files is not working...
+  - repo: https://github.com/codespell-project/codespell
+    rev: v2.2.2
+    hooks:
+      - id: codespell
+        args: ['--write-changes']
+        exclude: CHANGELOG\.rst|\.(svg|pyc)$

HiveCoreR0/src/ethercat_driver_ros2/CONTRIBUTING.md

@@ -0,0 +1,66 @@
+# Contributing Guidelines
+Thank you for your interest in contributing to `ethercat_driver_ros2`. Whether it's a bug report, new feature, correction, or additional documentation, we greatly value feedback and contributions from the community.
+
+Please read through this document before submitting any issues or pull requests to ensure we have all the necessary information to effectively respond to your bug report or contribution.
+
+
+## Reporting Bugs/Feature Requests
+We welcome you to use the GitHub issue tracker to report bugs or suggest features.
+
+When filing an issue, please check [existing open][issues], or [recently closed][closed-issues], issues to make sure somebody else hasn't already reported the issue.
+
+Please try to include as much information as you can. Details like these are incredibly useful:
+
+* A reproducible test case or series of steps
+* The version of our code being used
+* Any modifications you've made relevant to the bug
+* Anything unusual about your environment or deployment
+
+## Installation of pre-commit checks and hooks
+To accelerate the rate of development and to ensure that the code is formatted correctly, we use pre-commit checks and hooks. To install these, please follow the instructions below:
+  1. Install packages used to check the code:
+```bash
+$ sudo apt-get -y install clang-format-10 python3-autopep8 ament-cmake-uncrustify python3-ament-cpplint python3-ament-lint-cmake python3-ament-copyright
+$ pip install doc8
+```
+  2. Install pre-commit: `pip install pre-commit`
+  3. Configure hook such that it runs on every commit: `pre-commit install`
+
+You can run pre-commit manually on the repository to check if the code is formatted correctly.
+To do so, run `pre-commit run --all-files` in the root of the repository.
+
+## Contributing via Pull Requests
+Contributions via pull requests are much appreciated. Before sending us a pull request, please ensure that:
+
+1. You are working against the latest source on the *main* branch.
+2. You check existing open, and recently merged, pull requests to make sure someone else hasn't addressed the problem already.
+3. You have used pre-commit checks to correct any formatting issues.
+4. You open an issue to discuss any significant work - we would hate for your time to be wasted.
+
+**NOTE:** For contributing to the documentation, contribute to the sphinx pages placed in the ethercat_driver_ros2/sphinx folder.
+
+To send us a pull request, please:
+
+1. Fork the repository.
+2. Modify the source; please focus on the specific change you are contributing.
+  If you also reformat all the code, it will be hard for us to focus on your change.
+1. Ensure local tests pass.
+2. Commit to your fork using clear commit messages.
+3. Send a pull request, answering any default questions in the pull request interface.
+4. Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation.
+
+GitHub provides additional documentation on [forking a repository](https://help.github.com/articles/fork-a-repo/) and [creating a pull request](https://help.github.com/articles/creating-a-pull-request/).
+
+
+## Licensing
+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.
+~~~

HiveCoreR0/src/ethercat_driver_ros2/INSTALL.md

@@ -0,0 +1,102 @@
+# Installation
+***Required setup : Ubuntu 22.04 LTS***
+
+## 1. Installing EtherLab
+The proposed development builds upon the [IgH EtherCAT Master](https://etherlab.org/en/ethercat/). Installation steps are summarized here:
+- Install required tools:
+  ```shell
+  $ sudo apt-get update
+  $ sudo apt-get upgrade
+  $ sudo apt-get install git autoconf libtool pkg-config make build-essential net-tools
+  ```
+- Setup sources for the EtherCAT Master:
+  ```shell
+  $ git clone https://gitlab.com/etherlab.org/ethercat.git
+  $ cd ethercat
+  $ git checkout stable-1.5
+  $ sudo rm /usr/bin/ethercat
+  $ sudo rm /etc/init.d/ethercat
+  $ ./bootstrap  # to create the configure script
+  ```
+- Configure, build and install libs and kernel modules:
+  ```shell
+  $ ./configure --prefix=/usr/local/etherlab  --disable-8139too --disable-eoe --enable-generic
+
+  $ make all modules
+  $ sudo make modules_install install
+  $ sudo depmod
+  ```
+  **NOTE**: This step is needed every time the Linux kernel is updated.
+- Configure system:
+  ```shell
+  $ sudo ln -s /usr/local/etherlab/bin/ethercat /usr/bin/
+  $ sudo ln -s /usr/local/etherlab/etc/init.d/ethercat /etc/init.d/ethercat
+  $ sudo mkdir -p /etc/sysconfig
+  $ sudo cp /usr/local/etherlab/etc/sysconfig/ethercat /etc/sysconfig/ethercat
+  ```
+- Create a new `udev` rule:
+  ```shell
+  $ sudo gedit /etc/udev/rules.d/99-EtherCAT.rules
+  ```
+  containing:
+  ```shell
+  KERNEL=="EtherCAT[0-9]*", MODE="0664"
+  ```
+
+- Configure the network adapter for EtherCAT:
+
+  ```shell
+  $ sudo gedit /etc/sysconfig/ethercat
+  ```
+  In the configuration file specify the mac address of the network card to be used and its driver
+  ```shell
+  MASTER0_DEVICE="ff:ff:ff:ff:ff:ff"  # mac address
+  DEVICE_MODULES="generic"
+  ```
+
+Now you can start the EtherCAT master:
+```shell
+$ sudo /etc/init.d/ethercat start
+```
+it should print
+```shell
+Starting EtherCAT master 1.5.2  done
+```
+
+You can check connected slaves:
+```shell
+$ ethercat slaves
+```
+It should print information of connected slave device:
+```shell
+<id>  <alias>:<position>  <device_state>  +  <device_name>
+```
+Example:
+```shell
+0  0:0  PREOP  +  <device_0_name>
+0  0:1  PREOP  +  <device_1_name>
+```
+
+## 2. Building `ethercat_driver_ros2`
+1.  Install `ros2` packages. The current development is based of `ros2 humble`. Installation steps are described [here](https://docs.ros.org/en/humble/Installation.html).
+2. Source your `ros2` environment:
+    ```shell
+    source /opt/ros/humble/setup.bash
+    ```
+    **NOTE**: The ros2 environment needs to be sources in every used terminal. If only one distribution of ros2 is used, it can be added to the `~/.bashrc` file.
+3. Install `colcon` and its extensions :
+    ```shell
+    sudo apt install python3-colcon-common-extensions
+     ```
+3. Create a new ros2 workspace:
+    ```shell
+    mkdir ~/ros2_ws/src
+    ```
+4. Pull relevant packages, install dependencies, compile, and source the workspace by using:
+    ```shell
+    cd ~/ros2_ws
+    git clone https://github.com/ICube-Robotics/ethercat_driver_ros2.git src/ethercat_driver_ros2
+    rosdep install --ignore-src --from-paths . -y -r
+    colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install
+    source install/setup.bash
+    ```

HiveCoreR0/src/ethercat_driver_ros2/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.

HiveCoreR0/src/ethercat_driver_ros2/README.md

@@ -0,0 +1,27 @@
+# ethercat_driver_ros2
+[![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
+[![DOI](https://zenodo.org/badge/491930126.svg)](https://zenodo.org/badge/latestdoi/491930126)
+[![Build](https://github.com/ICube-Robotics/ethercat_driver_ros2/actions/workflows/ci.yml/badge.svg)](https://github.com/ICube-Robotics/ethercat_driver_ros2/actions/workflows/ci.yml)
+
+Implementation of a `Hardware Interface` for simple Ethercat module integration with [`ros2_control`](https://github.com/ros-controls/ros2_control) and building upon [IgH EtherCAT Master for Linux](https://etherlab.org/en/ethercat/).
+
+## About
+[EtherCAT](https://www.ethercat.org/default.htm) provides applications with the capacity of reliable, real-time communication between systems and is therefore a common industrial standard. In order to simplify the development/deployment of new application using EtherCAT modules, the `ethercat_driver` allows to combine them with [`ros2_control`](https://github.com/ros-controls/ros2_control). This driver proposes a generic ways to parametrise and assemble `Hardware Interfaces` based on EtherCAT modules that can be defined using parameter files.
+
+**For more information, please check the [documentation](https://ICube-Robotics.github.io/ethercat_driver_ros2/).**
+
+## Acknowledgments
+Parts of the driver are based on the implementation of [`SimplECAT`](https://bitbucket.org/bsoe/simplecat/src/master/).
+
+## Contacts ##
+![icube](https://icube.unistra.fr/fileadmin/templates/DUN/icube/images/logo.png)
+
+[ICube Laboratory](https://icube.unistra.fr), [University of Strasbourg](https://www.unistra.fr/), France
+
+__Manuel Yguel:__ [yguel@unistra.fr](mailto:yguel@unistra.fr)
+
+__Laurent Barbé:__ [laurent.barbe@unistra.fr](mailto:laurent.barbe@unistra.fr)
+
+__Philippe Zanne:__ [philippe.zanne@unistra.fr](mailto:philippe.zanne@unistra.fr)
+
+__Maciej Bednarczyk:__ [m.bednarczyk@unistra.fr](mailto:m.bednarczyk@unistra.fr), @github: [mcbed](https://github.com/mcbed)

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/CMakeLists.txt

@@ -0,0 +1,71 @@
+cmake_minimum_required(VERSION 3.8)
+project(ethercat_driver)
+
+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 REQUIRED)
+find_package(hardware_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rclcpp_lifecycle REQUIRED)
+find_package(ethercat_interface REQUIRED)
+
+add_library(
+  ${PROJECT_NAME}
+  SHARED
+  src/ethercat_driver.cpp)
+
+target_include_directories(
+  ${PROJECT_NAME}
+  PRIVATE
+  include
+)
+
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  ethercat_interface
+)
+
+# Causes the visibility macros to use dllexport rather than dllimport,
+# which is appropriate when building the dll but not consuming it.
+target_compile_definitions(ethercat_driver PRIVATE "ETHERCAT_DRIVER_BUILDING_LIBRARY")
+# prevent pluginlib from using boost
+target_compile_definitions(${PROJECT_NAME} PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS")
+pluginlib_export_plugin_description_file(hardware_interface ethercat_driver_plugin.xml)
+
+# INSTALL
+install(
+  TARGETS ${PROJECT_NAME}
+  DESTINATION lib
+)
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+if(BUILD_TESTING)
+  find_package(ament_cmake_gtest REQUIRED)
+endif()
+
+## EXPORTS
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+)
+ament_export_dependencies(
+  hardware_interface
+  pluginlib
+  rclcpp
+  rclcpp_lifecycle
+  ethercat_interface
+)
+ament_package()

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/ethercat_driver_plugin.xml

@@ -0,0 +1,9 @@
+<library path="ethercat_driver">
+  <class name="ethercat_driver/EthercatDriver"
+         type="ethercat_driver::EthercatDriver"
+         base_class_type="hardware_interface::SystemInterface">
+    <description>
+        EtherCAT Driver for ros2_control.
+    </description>
+  </class>
+</library>

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp

@@ -0,0 +1,92 @@
+// Copyright 2022 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_
+#define ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_
+
+#include <unordered_map>
+#include <memory>
+#include <string>
+#include <vector>
+#include <pluginlib/class_loader.hpp>
+#include "hardware_interface/handle.hpp"
+#include "hardware_interface/hardware_info.hpp"
+#include "hardware_interface/system_interface.hpp"
+#include "hardware_interface/types/hardware_interface_return_values.hpp"
+#include "rclcpp/macros.hpp"
+#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
+#include "rclcpp_lifecycle/state.hpp"
+#include "ethercat_driver/visibility_control.h"
+#include "ethercat_interface/ec_slave.hpp"
+#include "ethercat_interface/ec_master.hpp"
+
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+
+namespace ethercat_driver
+{
+
+class EthercatDriver : public hardware_interface::SystemInterface
+{
+public:
+  RCLCPP_SHARED_PTR_DEFINITIONS(EthercatDriver)
+
+  ETHERCAT_DRIVER_PUBLIC
+  CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  std::vector<hardware_interface::StateInterface> export_state_interfaces() override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  std::vector<hardware_interface::CommandInterface> export_command_interfaces() override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  hardware_interface::return_type read(const rclcpp::Time &, const rclcpp::Duration &) override;
+
+  ETHERCAT_DRIVER_PUBLIC
+  hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override;
+
+private:
+  std::vector<std::unordered_map<std::string, std::string>> getEcModuleParam(
+    std::string & urdf, std::string component_name, std::string component_type);
+
+  std::vector<std::shared_ptr<ethercat_interface::EcSlave>> ec_modules_;
+  std::vector<std::unordered_map<std::string, std::string>> ec_module_parameters_;
+
+  std::vector<std::vector<double>> hw_joint_commands_;
+  std::vector<std::vector<double>> hw_sensor_commands_;
+  std::vector<std::vector<double>> hw_gpio_commands_;
+  std::vector<std::vector<double>> hw_joint_states_;
+  std::vector<std::vector<double>> hw_sensor_states_;
+  std::vector<std::vector<double>> hw_gpio_states_;
+
+  pluginlib::ClassLoader<ethercat_interface::EcSlave> ec_loader_{
+    "ethercat_interface", "ethercat_interface::EcSlave"};
+
+  int control_frequency_;
+  ethercat_interface::EcMaster master_;
+  std::mutex ec_mutex_;
+  bool activated_;
+};
+}  // namespace ethercat_driver
+
+#endif  // ETHERCAT_DRIVER__ETHERCAT_DRIVER_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/include/ethercat_driver/visibility_control.h

@@ -0,0 +1,49 @@
+// Copyright 2022 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ETHERCAT_DRIVER__VISIBILITY_CONTROL_H_
+#define ETHERCAT_DRIVER__VISIBILITY_CONTROL_H_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+//     https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+  #ifdef __GNUC__
+    #define ETHERCAT_DRIVER_EXPORT __attribute__ ((dllexport))
+    #define ETHERCAT_DRIVER_IMPORT __attribute__ ((dllimport))
+  #else
+    #define ETHERCAT_DRIVER_EXPORT __declspec(dllexport)
+    #define ETHERCAT_DRIVER_IMPORT __declspec(dllimport)
+  #endif
+  #ifdef ETHERCAT_DRIVER_BUILDING_LIBRARY
+    #define ETHERCAT_DRIVER_PUBLIC ETHERCAT_DRIVER_EXPORT
+  #else
+    #define ETHERCAT_DRIVER_PUBLIC ETHERCAT_DRIVER_IMPORT
+  #endif
+  #define ETHERCAT_DRIVER_PUBLIC_TYPE ETHERCAT_DRIVER_PUBLIC
+  #define ETHERCAT_DRIVER_LOCAL
+#else
+  #define ETHERCAT_DRIVER_EXPORT __attribute__ ((visibility("default")))
+  #define ETHERCAT_DRIVER_IMPORT
+  #if __GNUC__ >= 4
+    #define ETHERCAT_DRIVER_PUBLIC __attribute__ ((visibility("default")))
+    #define ETHERCAT_DRIVER_LOCAL  __attribute__ ((visibility("hidden")))
+  #else
+    #define ETHERCAT_DRIVER_PUBLIC
+    #define ETHERCAT_DRIVER_LOCAL
+  #endif
+  #define ETHERCAT_DRIVER_PUBLIC_TYPE
+#endif
+
+#endif  // ETHERCAT_DRIVER__VISIBILITY_CONTROL_H_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/package.xml

@@ -0,0 +1,24 @@
+<?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>ethercat_driver</name>
+  <version>1.2.0</version>
+  <description>EtherCAT Driver for `ros2_control`</description>
+  <maintainer email="mcbed.robotics@gamil.com">Maciej Bednarczyk</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <depend>hardware_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>rclcpp</depend>
+  <depend>rclcpp_lifecycle</depend>
+  <depend>ethercat_interface</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver/src/ethercat_driver.cpp

@@ -0,0 +1,463 @@
+// Copyright 2022 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ethercat_driver/ethercat_driver.hpp"
+
+#include <tinyxml2.h>
+#include <string>
+#include <regex>
+
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
+#include "rclcpp/rclcpp.hpp"
+
+namespace ethercat_driver
+{
+CallbackReturn EthercatDriver::on_init(
+  const hardware_interface::HardwareInfo & info)
+{
+  if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) {
+    return CallbackReturn::ERROR;
+  }
+
+  const std::lock_guard<std::mutex> lock(ec_mutex_);
+  activated_ = false;
+
+  hw_joint_states_.resize(info_.joints.size());
+  for (uint j = 0; j < info_.joints.size(); j++) {
+    hw_joint_states_[j].resize(
+      info_.joints[j].state_interfaces.size(),
+      std::numeric_limits<double>::quiet_NaN());
+  }
+  hw_sensor_states_.resize(info_.sensors.size());
+  for (uint s = 0; s < info_.sensors.size(); s++) {
+    hw_sensor_states_[s].resize(
+      info_.sensors[s].state_interfaces.size(),
+      std::numeric_limits<double>::quiet_NaN());
+  }
+  hw_gpio_states_.resize(info_.gpios.size());
+  for (uint g = 0; g < info_.gpios.size(); g++) {
+    hw_gpio_states_[g].resize(
+      info_.gpios[g].state_interfaces.size(),
+      std::numeric_limits<double>::quiet_NaN());
+  }
+  hw_joint_commands_.resize(info_.joints.size());
+  for (uint j = 0; j < info_.joints.size(); j++) {
+    hw_joint_commands_[j].resize(
+      info_.joints[j].command_interfaces.size(),
+      std::numeric_limits<double>::quiet_NaN());
+  }
+  hw_sensor_commands_.resize(info_.sensors.size());
+  for (uint s = 0; s < info_.sensors.size(); s++) {
+    hw_sensor_commands_[s].resize(
+      info_.sensors[s].command_interfaces.size(),
+      std::numeric_limits<double>::quiet_NaN());
+  }
+  hw_gpio_commands_.resize(info_.gpios.size());
+  for (uint g = 0; g < info_.gpios.size(); g++) {
+    hw_gpio_commands_[g].resize(
+      info_.gpios[g].command_interfaces.size(),
+      std::numeric_limits<double>::quiet_NaN());
+  }
+
+  for (uint j = 0; j < info_.joints.size(); j++) {
+    RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "joints");
+    // check all joints for EC modules and load into ec_modules_
+    auto module_params = getEcModuleParam(info_.original_xml, info_.joints[j].name, "joint");
+    ec_module_parameters_.insert(
+      ec_module_parameters_.end(), module_params.begin(), module_params.end());
+    for (auto i = 0ul; i < module_params.size(); i++) {
+      for (auto k = 0ul; k < info_.joints[j].state_interfaces.size(); k++) {
+        module_params[i]["state_interface/" +
+          info_.joints[j].state_interfaces[k].name] = std::to_string(k);
+      }
+      for (auto k = 0ul; k < info_.joints[j].command_interfaces.size(); k++) {
+        module_params[i]["command_interface/" +
+          info_.joints[j].command_interfaces[k].name] = std::to_string(k);
+      }
+      try {
+        auto module = ec_loader_.createSharedInstance(module_params[i].at("plugin"));
+        if (!module->setupSlave(
+            module_params[i], &hw_joint_states_[j], &hw_joint_commands_[j]))
+        {
+          RCLCPP_FATAL(
+            rclcpp::get_logger("EthercatDriver"),
+            "Setup of Joint module %li FAILED.", i + 1);
+          return CallbackReturn::ERROR;
+        }
+        ec_modules_.push_back(module);
+      } catch (pluginlib::PluginlibException & ex) {
+        RCLCPP_FATAL(
+          rclcpp::get_logger("EthercatDriver"),
+          "The plugin of %s failed to load for some reason. Error: %s\n",
+          info_.joints[j].name.c_str(), ex.what());
+      }
+    }
+  }
+  for (uint g = 0; g < info_.gpios.size(); g++) {
+    RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "gpios");
+    // check all gpios for EC modules and load into ec_modules_
+    auto module_params = getEcModuleParam(info_.original_xml, info_.gpios[g].name, "gpio");
+    ec_module_parameters_.insert(
+      ec_module_parameters_.end(), module_params.begin(), module_params.end());
+    for (auto i = 0ul; i < module_params.size(); i++) {
+      for (auto k = 0ul; k < info_.gpios[g].state_interfaces.size(); k++) {
+        module_params[i]["state_interface/" +
+          info_.gpios[g].state_interfaces[k].name] = std::to_string(k);
+      }
+      for (auto k = 0ul; k < info_.gpios[g].command_interfaces.size(); k++) {
+        module_params[i]["command_interface/" +
+          info_.gpios[g].command_interfaces[k].name] = std::to_string(k);
+      }
+      try {
+        auto module = ec_loader_.createSharedInstance(module_params[i].at("plugin"));
+        if (!module->setupSlave(
+            module_params[i], &hw_gpio_states_[g], &hw_gpio_commands_[g]))
+        {
+          RCLCPP_FATAL(
+            rclcpp::get_logger("EthercatDriver"),
+            "Setup of GPIO module %li FAILED.", i + 1);
+          return CallbackReturn::ERROR;
+        }
+        ec_modules_.push_back(module);
+      } catch (pluginlib::PluginlibException & ex) {
+        RCLCPP_FATAL(
+          rclcpp::get_logger("EthercatDriver"),
+          "The plugin of %s failed to load for some reason. Error: %s\n",
+          info_.gpios[g].name.c_str(), ex.what());
+      }
+    }
+  }
+  for (uint s = 0; s < info_.sensors.size(); s++) {
+    RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "sensors");
+    // check all sensors for EC modules and load into ec_modules_
+    auto module_params = getEcModuleParam(info_.original_xml, info_.sensors[s].name, "sensor");
+    ec_module_parameters_.insert(
+      ec_module_parameters_.end(), module_params.begin(), module_params.end());
+    for (auto i = 0ul; i < module_params.size(); i++) {
+      for (auto k = 0ul; k < info_.sensors[s].state_interfaces.size(); k++) {
+        module_params[i]["state_interface/" +
+          info_.sensors[s].state_interfaces[k].name] = std::to_string(k);
+      }
+      for (auto k = 0ul; k < info_.sensors[s].command_interfaces.size(); k++) {
+        module_params[i]["command_interface/" +
+          info_.sensors[s].command_interfaces[k].name] = std::to_string(k);
+      }
+      try {
+        auto module = ec_loader_.createSharedInstance(module_params[i].at("plugin"));
+        if (!module->setupSlave(
+            module_params[i], &hw_sensor_states_[s], &hw_sensor_commands_[s]))
+        {
+          RCLCPP_FATAL(
+            rclcpp::get_logger("EthercatDriver"),
+            "Setup of Sensor module %li FAILED.", i + 1);
+          return CallbackReturn::ERROR;
+        }
+        ec_modules_.push_back(module);
+      } catch (pluginlib::PluginlibException & ex) {
+        RCLCPP_FATAL(
+          rclcpp::get_logger("EthercatDriver"),
+          "The plugin of %s failed to load for some reason. Error: %s\n",
+          info_.sensors[s].name.c_str(), ex.what());
+      }
+    }
+  }
+
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Got %li modules", ec_modules_.size());
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn EthercatDriver::on_configure(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  return CallbackReturn::SUCCESS;
+}
+
+std::vector<hardware_interface::StateInterface>
+EthercatDriver::export_state_interfaces()
+{
+  std::vector<hardware_interface::StateInterface> state_interfaces;
+  // export joint state interface
+  for (uint j = 0; j < info_.joints.size(); j++) {
+    for (uint i = 0; i < info_.joints[j].state_interfaces.size(); i++) {
+      state_interfaces.emplace_back(
+        hardware_interface::StateInterface(
+          info_.joints[j].name,
+          info_.joints[j].state_interfaces[i].name,
+          &hw_joint_states_[j][i]));
+    }
+  }
+  // export sensor state interface
+  for (uint s = 0; s < info_.sensors.size(); s++) {
+    for (uint i = 0; i < info_.sensors[s].state_interfaces.size(); i++) {
+      state_interfaces.emplace_back(
+        hardware_interface::StateInterface(
+          info_.sensors[s].name,
+          info_.sensors[s].state_interfaces[i].name,
+          &hw_sensor_states_[s][i]));
+    }
+  }
+  // export gpio state interface
+  for (uint g = 0; g < info_.gpios.size(); g++) {
+    for (uint i = 0; i < info_.gpios[g].state_interfaces.size(); i++) {
+      state_interfaces.emplace_back(
+        hardware_interface::StateInterface(
+          info_.gpios[g].name,
+          info_.gpios[g].state_interfaces[i].name,
+          &hw_gpio_states_[g][i]));
+    }
+  }
+  return state_interfaces;
+}
+
+std::vector<hardware_interface::CommandInterface>
+EthercatDriver::export_command_interfaces()
+{
+  std::vector<hardware_interface::CommandInterface> command_interfaces;
+  // export joint command interface
+  std::vector<double> test;
+  for (uint j = 0; j < info_.joints.size(); j++) {
+    for (uint i = 0; i < info_.joints[j].command_interfaces.size(); i++) {
+      command_interfaces.emplace_back(
+        hardware_interface::CommandInterface(
+          info_.joints[j].name,
+          info_.joints[j].command_interfaces[i].name,
+          &hw_joint_commands_[j][i]));
+    }
+  }
+  // export sensor command interface
+  for (uint s = 0; s < info_.sensors.size(); s++) {
+    for (uint i = 0; i < info_.sensors[s].command_interfaces.size(); i++) {
+      command_interfaces.emplace_back(
+        hardware_interface::CommandInterface(
+          info_.sensors[s].name,
+          info_.sensors[s].command_interfaces[i].name,
+          &hw_sensor_commands_[s][i]));
+    }
+  }
+  // export gpio command interface
+  for (uint g = 0; g < info_.gpios.size(); g++) {
+    for (uint i = 0; i < info_.gpios[g].command_interfaces.size(); i++) {
+      command_interfaces.emplace_back(
+        hardware_interface::CommandInterface(
+          info_.gpios[g].name,
+          info_.gpios[g].command_interfaces[i].name,
+          &hw_gpio_commands_[g][i]));
+    }
+  }
+  return command_interfaces;
+}
+
+CallbackReturn EthercatDriver::on_activate(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  const std::lock_guard<std::mutex> lock(ec_mutex_);
+  if (activated_) {
+    RCLCPP_FATAL(rclcpp::get_logger("EthercatDriver"), "Double on_activate()");
+    return CallbackReturn::ERROR;
+  }
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Starting ...please wait...");
+  if (info_.hardware_parameters.find("control_frequency") == info_.hardware_parameters.end()) {
+    control_frequency_ = 100;
+  } else {
+    control_frequency_ = std::stod(info_.hardware_parameters["control_frequency"]);
+  }
+
+  if (control_frequency_ < 0) {
+    RCLCPP_FATAL(
+      rclcpp::get_logger("EthercatDriver"), "Invalid control frequency!");
+    return CallbackReturn::ERROR;
+  }
+
+  // start EC and wait until state operative
+
+  master_.setCtrlFrequency(control_frequency_);
+
+  for (auto i = 0ul; i < ec_modules_.size(); i++) {
+    master_.addSlave(
+      std::stod(ec_module_parameters_[i]["alias"]),
+      std::stod(ec_module_parameters_[i]["position"]),
+      ec_modules_[i].get());
+  }
+
+  // configure SDO
+  for (auto i = 0ul; i < ec_modules_.size(); i++) {
+    for (auto & sdo : ec_modules_[i]->sdo_config) {
+      uint32_t abort_code;
+      int ret = master_.configSlaveSdo(
+        std::stod(ec_module_parameters_[i]["position"]),
+        sdo,
+        &abort_code
+      );
+      if (ret) {
+        RCLCPP_INFO(
+          rclcpp::get_logger("EthercatDriver"),
+          "Failed to download config SDO for module at position %s with Error: %d",
+          ec_module_parameters_[i]["position"].c_str(),
+          abort_code
+        );
+      }
+    }
+  }
+
+  if (!master_.activate()) {
+    RCLCPP_ERROR(rclcpp::get_logger("EthercatDriver"), "Activate EcMaster failed");
+    return CallbackReturn::ERROR;
+  }
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Activated EcMaster!");
+
+  // start after one second
+  struct timespec t;
+  clock_gettime(CLOCK_MONOTONIC, &t);
+  t.tv_sec++;
+
+  bool running = true;
+  while (running) {
+    // wait until next shot
+    clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
+    // update EtherCAT bus
+
+    master_.update();
+    RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!");
+
+    // check if operational
+    bool isAllInit = true;
+    for (auto & module : ec_modules_) {
+      isAllInit = isAllInit && module->initialized();
+    }
+    if (isAllInit) {
+      running = false;
+    }
+    // calculate next shot. carry over nanoseconds into microseconds.
+    t.tv_nsec += master_.getInterval();
+    while (t.tv_nsec >= 1000000000) {
+      t.tv_nsec -= 1000000000;
+      t.tv_sec++;
+    }
+  }
+
+  RCLCPP_INFO(
+    rclcpp::get_logger("EthercatDriver"), "System Successfully started!");
+
+  activated_ = true;
+
+  return CallbackReturn::SUCCESS;
+}
+
+CallbackReturn EthercatDriver::on_deactivate(
+  const rclcpp_lifecycle::State & /*previous_state*/)
+{
+  const std::lock_guard<std::mutex> lock(ec_mutex_);
+  activated_ = false;
+
+  RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Stopping ...please wait...");
+
+  // stop EC and disconnect
+  master_.stop();
+
+  RCLCPP_INFO(
+    rclcpp::get_logger("EthercatDriver"), "System successfully stopped!");
+
+  return CallbackReturn::SUCCESS;
+}
+
+hardware_interface::return_type EthercatDriver::read(
+  const rclcpp::Time & /*time*/,
+  const rclcpp::Duration & /*period*/)
+{
+  // try to lock so we can avoid blocking the read/write loop on the lock.
+  const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
+  if (lock.owns_lock() && activated_) {
+    master_.readData();
+  }
+  return hardware_interface::return_type::OK;
+}
+
+hardware_interface::return_type EthercatDriver::write(
+  const rclcpp::Time & /*time*/,
+  const rclcpp::Duration & /*period*/)
+{
+  // try to lock so we can avoid blocking the read/write loop on the lock.
+  const std::unique_lock<std::mutex> lock(ec_mutex_, std::try_to_lock);
+  if (lock.owns_lock() && activated_) {
+    master_.writeData();
+  }
+  return hardware_interface::return_type::OK;
+}
+
+std::vector<std::unordered_map<std::string, std::string>> EthercatDriver::getEcModuleParam(
+  std::string & urdf,
+  std::string component_name,
+  std::string component_type)
+{
+  // Check if everything OK with URDF string
+  if (urdf.empty()) {
+    throw std::runtime_error("empty URDF passed to robot");
+  }
+  tinyxml2::XMLDocument doc;
+  if (!doc.Parse(urdf.c_str()) && doc.Error()) {
+    throw std::runtime_error("invalid URDF passed in to robot parser");
+  }
+  if (doc.Error()) {
+    throw std::runtime_error("invalid URDF passed in to robot parser");
+  }
+
+  tinyxml2::XMLElement * robot_it = doc.RootElement();
+  if (std::string("robot").compare(robot_it->Name())) {
+    throw std::runtime_error("the robot tag is not root element in URDF");
+  }
+
+  const tinyxml2::XMLElement * ros2_control_it = robot_it->FirstChildElement("ros2_control");
+  if (!ros2_control_it) {
+    throw std::runtime_error("no ros2_control tag");
+  }
+
+  std::vector<std::unordered_map<std::string, std::string>> module_params;
+  std::unordered_map<std::string, std::string> module_param;
+
+  while (ros2_control_it) {
+    const auto * ros2_control_child_it = ros2_control_it->FirstChildElement(component_type.c_str());
+    while (ros2_control_child_it) {
+      if (!component_name.compare(ros2_control_child_it->Attribute("name"))) {
+        const auto * ec_module_it = ros2_control_child_it->FirstChildElement("ec_module");
+        while (ec_module_it) {
+          module_param.clear();
+          module_param["name"] = ec_module_it->Attribute("name");
+          const auto * plugin_it = ec_module_it->FirstChildElement("plugin");
+          if (NULL != plugin_it) {
+            module_param["plugin"] = plugin_it->GetText();
+          }
+          const auto * param_it = ec_module_it->FirstChildElement("param");
+          while (param_it) {
+            module_param[param_it->Attribute("name")] = param_it->GetText();
+            param_it = param_it->NextSiblingElement("param");
+          }
+          module_params.push_back(module_param);
+          ec_module_it = ec_module_it->NextSiblingElement("ec_module");
+        }
+      }
+      ros2_control_child_it = ros2_control_child_it->NextSiblingElement(component_type.c_str());
+    }
+    ros2_control_it = ros2_control_it->NextSiblingElement("ros2_control");
+  }
+
+  return module_params;
+}
+
+}  // namespace ethercat_driver
+
+#include "pluginlib/class_list_macros.hpp"
+
+PLUGINLIB_EXPORT_CLASS(
+  ethercat_driver::EthercatDriver, hardware_interface::SystemInterface)

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 3.8)
+project(ethercat_driver_ros2 NONE)
+find_package(ament_cmake REQUIRED)
+ament_package()

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/doxygen/Doxyfile

@@ -0,0 +1,23 @@
+# All settings not listed here will use the Doxygen default values.
+
+PROJECT_NAME           = "ethercat_driver_ros2"
+PROJECT_NUMBER         = main
+PROJECT_BRIEF          = "C++ ROS test"
+
+# Use these lines to include the generated logging.hpp (update install path if needed)
+INPUT                  = ../../ethercat_driver/include ../../ethercat_interface/include
+
+RECURSIVE              = YES
+OUTPUT_DIRECTORY       = _build
+
+EXTRACT_ALL            = YES
+SORT_MEMBER_DOCS       = NO
+
+GENERATE_LATEX         = NO
+GENERATE_XML           = YES
+
+ENABLE_PREPROCESSING   = YES
+
+DOT_GRAPH_MAX_NODES    = 101
+
+FILE_PATTERNS = *.cpp *.h *.hpp *.md

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/package.xml

@@ -0,0 +1,17 @@
+<?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>ethercat_driver_ros2</name>
+  <version>1.2.0</version>
+  <description>Meta-package aggregating the ethercat_driver_ros2 packages and documentation</description>
+  <maintainer email="mcbed.robotics@gamil.com">Maciej Bednarczyk</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <exec_depend>ethercat_driver</exec_depend>
+  <exec_depend>ethercat_interface</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/requirements.txt

@@ -0,0 +1,7 @@
+sphinx-rtd-theme
+sphinxcontrib-plantuml
+sphinx-mdinclude
+breathe
+exhale
+sphinx-copybutton
+sphinx-tabs

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/Makefile

@@ -0,0 +1,24 @@
+# Minimal makefile for Sphinx documentation
+#
+
+# You can set these variables from the command line, and also
+# from the environment for the first two.
+SPHINXOPTS    ?=
+SPHINXBUILD   ?= sphinx-build
+SOURCEDIR     = .
+BUILDDIR      = _build
+
+# Put it first so that "make" without argument is like "make help".
+help:
+	@$(SPHINXBUILD) -M help "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
+
+.PHONY: help Makefile clean
+
+clean:
+	rm -rf "_doxygen/" "api/"
+	@$(SPHINXBUILD) -M clean "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)
+
+# Catch-all target: route all unknown targets to Sphinx using the new
+# "make mode" option.  $(O) is meant as a shortcut for $(SPHINXOPTS).
+%: Makefile
+	@$(SPHINXBUILD) -M $@ "$(SOURCEDIR)" "$(BUILDDIR)" $(SPHINXOPTS) $(O)

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/_static/css/custom.css

@@ -0,0 +1,21 @@
+.wy-table-responsive table td,
+.wy-table-responsive table th {
+    white-space: normal;
+}
+
+tr {
+    white-space: normal;
+}
+
+thead {
+    white-space: normal;
+}
+
+table {
+    white-space: normal;
+}
+
+pre {
+    white-space: pre-wrap !important;
+    word-break: break-all;
+}

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/conf.py

@@ -0,0 +1,102 @@
+# Copyright 2022 ICube Laboratory, University of Strasbourg
+#
+# 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.
+
+# Configuration file for the Sphinx documentation builder.
+#
+# This file only contains a selection of the most common options. For a full
+# list see the documentation:
+# https://www.sphinx-doc.org/en/master/usage/configuration.html
+
+# -- Path setup --------------------------------------------------------------
+
+# If extensions (or modules to document with autodoc) are in another directory,
+# add these directories to sys.path here. If the directory is relative to the
+# documentation root, use os.path.abspath to make it absolute, like shown here.
+#
+from pathlib import Path
+
+# -- Project information -----------------------------------------------------
+
+project = "ethercat_driver_ros2"
+copyright = "2023, ICUBE Laboratory, University of Strasbourg"
+author = "Maciej Bednarczyk, Philippe Zanne, Laurent Barbé"
+
+# The full version, including alpha/beta/rc tags
+release = "1.0.0"
+
+
+# -- General configuration ---------------------------------------------------
+
+# Add any Sphinx extension module names here, as strings. They can be
+# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom
+# ones.
+extensions = [
+    "sphinx_rtd_theme",
+    "sphinx_mdinclude",
+    "sphinx.ext.mathjax",
+    "sphinx.ext.todo",
+    "sphinx.ext.graphviz",
+    "sphinxcontrib.plantuml",
+    "sphinx_copybutton",
+    "sphinx_tabs.tabs",
+    "breathe",
+]
+
+breathe_default_project = "ethercat_driver_ros2"
+
+
+def get_package(package: str):
+    path = Path(__file__).parent.parent.parent.joinpath(f"{package}/include/{package}")
+    files_gen = path.glob("*.hpp")
+    files = []
+    for file in files_gen:
+        files.append(file.name)
+    return (path, files)
+
+
+breathe_projects = {
+    "ethercat_driver_ros2": "../doxygen/_build/xml/",
+}
+
+
+# Tell sphinx what the primary language being documented is.
+primary_domain = "cpp"
+
+# Tell sphinx what the pygments highlight language should be.
+highlight_language = "cpp"
+
+
+# Add any paths that contain templates here, relative to this directory.
+templates_path = ["_templates"]
+
+# List of patterns, relative to source directory, that match files and
+# directories to ignore when looking for source files.
+# This pattern also affects html_static_path and html_extra_path.
+exclude_patterns = ["_build", "Thumbs.db", ".DS_Store"]
+
+
+# -- Options for HTML output -------------------------------------------------
+
+# The theme to use for HTML and HTML Help pages.  See the documentation for
+# a list of builtin themes.
+#
+html_theme = "sphinx_rtd_theme"
+
+# Add any paths that contain custom static files (such as style sheets) here,
+# relative to this directory. They are copied after the builtin static files,
+# so a file named "default.css" will overwrite the builtin "default.css".
+html_static_path = ["_static"]
+html_logo = "images/logo-icube.png"
+html_css_files = ["css/custom.css"]
+pygments_style = "sphinx"

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/developer_guide/cia402_drive.rst

@@ -0,0 +1,220 @@
+CANopen over EtherCAT for electrical drives
+===========================================
+
+CANopen also defines several device profiles by establishing standard behavior and communication objects for similar devices. In the case of electrical drives, the profile is given by the **CiA402** ("CANopen device profile for drives and motion control") norm compliant with the `IEC <https://webstore.iec.ch/publication/23757>`_ standard that defines programming languages for programmable control systems. The CiA402 norm specifies specific objects in OD in range :code:`0x6000` to :code:`0x67fe`, what guarantees, that for a drive compatible with CiA402, typical process values such as position, velocity, or torque set-points and actual values will always be defined in the same objects with the same address regardless of the manufacturer and by such simplifies the commissioning of different drives. CIA402 also defines a state machine that corresponds to the drive’s actual operating state (:code:`Disabled`, :code:`Fault`, :code:`Switch ON`, etc.). This state machine is controlled and monitored by two mandatory PDO entries: :code:`Control Word` and :code:`Status Word`. The drive cannot be started until the state machine has been put into the appropriate state.
+
+The device states and possible control sequences of the drive are described by the CANopen state machine as follows:
+
+.. image:: https://webhelp.kollmorgen.com/kas/Content/Resources/Images/CoE%20status%20machine.png
+  :width: 400
+  :alt: ecm_lifecycle
+  :align: center
+
+The possible drive states are :
+
+.. list-table:: CiA402 Drive States
+  :widths: 15 35
+  :header-rows: 1
+
+  * - State
+    - Description
+  * - :code:`NOT_READY_TO_SWITCH_ON`
+    - The drive is not ready to switch on. The drive is in boot phase. Drive motion functions are disabled. No communication established.
+  * - :code:`SWITCH_ON_DISABLED`
+    - The drive cannot be enabled via the EtherCAT interface. Drive motion functions are disabled.
+  * - :code:`READY_TO_SWITCH_ON`
+    - The drive can be enabled. Parameters can be transferred. Power may be applied. Drive motion functions are disabled.
+  * - :code:`SWITCHED_ON`
+    - The drive is enabled but idle No fault detected. Power is enabled Drive motion functions are disabled. Parameters can be transferred. No set-points are transferred from the EtherCAT interface.
+  * - :code:`OPERATION_ENABLED`
+    - Normal operation mode No fault detected. Drive motion functions and power are enabled. Power enabled. Set-points are transferred from the EtherCAT interface.
+  * - :code:`QUICK_STOP_ACTIVE`
+    - The quick stop process is executed. Drive motion functions are disabled.  Power is enabled.
+  * - :code:`FAULT_REACTION_ACTIVE`
+    - A fault has occurred, fault reaction processes are executed.
+  * - :code:`FAULT`
+    - A fault is active, Drive is stopped and its functions are disabled.
+
+State transitions are the result of events that can be either internal or triggers through the :code:`Control Word`. When a command trigger for state change is received, the transition is performed before any other command is processed.
+
+The possible transition states, their trigger events and performed actions are :
+
+.. list-table:: CiA402 Drive State Transitions
+  :widths: 1 20 24
+  :header-rows: 1
+
+  * - State Transition
+    - Event
+    - Action
+  * - :code:`0`
+    - **Automatic transition** after power-on or reset application
+    - Drive device self-test and/or self initialization has to be performed
+  * - :code:`1`
+    - **Automatic transition**
+    - Communication has to be activated
+  * - :code:`2`
+    - Shutdown command from control device or local signal
+    - None
+  * - :code:`3`
+    - Switch on command received from control device or local signal
+    - The high-level power has to be switched ON, if possible
+  * - :code:`4`
+    - Enable operation command received from control device or local signal
+    - The drive function has to be enabled. All internal set-points cleared.
+  * - :code:`5`
+    - Disable operation command received from control device or local signal
+    - The drive function has to be disabled.
+  * - :code:`6`
+    - Shutdown command received from control device or local signal
+    - The high-level power has to be switched OFF, if possible.
+  * - :code:`7`
+    - Quick stop or disable voltage command from control device or local signal
+    - None
+  * - :code:`8`
+    - Shutdown command from control device or local signal
+    - The drive function has to be disabled. The high-level power has to be switched OFF, if possible.
+  * - :code:`9`
+    - Disable voltage command from control device or local signal
+    - The drive function has to be disabled. The high-level power has to be switched OFF, if possible.
+  * - :code:`10`
+    - Disable voltage or quick stop command from control device or local signal
+    - The high-level power has to be switched OFF, if possible.
+  * - :code:`11`
+    - Quick stop command from control device or local signal
+    - The quick stop function has to be started.
+  * - :code:`12`
+    - Either: - **Automatic transition** when the quick stop function is completed and quick stop option code is 1, 2, 3 or 4. - Disable voltage command received from control device (depends on the quick stop option code)
+    - The drive function has to be disabled. The high-level power has to be switched OFF, if possible.
+  * - :code:`13`
+    - Fault signal
+    - The configured fault reaction function has to be executed.
+  * - :code:`14`
+    - **Automatic transition**
+    - The drive function has to be disabled. The high-level power has to be switched OFF, if possible.
+  * - :code:`15`
+    - Fault reset command from control device or local signal.
+    - A reset of the fault condition is performed if no fault exists currently on the drive device. After leaving the Fault state, the Fault reset bit in the control word has to be cleared by the control device.
+  * - :code:`16`
+    - If the quick stop option code is 5, 6, 7, or 8, enable operation command from control device
+    - The drive function has to be enabled
+
+CANopen uses 16-bits :code:`Status Word` and :code:`Control Word` to monitor and control the state machine. These PDO entries have the following bit assignment :
+
+.. list-table:: CiA402 Drive Words
+  :widths: 1 20 24
+  :header-rows: 1
+
+  * - Bit
+    - :code:`Status Word` Name
+    - :code:`Control Word` Name
+  * - :code:`0`
+    - Ready to switch on
+    - Switch On
+  * - :code:`1`
+    - Switched on
+    - Disable Voltage
+  * - :code:`2`
+    - Operation Enabled
+    - Quick Stop
+  * - :code:`3`
+    - Fault
+    - Enable Operation
+  * - :code:`4`
+    - Voltage Enabled
+    - Operation-mode Specific
+  * - :code:`5`
+    - Quick Stop
+    - Operation-mode Specific
+  * - :code:`6`
+    - Switch On Disabled
+    - Operation-mode Specific
+  * - :code:`7`
+    - Warning
+    - Reset Fault (only effective for faults)
+  * - :code:`8`
+    - Manufacturer-specific (reserved)
+    - Pause/halt
+  * - :code:`9`
+    - Remote (always 1)
+    - Reserved
+  * - :code:`10`
+    - Target Reached
+    - Reserved
+  * - :code:`11`
+    - Internal Limit Active
+    - Reserved
+  * - :code:`12`
+    - Operation-mode Specific (reserved)
+    - Reserved
+  * - :code:`13`
+    - Operation-mode Specific (reserved)
+    - Manufacturer-specific
+  * - :code:`14`
+    - Manufacturer-specific (reserved)
+    - Manufacturer-specific
+  * - :code:`15`
+    - Manufacturer-specific (reserved)
+    - Manufacturer-specific
+
+.. image:: https://infosys.beckhoff.com/content/1033/ax2000-b110/Images/StateMachine04.gif
+  :width: 700
+  :alt: words
+
+The :code:`Status Word` is only updated and written by the drive in :code:`Safe-Op` and :code:`Operational` states. The current drive state can be decoded from the logical combination of the bits in the :code:`Status Word`:
+
+.. list-table:: CiA402 Drive State form :code:`Status Word`
+  :widths: 50 50
+  :header-rows: 1
+
+  * - :code:`Status Word`
+    - State
+  * - :code:`xxxx xxxx x0xx 0000`
+    - Not ready to switch on
+  * - :code:`xxxx xxxx x1xx 0000`
+    - Switch on disabled
+  * - :code:`xxxx xxxx x01x 0001`
+    - Ready to switch on
+  * - :code:`xxxx xxxx x01x 0011`
+    - Switched on
+  * - :code:`xxxx xxxx x01x 0111`
+    - Operation enabled
+  * - :code:`xxxx xxxx x00x 0111`
+    - Quick stop active
+  * - :code:`xxxx xxxx x0xx 1111`
+    - Fault reaction active
+  * - :code:`xxxx xxxx x0xx 1000`
+    - Fault
+
+The control commands allow the manipulation of the state of a drive by setting its control word. Commands are built up from the logical combination of the bits in the :code:`Control Word`:
+
+.. list-table:: CiA402 Drive Commands to :code:`Control Word`
+  :widths: 25 25 25
+  :header-rows: 1
+
+  * - :code:`Control Word`
+    - Command
+    - State Transitions
+  * - :code:`xxxx 0xxx x110`
+    - Shutdown
+    - 2, 6, 8
+  * - :code:`xxxx 0xxx 0111`
+    - Switch On
+    - 3
+  * - :code:`xxxx 0xxx 1111`
+    - Switch On + Enable Operation
+    - 3 + 4
+  * - :code:`xxxx 0xxx xx0x`
+    - Disable
+    - 7, 9, 10, 12
+  * - :code:`xxxx 0xxx x01x`
+    - Quick Stop
+    - 7, 10, 11
+  * - :code:`xxxx 0xxx 0111`
+    - Disable Operation
+    - 5
+  * - :code:`xxxx 0xxx 1111`
+    - Enable Operation
+    - 4, 16
+  * - :code:`xxxx 1xxx xxxx`
+    - Fault Reset
+    - 15

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/developer_guide/coe.rst

@@ -0,0 +1,37 @@
+CANopen over EtherCAT
+=====================
+
+The CANopen over EtherCAT (CoE) protocol allows devices equipped with CANopen to be used on EtherCAT-based Industrial Ethernet networks.
+
+CANopen
+-------
+
+CANopen is a communication protocol based on the `CAN (Controller Area Network) <https://www.ni.com/en-us/innovations/white-papers/06/controller-area-network--can--overview.html>`_ physical communication standard. In the `OSI communication model <https://www.motioncontroltips.com/what-is-industrial-ethernet-and-how-does-it-differ-from-standard-ethernet/>`_, CAN specifies the physical and data link layers, while CANopen addresses the higher layers — network, transport, session, presentation, and application.
+
+The CANopen protocol defines how automation devices are configured and accessed, and how messages are exchanged between them. CANopen is object-based, meaning that each node (drives, controllers, encoders, I/O, and other devices) in the network has an Object Dictionary (OD), which contains communication objects. These communication objects cover network management data; special functions; acyclic configuration data, which is handled by Service Data Objects (SDOs); cyclic real-time data, which is handled by Process Data Objects (PDOs):
+
+- **Process Data Objects (PDO)** contain OD entries that are cyclically transferred as process variables. Before cyclic communication is started during the configuration phase, particular OD objects are mapped to this structure. Each PDO entry has a defined offset in the exchanged dataset, encapsulated in the Ethernet frame, so that, during the cyclic phase, the slaves’ hardware can locate the relevant data. After cyclic communication is started, PDO entries are exchanged between master and slaves in every cycle and cannot be changed without reconfiguring the communication configuration of the network.
+-  **Service Data Objects (SDO)** contain object dictionary entries that can be exchanged acyclically. SDO works as a mailbox sending and buffering received data. This communication is acyclic and is dependent on available bandwidth in the communication cycle. This communication is not deterministic and is best suited for transmitting configuration data.
+
+The use of object dictionaries, Service Data Objects, and Process Data Objects is a key component of the CANopen protocol, with SDOs being the mechanism for read-write access to the object dictionary.
+
+Every entry of the OD objects is given an index address and sometimes a sub-index sub-address and each OD object consists of 16-bits and data indexes. In this context, addresses between :code:`0x1000` and :code:`0x1fff` contain communication objects, between :code:`0x2000` and :code:`0x5999` manufacturer specific objects and from :code:`0x6000` device profile objects.
+
+CANopen is widely used thanks to its low hardware cost, wide range of device and application profiles, and simple implementation. It’s also extremely reliable and provides real-time communication, making it suitable for industrial applications.
+
+EtherCAT
+--------
+
+EtherCAT is an Industrial Ethernet network. It’s based on standard Ethernet hardware but uses a “processing-on-the-fly” method for transporting and routing messages. In addition to being a real-time networking protocol, EtherCAT is deterministic, meaning it guarantees that a message will be transmitted (or an event will occur) in a specified, predictable period of time — not slower or faster. EtherCAT allows up to 100 meters between nodes (devices) and can provide data transmission rates up to 100 Mbps, with cycle times of less than 100 μs and extremely low jitter, thanks to distributed synchronized clocks.
+
+CANopen over EtherCAT (CoE) protocol
+------------------------------------
+
+CANopen over EtherCAT (CoE) allows the CANopen communication protocol to be implemented over an EtherCAT network, providing a user-friendly, cost-effective solution that provides deterministic data delivery along with faster transmission speeds over longer network lengths.
+
+The use of CANopen over EtherCAT is possible in significant part because EtherCAT implements the same communication system, including object dictionaries, SDOs (the SDO protocol is implemented directly from CANopen, without changes) and PDOs. And on an EtherCAT network, PDO frames are sent deterministically and without the 8-byte limit imposed by CANopen. CANopen over EtherCAT also supports the CANopen device profiles, which specify the parameters and behavior of the device, as well as the device class-specific state machines.
+
+.. image:: https://b2600047.smushcdn.com/2600047/wp-content/uploads/2021/12/Applied-Motion-CANopen-over-EtherCAT.png
+  :width: 400
+  :alt: coe
+  :align: center

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/developer_guide/new_plugin.rst

@@ -0,0 +1,166 @@
+Creating a new device driver
+============================
+
+Creating a new device driver to work with the :code:`ethercat_driver_ros2` is fairly easy. You should do this if you need to create a driver for a specific device or a specific device profile that is not yet supported. If you create a driver for a device profile we are happy to integrate the package into this repository - simply create a PR.
+
+What you need to do
+-------------------
+
+To create a dedicated driver for an EtherCAT module, you need to create a new package containing the module driver plugin and register it so that it can be called by the :code:`ethercat_driver_ros2`.
+
+How to do it
+------------
+
+Create a package
+~~~~~~~~~~~~~~~~
+
+Create a new package using the standard ros2 pkg commands. Make sure you add the following dependencies:
+
+* :code:`ethercat_interface`
+* :code:`pluginlib`
+
+Create your plugin class
+~~~~~~~~~~~~~~~~~~~~~~~~
+
+To be loaded by the :code:`ethercat_driver_ros2`, the new module plugin needs to inherit from the :code:`EcSlave` class and implement some of its virtual functions. To do so create in your package :code:`src` folder a new file :code:`my_ec_device_driver.cpp`:
+
+.. code-block:: cpp
+
+  #include "ethercat_interface/ec_slave.hpp"
+
+  namespace ethercat_plugins
+  {
+
+  class MyEcDeviceDriver : public ethercat_interface::EcSlave
+  {
+  public:
+    MyEcDeviceDriver()
+    : EcSlave(<vendor_id>, <product_id>) {}
+    virtual ~MyEcDeviceDriver() {}
+    // data processing method that will be called cyclically for every channel
+    // the index corresponds to the values registered in domains_
+    virtual void processData(size_t index, uint8_t * domain_address)
+    {
+      // Your process data logic goes here
+    }
+    virtual const ec_sync_info_t * syncs() {return &syncs_[0];}
+    virtual size_t syncSize()
+    {
+      return sizeof(syncs_) / sizeof(ec_sync_info_t);
+    }
+    virtual const ec_pdo_entry_info_t * channels()
+    {
+      return channels_;
+    }
+    virtual void domains(DomainMap & domains) const
+    {
+      domains = domains_;
+    }
+    // configure the slave module with urdf parameters
+    // and link ros2_control command and stat interface
+    virtual bool setupSlave(
+      std::unordered_map<std::string, std::string> slave_paramters,
+      std::vector<double> * state_interface,
+      std::vector<double> * command_interface)
+    {
+      state_interface_ptr_ = state_interface;
+      command_interface_ptr_ = command_interface;
+      paramters_ = slave_paramters;
+
+      // Your module setup logic goes here
+
+      return true;
+    }
+
+  private:
+    // configure module channels
+    ec_pdo_entry_info_t channels_[X] = {
+      {<index>, <sub_index>, <type>},
+    };
+    // configure module pdos
+    ec_pdo_info_t pdos_[X] = {
+      {<index>, <nbr_channels>, <channel_ptr>},
+    };
+    // configure module syncs
+    ec_sync_info_t syncs_[X] = {
+      {<index>, <type>, <nbr_pdos>, <pdos_ptr>, <watchdog>},
+      {0xff}
+    };
+    // configure module domain
+    DomainMap domains_ = {
+      {0, {0}} // index of channels that should call processData()
+    };
+  };
+
+  }  // namespace ethercat_plugins
+
+  #include <pluginlib/class_list_macros.hpp>
+
+  PLUGINLIB_EXPORT_CLASS(ethercat_plugins::MyEcDeviceDriver, ethercat_interface::EcSlave)
+
+Export your plugin
+~~~~~~~~~~~~~~~~~~
+
+In the package root directory create a plugin description file :code:`ethercat_plugins.xml` :
+
+.. code-block:: xml
+
+  <library path="ethercat_plugins">
+    <class name="ethercat_plugins/MyEcDeviceDriver"
+           type="ethercat_plugins::MyEcDeviceDriver"
+           base_class_type="ethercat_interface::EcSlave">
+      <description>Description of the device driver.</description>
+    </class>
+  </library>
+
+Modify your :code:`CMakeLists.txt` file so that it looks like this:
+
+.. code-block:: cmake
+
+  cmake_minimum_required(VERSION 3.8)
+  project(<your_package>)
+
+  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 REQUIRED)
+  find_package(ament_cmake_ros REQUIRED)
+  find_package(ethercat_interface REQUIRED)
+  find_package(pluginlib REQUIRED)
+
+  file(GLOB_RECURSE PLUGINS_SRC src/*.cpp)
+  add_library(${PROJECT_NAME} ${PLUGINS_SRC})
+  target_compile_features(${PROJECT_NAME} PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+  target_include_directories(${PROJECT_NAME} PUBLIC
+    $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+    $<INSTALL_INTERFACE:include>
+  )
+  ament_target_dependencies(
+    ${PROJECT_NAME}
+    "ethercat_interface"
+    "pluginlib"
+  )
+  pluginlib_export_plugin_description_file(ethercat_interface ethercat_plugins.xml)
+  install(
+    DIRECTORY include/
+    DESTINATION include
+  )
+  install(
+    TARGETS ${PROJECT_NAME}
+    EXPORT export_${PROJECT_NAME}
+    ARCHIVE DESTINATION lib
+    LIBRARY DESTINATION lib
+    RUNTIME DESTINATION bin
+  )
+  ament_export_include_directories(
+    include
+  )
+  ament_export_libraries(
+    ethercat_plugins
+  )
+  ament_export_targets(
+    export_${PROJECT_NAME}
+  )
+  ament_package()

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/index.rst

@@ -0,0 +1,35 @@
+EtherCAT Driver ROS2 Stack
+==========================
+
+`EtherCAT <https://www.ethercat.org/default.htm>`_ provides applications with the capacity of reliable, real-time communication between systems and is therefore a common industrial standard. In order to simplify the development/deployment of new application using EtherCAT modules, this stack allows to combine them with `ros2_control <https://github.com/ros-controls/ros2_control>`_. This driver proposes a generic ways to parametrize and assemble Hardware Interfaces based on EtherCAT modules that can be defined using parameter files.
+
+**Project GitHub repository:** `ethercat_driver_ros2 <https://github.com/ICube-Robotics/ethercat_driver_ros2>`_
+
+.. toctree::
+  :maxdepth: 1
+  :caption: Quickstart
+  :glob:
+
+  quickstart/installation
+  quickstart/configuration
+  quickstart/usage
+
+.. toctree::
+  :maxdepth: 1
+  :caption: User Guide
+  :glob:
+
+  user_guide/config_generic_slave
+  user_guide/config_cia402_drive
+  user_guide/config_use_case_motor_with_gear_box
+  user_guide/sdo_async_com
+
+.. toctree::
+  :maxdepth: 1
+  :caption: Developer Guide
+  :glob:
+
+  developer_guide/coe
+  developer_guide/cia402_drive
+  developer_guide/new_plugin
+  API Reference <https://ICube-Robotics.github.io/ethercat_driver_ros2/api/>

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/make.bat

@@ -0,0 +1,35 @@
+@ECHO OFF
+
+pushd %~dp0
+
+REM Command file for Sphinx documentation
+
+if "%SPHINXBUILD%" == "" (
+	set SPHINXBUILD=sphinx-build
+)
+set SOURCEDIR=.
+set BUILDDIR=_build
+
+if "%1" == "" goto help
+
+%SPHINXBUILD% >NUL 2>NUL
+if errorlevel 9009 (
+	echo.
+	echo.The 'sphinx-build' command was not found. Make sure you have Sphinx
+	echo.installed, then set the SPHINXBUILD environment variable to point
+	echo.to the full path of the 'sphinx-build' executable. Alternatively you
+	echo.may add the Sphinx directory to PATH.
+	echo.
+	echo.If you don't have Sphinx installed, grab it from
+	echo.https://www.sphinx-doc.org/
+	exit /b 1
+)
+
+%SPHINXBUILD% -M %1 %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
+goto end
+
+:help
+%SPHINXBUILD% -M help %SOURCEDIR% %BUILDDIR% %SPHINXOPTS% %O%
+
+:end
+popd

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/quickstart/configuration.rst

@@ -0,0 +1,74 @@
+Configuration
+=============
+
+Hardware Interfaces and EtherCAT Master
+---------------------------------------
+
+The EtherCAT Driver is designed in such a way that each EtherCAT Master is defined in :code:`ros2_control` as a specific Hardware Interface. This is done in the :code:`ros2_control` description file, where the EtherCAT driver is loaded as Hardware Interface and linked to an EtherCAT Master by its ID:
+
+.. code-block:: xml
+
+  <ros2_control name="mySystem" type="system">
+    <hardware>
+      <plugin>ethercat_driver/EthercatDriver</plugin>
+      <param name="master_id">0</param>
+      <param name="control_frequency">100</param>
+    </hardware>
+  </ros2_control>
+
+.. note:: As in the current implementation of :code:`ros2_control` there is no information about the system update frequency, it needs to be passed here as parameter. This is only needed by systems that include EtherCAT modules that use the Distributed Clock.
+
+EtherCAT Slave modules as Plugins
+---------------------------------
+
+In this driver, the EtherCAT Slave modules are defined as `Plugins <https://docs.ros.org/en/foxy/Tutorials/Pluginlib.html>`_ and can be parametrized in the :code:`ros2_control` description file :
+
+.. code-block:: xml
+
+  <ec_module name="ECModule">
+    <plugin>ethercat_plugins/ECModule</plugin>
+    <param name="alias">0</param>
+    <param name="position">1</param>
+  </ec_module>
+
+.. note:: All modules have :code:`alias` and :code:`position` parameters that specify their address in the EtherCAT Bus topology. Additional parameters can be specified depending on the purpose of the module.
+
+EtherCAT Slave module plugins come in two version:
+
+* **Generic plugins** : generic module implementation configured using a configuration file which purpose is to facilitate the use of generally available devices. For most applications, the use of these plugins is encouraged.
+* **Specific plugins** : specific implementations for dedicated devices or dedicated functionalities.
+
+Creating components with EtherCAT Slave modules
+-----------------------------------------------
+
+In :code:`ros2_control` the access to resources within a system from a controller is done by means of `Hardware Resources <https://github.com/ros-controls/roadmap/blob/master/design_drafts/hardware_access.md>`_. For this purpose :code:`state_interface` and :code:`command_interface` tags need to be defined and associated with the module functionalities.
+Also, for better understanding of the overall system, the purpose of the used modules need to be clearly identified and sorted into the following types:
+
+* :code:`<joint>`: logical component actuated by at least one actuator with read-write capacity.
+* :code:`<sensor>`: logical component to read-only states from system.
+* :code:`<gpio>`: logical component for general purpose IO systems.
+
+Here is an example of a :code:`gpio` resource built using 2 EtherCAT IO modules:
+
+.. code-block:: xml
+
+  <gpio name="myGPIO">
+    <command_interface name="dig_output.1"/>
+    <command_interface name="dig_output.2"/>
+    <state_interface name="ana_input.1"/>
+    <state_interface name="ana_input.2"/>
+    <ec_module name="EL3104">
+      <plugin>ethercat_generic_plugins/GenericEcSlave</plugin>
+      <param name="alias">0</param>
+      <param name="position">0</param>
+      <param name="slave_config">/path/to/EL3104_slave_config.yaml</param>
+    </ec_module>
+    <ec_module name="EL2008">
+      <plugin>ethercat_generic_plugins/GenericEcSlave</plugin>
+      <param name="alias">0</param>
+      <param name="position">1</param>
+      <param name="slave_config">/path/to/EL2008_slave_config.yaml</param>
+    </ec_module>
+  </gpio>
+
+.. note:: To send commands to :code:`gpio` resources, a generic controller was developed and can be found on this `branch <https://github.com/mcbed/ros2_controllers/tree/gpio_controllers_only>`_.

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/quickstart/installation.rst

@@ -0,0 +1,189 @@
+Installation
+===============================
+
+**Required setup : Ubuntu 22.04 LTS**
+
+Installing EtherLab
+-------------------
+The proposed development builds upon the `IgH EtherCAT Master <https://etherlab.org/en/ethercat/>`_.
+Installation steps are summarized here:
+
+* Verify that you can run unsigned kernel modules
+
+Etherlab is a kernel module that is not signed by default.
+To allow the kernel to load unsigned modules, you need to disable secure boot.
+
+Verify if secure boot is enabled (you need to install ''mokutil'' first):
+  .. code-block:: console
+
+    $ sudo apt-get install mokutil
+    $ mokutil --sb-state
+
+it should print:
+  .. code-block:: console
+
+    SecureBoot disabled
+
+if it prints:
+  .. code-block:: console
+
+    SecureBoot enabled
+
+Then you need to disable secure boot.
+To do so:
+
+  1. reboot your computer and enter the BIOS settings.
+  2. In the security tab, disable secure boot.
+  3. Save and exit.
+
+* Install required tools:
+
+  .. code-block:: console
+
+    $ sudo apt-get update
+    $ sudo apt-get upgrade
+    $ sudo apt-get install git autoconf libtool pkg-config make build-essential net-tools
+
+* Setup sources for the EtherCAT Master:
+
+  .. code-block:: console
+
+    $ git clone https://gitlab.com/etherlab.org/ethercat.git
+    $ cd ethercat
+    $ git checkout stable-1.5
+    $ sudo rm /usr/bin/ethercat
+    $ sudo rm /etc/init.d/ethercat
+    $ ./bootstrap  # to create the configure script
+
+* Configure, build and install libs and kernel modules:
+
+  .. code-block:: console
+
+    $ ./configure --prefix=/usr/local/etherlab  --disable-8139too --disable-eoe --enable-generic
+
+    $ make all modules
+    $ sudo make modules_install install
+    $ sudo depmod
+
+* Configure system:
+
+  .. code-block:: console
+
+    $ sudo ln -s /usr/local/etherlab/bin/ethercat /usr/bin/
+    $ sudo ln -s /usr/local/etherlab/etc/init.d/ethercat /etc/init.d/ethercat
+    $ sudo mkdir -p /etc/sysconfig
+    $ sudo cp /usr/local/etherlab/etc/sysconfig/ethercat /etc/sysconfig/ethercat
+
+  .. note::
+
+    These 4 steps may be needed every time the Linux kernel is updated.
+    Before re-doing the 4 steps, you can try the following lighter steps:
+
+    Go in the folder where the ethercat project was cloned, from step 2 (Setup sources) do:
+
+    .. code-block:: console
+
+      cd ethercat
+      sudo rm /usr/bin/ethercat /etc/init.d/ethercat
+      ./bootstrap
+
+    Do integrally step 3 (Configure, build and install ...)
+    From Step 4 (Configure system)
+
+    .. code-block:: console
+
+     sudo ln -s /usr/local/etherlab/bin/ethercat /usr/bin/
+     sudo ln -s /usr/local/etherlab/etc/init.d/ethercat /etc/init.d/ethercat
+
+
+* Create a new :code:`udev` rule:
+
+  .. code-block:: console
+
+    $ sudo gedit /etc/udev/rules.d/99-EtherCAT.rules
+
+  containing:
+
+  .. code-block:: console
+
+    KERNEL=="EtherCAT[0-9]*", MODE="0666"
+
+
+* Configure the network adapter for EtherCAT:
+
+  .. code-block:: console
+
+    $ sudo gedit /etc/sysconfig/ethercat
+
+  In the configuration file specify the mac address of the network card to be used and its driver
+
+  .. code-block:: console
+
+    MASTER0_DEVICE="ff:ff:ff:ff:ff:ff"  # mac address
+    DEVICE_MODULES="generic"
+
+Now you can start the EtherCAT master:
+
+.. code-block:: console
+
+  $ sudo /etc/init.d/ethercat start
+
+it should print
+
+.. code-block:: console
+
+  Starting EtherCAT master 1.5.2  done
+
+
+You can check connected slaves:
+
+.. code-block:: console
+
+  $ ethercat slaves
+
+It should print information of connected slave device:
+
+.. code-block:: console
+
+  <id>  <alias>:<position>  <device_state>  +  <device_name>
+
+Example:
+
+.. code-block:: console
+
+  0  0:0  PREOP  +  <device_0_name>
+  0  0:1  PREOP  +  <device_1_name>
+
+Building :code:`ethercat_driver_ros2`
+-------------------------------------
+
+1.  Install ROS2 packages. The current development is based of :code:`ros2 humble`. Installation steps are described in the `ROS2 Humble Documentation <https://docs.ros.org/en/humble/Installation.html>`_.
+2. Source your ROS2` environment:
+
+  .. code-block:: console
+
+    source /opt/ros/humble/setup.bash
+
+  .. note:: The ROS2 environment needs to be sources in every used terminal. If only one distribution of ROS2 is used, it can be added to the :code:`~/.bashrc` file.
+
+3. Install :code:`colcon` and its extensions :
+
+  .. code-block:: console
+
+    sudo apt install python3-colcon-common-extensions
+
+4. Create a new ROS2 workspace:
+
+  .. code-block:: console
+
+    mkdir ~/ros2_ws/src
+
+5. Pull relevant packages, install dependencies, compile, and source the workspace by using:
+
+  .. code-block:: console
+
+    cd ~/ros2_ws
+    git clone https://github.com/ICube-Robotics/ethercat_driver_ros2.git src/ethercat_driver_ros2
+    rosdep install --ignore-src --from-paths . -y -r
+    colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install
+    source install/setup.bash

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/quickstart/usage.rst

@@ -0,0 +1,40 @@
+Usage
+=====
+
+Start EtherCAT Master
+---------------------
+
+First start the EtherCAT Master
+
+.. code-block:: console
+
+  $ sudo /etc/init.d/ethercat start
+
+it should print
+
+.. code-block:: console
+
+  Starting EtherCAT master 1.5.2  done
+
+You can check connected slaves:
+
+.. code-block:: console
+
+  $ ethercat slaves
+
+It should print information of connected slave device:
+
+.. code-block:: console
+
+  <id>  <alias>:<position>  <device_state>  +  <device_name>
+
+.. note:: If nothing is displayed or some slave modules are missing, it means that your EtherCAT Master is either not well configured, or that the connection on your field-bus is interrupted.
+
+Launch configuration
+--------------------
+
+Once the EtherCAT Master is running you can go on and launch your configuration package.
+
+.. code-block:: console
+
+  $ ros2 launch [package] [launchfile]

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/user_guide/config_cia402_drive.rst

@@ -0,0 +1,125 @@
+CiA402 EtherCAT Motor Drive configuration
+=========================================
+
+The generic plugin :code:`EcCiA402Drive` is a particular case of the :code:`GenericEcSlave` enabling features compliant with the CiA402 (“CANopen device profile for drives and motion control”) norm. Therefore, the configuration of such a drive uses the same formalism for the :code:`slave_config` file as in the case of the :code:`GenericEcSlave`.
+
+Plugin features
+---------------
+
+* **Drive State transitions**: Management of the motor drive states and their transitions.
+* **Drive Fault reset**: Management of the motor drive fault reset using :code:`command_interface` "reset_fault".
+* **Mode of Operation**: Management of multiple cyclic modes of operation : position (8), velocity (9), effort (10) and homing (6) with the possibility of switch between them.
+* **Default position**: Management of the target position when not controlled.
+
+Configuration options
+---------------------
+
+In addition to the configuration options given by the :code:`GenericEcSlave`, the :code:`EcCiA402Drive` allows to configure the following options in the :code:`slave_config` file:
+
+.. list-table::
+  :widths: 15 35
+  :header-rows: 1
+
+  * - Configuration flag
+    - Description
+  * - :code:`auto_fault_reset`
+    - if set to :code:`true` the drive performs automatic fault reset; if set to :code:`false`, fault reset is only performed on rising edge (0 -> 1) command on the :code:`command_interface` "reset_fault".
+
+Behavior
+--------
+Here are some remarks about the implemented motor drive behavior logic.
+
+After launching the well-configured drive, by default and without fault, motor drive module is brought automatically into the state :code:`OPERATION_ENABLED` making it ready for use. Automatic transition is only enabled when the :code:`control_word` command interface is either missing or set to :code:`NaN` making it possible for the user to take control over the motor drive's state machine by sending corresponding state transition values using the :code:`control_word` command interface.
+
+The default mode of operation of the motor drive can be set either in the configuration yaml file as the default value of the corresponding PDO channel or in urdf using the :code:`mode_of_operation` parameter of the the :code:`EcCiA402Drive`. If both are set, the urdf parameter value overrides the default one from the configuration yaml file.
+
+In order to prevent unwanted movements of the motor, if uncontrolled, the default target position that is send to the drive in all modes of operation is always the last read position. That is why, it is important to send :code:`NaN` in the position command interface when not controlling the motor position. This applies especially for cases when switching between modes.
+
+Usage
+-----
+
+Example configuration for the Maxon EPOS3 motor dive:
+
+..  code-block:: yaml
+  :linenos:
+
+  # Configuration file for Maxon EPOS3 drive
+  vendor_id: 0x000000fb
+  product_id: 0x64400000
+  assign_activate: 0x0300  # DC Synch register
+  auto_fault_reset: false  # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"
+  sdo:  # sdo data to be transferred at drive startup
+    - {index: 0x60C2, sub_index: 1, type: int8, value: 10} # Set interpolation time for cyclic modes to 10 ms
+    - {index: 0x60C2, sub_index: 2, type: int8, value: -3} # Set base 10-3s
+  rpdo:  # RxPDO = receive PDO Mapping
+    - index: 0x1603
+      channels:
+        - {index: 0x6040, sub_index: 0, type: uint16, default: 0}  # Control word
+        - {
+            index: 0x607a,
+            sub_index: 0,
+            type: int32,
+            command_interface: position,
+            default: .nan}  # Target position
+        - {index: 0x60ff, sub_index: 0, type: int32, default: 0}  # Target velocity
+        - {index: 0x6071, sub_index: 0, type: int16, default: 0}  # Target torque
+        - {index: 0x60b0, sub_index: 0, type: int32, default: 0}  # Offset position
+        - {index: 0x60b1, sub_index: 0, type: int32, default: 0}  # Offset velocity
+        - {index: 0x60b2, sub_index: 0, type: int16, default: 0}  # Offset torque
+        - {index: 0x6060, sub_index: 0, type: int8, default: 8}  # Mode of operation
+        - {index: 0x2078, sub_index: 1, type: uint16, default: 0}  # Digital Output Functionalities
+        - {index: 0x60b8, sub_index: 0, type: uint16, default: 0}  # Touch Probe Function
+  tpdo:  # TxPDO = transmit PDO Mapping
+    - index: 0x1a03
+      channels:
+        - {index: 0x6041, sub_index: 0, type: uint16}  # Status word
+        - {
+            index: 0x6064,
+            sub_index: 0,
+            type: int32,
+            state_interface: position
+          }  # Position actual value
+        - {
+            index: 0x606c,
+            sub_index: 0,
+            type: int32,
+            state_interface: velocity
+          }  # Velocity actual value
+        - {
+            index: 0x6077,
+            sub_index: 0,
+            type: int16,
+            state_interface: effort
+          }  # Torque actual value
+        - {index: 0x6061, sub_index: 0, type: int8}  # Mode of operation display
+        - {index: 0x2071, sub_index: 1, type: int16}  # Digital Input Functionalities State
+        - {index: 0x60b9, sub_index: 0, type: int16}  # Touch Probe Status
+        - {index: 0x60ba, sub_index: 0, type: int32}  # Touch Probe Position 1 Positive Value
+        - {index: 0x60bb, sub_index: 0, type: int32}  # Touch Probe Position 1 Negative Value
+
+This configuration can be used for controlling a :code:`joint` component. Here is an example urdf for :code:`ros2_control` using this configuration together with the :code:`EcCiA402Drive` plugin:
+
+.. code-block:: xml
+
+  <ros2_control name="ec_single_axis" type="system">
+      <hardware>
+        <plugin>ethercat_driver/EthercatDriver</plugin>
+        <param name="master_id">0</param>
+        <param name="control_frequency">100</param>
+      </hardware>
+
+     <joint name="joint_0">
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <state_interface name="effort"/>
+        <command_interface name="position"/>
+        <command_interface name="reset_fault"/>
+        <ec_module name="MAXON">
+          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
+          <param name="alias">0</param>
+          <param name="position">0</param>
+          <param name="mode_of_operation">8</param>
+          <param name="slave_config">/path/to/maxon.yaml</param>
+        </ec_module>
+      </joint>
+    </ros2_control>

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/user_guide/config_generic_slave.rst

@@ -0,0 +1,238 @@
+Generic EtherCAT Slave configuration
+====================================
+
+Configuration options
+---------------------
+
+The :code:`GenericEcSlave` allows to configure the following options in the :code:`slave_config` file:
+
+.. list-table::
+  :widths: 15 35
+  :header-rows: 1
+
+  * - Configuration flag
+    - Description
+  * - :code:`vendor_id`
+    - Vendor identification number in hexadecimal format :code:`0x...`.
+  * - :code:`product_id`
+    - Product identification number in hexadecimal format :code:`0x...`.
+  * - :code:`assign_activate`
+    - Distributed Clock Synchronization register in hexadecimal format :code:`0x...`. If not used remove or set to :code:`0x00`.
+  * - :code:`sdo`
+    - SDO data to be transferred at drive startup for configuration purposes.
+  * - :code:`tpdo`
+    - Transmit PDO mapping configuration.
+  * - :code:`rpdo`
+    - Receive PDO mapping configuration.
+  * - :code:`sm`
+    - Sync Manager configuration.
+
+SDO configuration
+~~~~~~~~~~~~~~~~~
+
+Service Data Objects (SDO) are used to setup the module at startup. This is done only one during the activation phase.
+Each SDO has the following configuration flags:
+
+.. list-table::
+  :widths: 15 35
+  :header-rows: 1
+
+  * - SDO flag
+    - Description
+  * - :code:`index`
+    - SDO index in hexadecimal format :code:`0x...`.
+  * - :code:`sub_index`
+    - SDO sub-index in hexadecimal format :code:`0x...`.
+  * - :code:`type`
+    - SDO data type. Possible types: :code:`bool`, :code:`uint8`, :code:`int8`, :code:`uint16`, :code:`int16`, :code:`uint32`, :code:`uint32`, :code:`uint64`, :code:`uint64`, :code:`bitN` with N the number of bits required.
+  * - :code:`value`
+    - Value to be transferred to the module.
+
+PDO mapping configuration
+~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The :code:`tpdo` and :code:`rpdo` Process Data Object (PDO) mapping configurations can be composed of multiple PDO mappings.
+Each PDO mapping requires to specify its register :code:`index` and the configuration of the PDO Channels it is composed of.
+
+PDO channel configuration
+~~~~~~~~~~~~~~~~~~~~~~~~~
+
+Each PDO Channel has the following configuration flags:
+
+.. list-table::
+  :widths: 15 35
+  :header-rows: 1
+
+  * - PDO Channel flag
+    - Description
+  * - :code:`index`
+    - Channel index in hexadecimal format :code:`0x...`.
+  * - :code:`sub_index`
+    - Channel sub-index in hexadecimal format :code:`0x...`.
+  * - :code:`type`
+    - Channel data type. Possible types: :code:`bool`, :code:`uint8`, :code:`int8`, :code:`uint16`, :code:`int16`, :code:`uint32`, :code:`uint32`, :code:`uint64`, :code:`uint64`, :code:`bitN` with N the number of bits required.
+  * - :code:`command_interface`
+    - **Only for** :code:`rpdo`. Name of the command interface to be used inside :code:`ros2_control`.
+  * - :code:`state_interface`
+    - **Only for** :code:`tpdo`. Name of the state interface to be used inside :code:`ros2_control`.
+  * - :code:`default`
+    - **Only for** :code:`rpdo`. Default value to be sent if data received on the command interface is :code:`NaN`.
+  * - :code:`mask`
+    - Data mask, to be used with :code:`type` = :code:`bool`.
+  * - :code:`factor`
+    - Data conversion factor/scale (:code:`type` : :code:`double`).
+  * - :code:`offset`
+    - Data offset term (:code:`type` : :code:`double`).
+
+
+.. warning:: For each channel, tags :code:`index`, :code:`sub_index` and :code:`type` are **mandatory** even if the channel is not used in order to fill the data layout expected by the module. All other tags can remain unset.
+.. note:: Data type :code:`bitN` is used for gaps in the config. Refer to module manual if required.
+.. note:: Data type :code:`bool` requires the use of the :code:`mask` option as the registers can only be read as a multiple of 8 bits.
+
+.. note::
+
+   Data (d) can be modified using the optional :code:`factor` (f) and :code:`offset` (o) parameters following a linear relation: :math:`d \longrightarrow f\times d + o`. Default value are :math:`f=1` and :math:`o=0`. It is particularly useful for:
+
+  - scaling analog values to physical units,
+  - take into account calibration offsets,
+  - convert between different units,
+  - take into account transmission parameters like gear reduction or screw lead for motor control.
+
+Sync Manager Configuration
+~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+A SyncManager protects a DPRAM area from simultaneous access and thus ensures data consistency. For more information, refer to the `Synch Manager EtherCAT documentation <https://infosys.beckhoff.com/english.php?content=../content/1033/tc3_io_intro/4981170059.html&id=>`_.
+Sync Manager can be configured with the following options:
+
+.. list-table::
+  :widths: 15 35
+  :header-rows: 1
+
+  * - Sync Manager flag
+    - Description
+  * - :code:`index`
+    - Sync Manager index.
+  * - :code:`type`
+    - Sync Manager type. Can be either :code:`output` or :code:`input`.
+  * - :code:`pdo`
+    - PDO to be mapped on the Sync Manager. Can be :code:`rpdo`, :code:`tpdo` or :code:`~` if empty.
+  * - :code:`watchdog`
+    - Enables Sync Manager Watchdog. Can be :code:`disable` or :code:`enable`.
+
+.. note:: If :code:`sm` is not specified, the default Sync Manager configuration is :
+
+  .. code-block:: yaml
+
+    sm:  # Sync Manager
+      - {index: 0, type: output, pdo: ~, watchdog: disable}
+      - {index: 1, type: input, pdo: ~, watchdog: disable}
+      - {index: 2, type: output, pdo: rpdo, watchdog: enable}
+      - {index: 3, type: input, pdo: tpdo, watchdog: disable}
+
+Usage
+-----
+
+Example configuration for the Beckhoff EL3104 analog input module:
+
+..  code-block:: yaml
+
+  # Configuration file for Beckhoff EL3104
+  vendor_id: 0x00000002
+  product_id: 0x0c1e3052
+  tpdo:  # TxPDO
+    - index: 0x1a00
+      channels:
+        - {index: 0x3101, sub_index: 1, type: uint8}
+        - {
+            index: 0x3101,
+            sub_index: 2,
+            type: int16,
+            state_interface: analog_input.1,
+            factor: 0.000305185
+          }
+    - index: 0x1a01
+      channels:
+        - {index: 0x3102, sub_index: 1, type: uint8}
+        - {
+            index: 0x3102,
+            sub_index: 2,
+            type: int16,
+            state_interface: analog_input.2,
+            factor: 0.000305185
+          }
+  sm:  # Sync Manager
+    - {index: 0, type: output, pdo: ~, watchdog: disable}
+    - {index: 1, type: input, pdo: ~, watchdog: disable}
+    - {index: 2, type: output, pdo: ~, watchdog: disable}
+    - {index: 3, type: input, pdo: tpdo, watchdog: disable}
+
+Example configuration for the Beckhoff EL2008 digital output module using data type :code:`bool` with :code:`mask`:
+
+.. code-block:: yaml
+
+ # Configuration file for Beckhoff EL2008
+  vendor_id: 0x00000002
+  product_id: 0x07d83052
+  rpdo:  # RxPDO
+    - index: 0x1a00
+      channels:
+        - {index: 0x6000, sub_index: 1, type: bool, mask: 1, command_interface: d_output.1}
+    - index: 0x1a01
+      channels:
+        - {index: 0x6010, sub_index: 1, type: bool}
+    - index: 0x1a02
+      channels:
+        - {index: 0x6020, sub_index: 1, type: bool}
+    - index: 0x1a03
+      channels:
+        - {index: 0x6030, sub_index: 1, type: bool, mask: 8, command_interface: d_output.4}
+    - index: 0x1a04
+      channels:
+        - {index: 0x6040, sub_index: 1, type: bool}
+    - index: 0x1a05
+      channels:
+        - {index: 0x6050, sub_index: 1, type: bool}
+    - index: 0x1a06
+      channels:
+        - {index: 0x6060, sub_index: 1, type: bool}
+    - index: 0x1a07
+      channels:
+        - {index: 0x6070, sub_index: 1, type: bool}
+  sm:  # Sync Manager
+    - {index: 0, type: output, pdo: rpdo, watchdog: enable}
+
+.. note:: In this configuration only digital output 1 and 4 will be used and are therefore configured. The other channels are set up with the mandatory tags :code:`index`, :code:`sub_index` and :code:`type` to fill the data layout expected by the module.
+
+This configuration can be used for controlling a :code:`gpio` component. Here is an example urdf for :code:`ros2_control` using this configuration together with the :code:`GenericEcSlave` plugin:
+
+.. code-block:: xml
+
+  <ros2_control name="ec_single_gpio" type="system">
+      <hardware>
+        <plugin>ethercat_driver/EthercatDriver</plugin>
+        <param name="master_id">0</param>
+        <param name="control_frequency">100</param>
+      </hardware>
+
+     <gpio name="gpio_0">
+        <state_interface name="analog_input.1"/>
+        <state_interface name="analog_input.2"/>
+        <ec_module name="EL3104">
+          <plugin>ethercat_generic_plugins/GenericEcSlave</plugin>
+          <param name="alias">0</param>
+          <param name="position">0</param>
+          <param name="slave_config">/path/to/EL3104_slave_config.yaml</param>
+        </ec_module>
+      </gpio>
+
+      <gpio name="gpio_1">
+        <command_interface name="d_output.1"/>
+        <command_interface name="d_output.4"/>
+        <ec_module name="EL2008">
+          <plugin>ethercat_generic_plugins/GenericEcSlave</plugin>
+          <param name="alias">0</param>
+          <param name="position">1</param>
+          <param name="slave_config">/path/to/EL2008_slave_config.yaml</param>
+        </ec_module>
+      </gpio>
+    </ros2_control>

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/user_guide/config_use_case_motor_with_gear_box.rst

@@ -0,0 +1,286 @@
+Case study: motor coupled to a gearbox and a transmission screw with an encoder
+===============================================================================
+
+This case study is a simple example to show how to set up the config of the :code:`EcCiA402Drive` generic plugin to take into account transmission parameters and encoder parameters.
+
+System configuration
+--------------------
+
+The system is composed of a motor coupled to a transmission line (a gearbox and a transmission screw) whose rotation is measured by an encoder.
+
+
+Transmission configuration
+~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The transmission is composed of a motor, a gearbox, and a transmission screw (lead screw with ball bearing for zero friction) with the following characteristics:
+
+- gearbox with gear ratio, reduction absolute value: 57/13 (:math:`r \triangleq \frac{13}{57}`)
+- gearbox maximum efficiency: 0.84 (:math:`\eta_g \triangleq 0.84`)
+- lead screw with lead: 1 mm (:math:`l \triangleq 1 mm`)
+
+Encoder configuration
+~~~~~~~~~~~~~~~~~~~~~
+
+The encoder has the following characteristics:
+
+- Resolution (number of pulses/revolution):	500 CPR
+- Encoder type: quadrature encoder
+- Digital resolution (number of counts/revolution): :math:`\text{C} \triangleq 500\times 4 = 2000`
+
+Convert state values
+--------------------
+
+The drive will transmit the following state values:
+
+- :code:`position` in :math:`\text{counts}` or :math:`c`
+- :code:`velocity` in :math:`\theta` (revolutions per minute)
+- :code:`torque` (:math:`T_i`) in :math:`\text{mNm}` (torque of the motor at the input of the gearbox)
+
+We would  like to have the following state values:
+
+- :code:`position` (:math:`d`) in :math:`\text{m}`
+- :code:`velocity` (:math:`v`) in :math:`\text{m/s}`
+- :code:`force` (:math:`F_o`) in :math:`\text{mN}` (Force of the lead screw)
+
+This is the configuration part of the tpdo of the :code:`EcCiA402Drive` plugin and corresponding to the :code:`state_interface` in ROS2 control terminology.
+
+
+Linear displacement per count of the encoder
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+The motor has a reductor so each revolution of the motor does not correspond to a revolution of the system.
+It is necessary to first compute the rotation of the shaft at the output of the gearbox.
+
+  * reduction absolute value is :math:`r \triangleq \frac{13}{57}`
+
+So for 57 revolutions of the motor, the shaft will only turn 13 times. So the motor has to turn approximately 4.4 times to make the system turn once.
+
+The lead of the screw is defined as the linear distance traveled per rotation of the screw and in this case is :math:`1mm`.
+
+The encoder reports :math:`C \triangleq 2000` counts per revolution (CPR) so 2000 counts per revolution of the shaft, so an angular resolution of :math:`\frac{360}{2000} = 0.18^{\circ}`.
+
+Let :math:`d` be the linear displacement in meters, :math:`r` the reduction absolute value, :math:`l` the lead of the screw in meters and :math:`C` the counts per revolution of the encoder and :math:`c` the value of the encoder.
+Then, a count :math:`c` of the encoder corresponds to a linear displacement :math:`d` of
+
+.. math::
+
+  \begin{eqnarray}
+    c ~~\text{(in counts)} \longrightarrow d & = & r \times l \times \frac{c}{C} ~~\text{m}\\
+    & = & \frac{13}{57} \times 10^{-3} \frac{c}{2000} ~~\text{m}\\
+    & \approx & 0.000000114 \times c ~~\text{m}
+  \end{eqnarray}
+
+
+The conversion factor is more precisely :math:`0.11403508771929824 \times 10^{-6}`.
+So the displacement resolution, the displacement corresponding to one count of the encoder, is approximately :math:`0.114 µm`.
+
+
+The tpdo part of the configuration of the :code:`EcCiA402Drive` plugin will then be set up with the following values:
+
+..  code-block:: yaml
+
+  tpdo: # TxPDO = transmit PDO Mapping, slave (out) to master (in) (MISO), state values transmitted by the drive to ROS2 control
+    - index: 0x1a03
+      channels:
+        - {
+            index: 0x6064,
+            sub_index: 0,
+            type: int32,
+            state_interface: position,
+            factor: 0.11403508771929824e-6,
+            offset: 0
+          }  # Position actual value
+
+Convert velocity in :math:`\text{m/s}` from :math:`\text{rpm}`
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+If the motor moves at a velocity of :math:`\theta` RPM, then the linear velocity :math:`v` in :math:`\text{m/s}` of the system is given by:
+
+.. math::
+
+  \begin{eqnarray}
+    \theta ~~\text{(in rpm)} \longrightarrow v & = & \frac{13}{57} \times 10^{-3} \times \frac{1}{60} \times \theta ~~ \text{m/s} \\
+    & \approx & 3.8011695906432747 \times 10^{-6} \times \theta ~~\text{m/s}
+  \end{eqnarray}
+
+The tpdo part of the configuration of the :code:`EcCiA402Drive` plugin will then be set up with the following values:
+
+..  code-block:: yaml
+
+  tpdo: # TxPDO = transmit PDO Mapping, slave (out) to master (in) (MISO), state values transmitted by the drive to ROS2 control
+    - index: 0x1a03
+      channels:
+        - {
+            index: 0x606c,
+            sub_index: 0,
+            type: int32,
+            state_interface: velocity,
+            factor: 3.8011695906432747e-6,
+            offset: 0
+          }  # Velocity actual value
+
+Transform torque
+~~~~~~~~~~~~~~~~
+
+The output torque (:math:`T_o`) of the gearbox given the input torque of the motor (:math:`T_i`) is given by the formula:
+
+.. math::
+
+  \begin{eqnarray}
+    T_i \longrightarrow T_o & = & \frac{57}{13} \times \eta_g \times T_i \\
+    & \approx & 3.683076923076923 \times T_i
+  \end{eqnarray}
+
+Torque to Force conversion
+~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The force :math:`F_o` of the lead screw is given by the formula:
+
+.. math::
+
+  \begin{eqnarray}
+    T_o \longrightarrow F_o & = & \frac{2\pi}{l}T_o \\
+  \end{eqnarray}
+
+
+Thus the force :math:`F_o` in :math:`\text{mN}` given the input torque of the motor (:math:`T_i` in :math:`\text{mNm}`) is provided by the formula:
+
+.. math::
+
+  \begin{eqnarray}
+    T_i \longrightarrow F_o & = & \frac{2\pi}{l} \times \frac{57}{13} \times \eta_g \times T_i \\
+    & \approx & 23141.45480828912 \times T_i
+  \end{eqnarray}
+
+
+The tpdo part of the configuration of the :code:`EcCiA402Drive` plugin will then be set up with the following values:
+
+..  code-block:: yaml
+
+  tpdo: # TxPDO = transmit PDO Mapping, slave (out) to master (in) (MISO), state values transmitted by the drive to ROS2 control
+    - index: 0x1a03
+      channels:
+        - {
+            index: 0x6077,
+            sub_index: 0,
+            type: int32,
+            state_interface: effort,
+            factor: 23141.45480828912,
+            offset: 0
+          }  # Force actual value in mN
+
+
+Convert command values
+----------------------
+
+This is the configuration part of the rpdo of the :code:`EcCiA402Drive` plugin and corresponding to the :code:`command_interface` in ROS2 control terminology.
+
+The drive will take the following command values:
+
+- :code:`position` in :math:`\text{counts}` or :math:`c`
+- :code:`velocity` in :math:`\theta` (revolutions per minute)
+- :code:`torque` (:math:`T_i`) in :math:`\text{mNm}` (torque of the motor at the input of the gearbox)
+
+We would  like to send the following command values:
+
+- :code:`position` (:math:`d`) in :math:`\text{m}`
+- :code:`velocity` (:math:`v`) in :math:`\text{m/s}`
+- :code:`force` (:math:`F_o`) in :math:`\text{mN}` (Force of the lead screw)
+
+Command in counts to move by a certain distance in m
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+To command the motor to move by a certain distance :math:`d` in meters, the number of counts :math:`c` to send to the motor is given by the formula:
+
+.. math::
+
+  \begin{equation}
+    d \longrightarrow \text{c} = \frac{C}{rl} \times d \approx 8769.23076923077 \times d
+  \end{equation}
+
+The 'rpdo' part of the configuration of the :code:`EcCiA402Drive` plugin will then be set up with the following values:
+
+
+..  code-block:: yaml
+
+  rpdo: # RxPDO = receive PDO Mapping, master (out) to slave (in) (MOSI), command values received by the drive from ROS2 control
+    - index: 0x1603
+      channels:
+        - {
+            index: 0x607a,
+            sub_index: 0,
+            type: int32,
+            command_interface: position,
+            factor: 8769.23076923077,
+            offset: 0
+          }  # Target position
+
+
+
+Command in rpm to move at a certain velocity given in m/s
+~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+To command the motor to move at a certain velocity in m/s, the number of rpm to send to the motor is given by the formula:
+
+.. math::
+
+  \begin{eqnarray}
+    v ~~\text{(in m/s)}\longrightarrow \theta & = & \frac{60}{rl} \times v ~~\text{(in counts)}\\
+               & \approx  & 263076.92307692306 \times v ~~\text{(in counts)}
+  \end{eqnarray}
+
+The 'rpdo' part of the configuration of the :code:`EcCiA402Drive` plugin will then be set up with the following values:
+
+
+..  code-block:: yaml
+
+  rpdo: # RxPDO = receive PDO Mapping, master (out) to slave (in) (MOSI), command values received by the drive from ROS2 control
+    - index: 0x1603
+      channels:
+        - {
+            index: 0x60ff,
+            sub_index: 0,
+            type: int32,
+            command_interface: velocity,
+            factor: 263076.92307692306,
+            offset: 0
+          }  # Target velocity
+
+Force to torque conversion
+~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+The input torque of the motor (:math:`T_i` in :math:`\text{mNm}`) given the target command force :math:`F_o` in :math:`\text{mN}` is provided by the formula:
+
+.. math::
+
+  \begin{eqnarray}
+    F_o \longrightarrow T_i & = & \frac{l}{2\pi} \times \frac{13}{57} \times \frac{1}{\eta_g} \times F_o \\
+    & \approx & 23141.45480828912 \times F_o
+  \end{eqnarray}
+
+
+The tpdo part of the configuration of the :code:`EcCiA402Drive` plugin will then be set up with the following values:
+
+..  code-block:: yaml
+
+  rpdo: # RxPDO = receive PDO Mapping, master (out) to slave (in) (MOSI), command values received by the drive from ROS2 control
+    - index: 0x1603
+      channels:
+        - {
+            index: 0x6071,
+            sub_index: 0,
+            type: int32,
+            state_interface: effort,
+            factor: 4.321249499153383e-05,
+            offset: 0
+          }  # Target force
+
+
+Complete configuration example for an EPOS4 drive
+-------------------------------------------------
+
+Note that we omit the ``offset`` parameter since their default value is equal to zero.
+
+.. literalinclude:: epos4_config_with_gear_box.yaml
+   :language: yaml
+   :caption: Example of a complete configuration for an EPOS4 drive with a gearbox and a transmission screw
+   :linenos:

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/user_guide/epos4_config_with_gear_box.yaml

@@ -0,0 +1,92 @@
+# Configuration file for Maxon EPOS4 50/5 drive
+vendor_id: 0x000000fb
+product_id: 0x63500000 # Product code
+assign_activate: 0x0300 # DC Synch register
+auto_fault_reset: true # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"
+sdo: # sdo data to be transferred at drive startup
+  - { index: 0x60C2, sub_index: 1, type: int8, value: 10 } # Set interpolation time for cyclic modes to 10 ms
+  - { index: 0x60C2, sub_index: 2, type: int8, value: -3 } # Set base 10-3s
+rpdo: # RxPDO = receive PDO 4 Mapping, master (out) to slave (in) (MOSI), command values received by the drive from ROS2 control
+  - index: 0x1603
+    channels:
+      - {
+          index: 0x6040,
+          sub_index: 0,
+          type: uint16,
+          command_interface: control_word,
+          default: 0,
+        } # Control word
+      - {
+          index: 0x607a,
+          sub_index: 0,
+          type: int32,
+          command_interface: position,
+          factor: 8769.23076923077,
+          default: .nan,
+        } # Target position
+      - {
+          index: 0x60ff,
+          sub_index: 0,
+          type: int32,
+          command_interface: velocity,
+          factor: 263076.92307692306,
+          default: 0,
+        } # Target velocity
+      - {
+          index: 0x6071,
+          sub_index: 0,
+          type: int16,
+          command_interface: effort,
+          factor: 4.321249499153383e-05,
+          default: 0,
+        } # Target torque
+      - { index: 0x60b0, sub_index: 0, type: int32, default: 0 } # Offset position
+      - { index: 0x60b1, sub_index: 0, type: int32, default: 0 } # Offset velocity
+      - { index: 0x60b2, sub_index: 0, type: int16, default: 0 } # Offset torque
+      - {
+          index: 0x6060,
+          sub_index: 0,
+          type: int8,
+          command_interface: mode_of_operation,
+          default: 9,
+        } # Mode of operation. Available modes are: position (8), velocity (9), effort (10) and homing (6)
+tpdo: # TxPDO = transmit PDO 4 Mapping, slave (out) to master (in) (MISO), state values transmitted by the drive to ROS2 control
+  - index: 0x1a03
+    channels:
+      - {
+          index: 0x6041,
+          sub_index: 0,
+          type: uint16,
+          state_interface: status_word,
+        } # Status word
+      - {
+          index: 0x6064,
+          sub_index: 0,
+          type: int32,
+          state_interface: position,
+          factor: 0.11403508771929824e-6,
+        } # Position actual value
+      - {
+          index: 0x606c,
+          sub_index: 0,
+          type: int32,
+          state_interface: velocity,
+          factor: 3.8011695906432747e-6,
+        } # Velocity actual value
+      - {
+          index: 0x6077,
+          sub_index: 0,
+          type: int16,
+          state_interface: effort,
+          factor: 23141.45480828912,
+        } # Force actual value
+      - {
+          index: 0x6061,
+          sub_index: 0,
+          type: int8,
+          state_interface: mode_of_operation,
+        } # Mode of operation display
+      - { index: 0x3141, sub_index: 1, type: int16 } # Digital Input Properties
+      - { index: 0x60b9, sub_index: 0, type: int16 } # Touch Probe Status
+      - { index: 0x60ba, sub_index: 0, type: int32 } # Touch Probe Position 1 Positive edge
+      - { index: 0x60bb, sub_index: 0, type: int32 } # Touch Probe Position 1 Negative edge

HiveCoreR0/src/ethercat_driver_ros2/ethercat_driver_ros2/sphinx/user_guide/sdo_async_com.rst

@@ -0,0 +1,57 @@
+SDO acyclic exchange using ROS2 services
+========================================
+
+Service Data Objects (SDO) contain object dictionary entries that can be exchanged acyclically. SDO works as a mailbox sending and buffering received data. This communication is acyclic and is dependent on available bandwidth in the communication cycle. This communication is not deterministic and is best suited for transmitting configuration data.
+
+Services
+--------
+
+The EtherCAT Driver ROS2 stack comes with a service server that allows the exchange of data with an EtherCAT slave using ROS2 services through SDO.
+The following services are available:
+
+.. list-table::
+  :header-rows: 1
+
+  * - Service
+    - Interface
+    - Description
+  * - :code:`ethercat_manager/get_sdo`
+    - :code:`ethercat_msgs::srv::GetSdo`
+    - Read data from slave SDO register
+  * - :code:`ethercat_manager/set_sdo`
+    - :code:`ethercat_msgs::srv::SetSdo`
+    - Write data to slave SDO register
+
+Interfaces
+----------
+
+The interfaces used by the services are defined such that:
+
++---------------+-------------------------------------+-------------------------------------+
+|               | :code:`ethercat_msgs::srv::GetSdo`  | :code:`ethercat_msgs::srv::SetSdo`  |
++---------------+-------------------------------------+-------------------------------------+
+| **Request**   | .. code-block:: shell               | .. code-block:: shell               |
+|               |                                     |                                     |
+|               |   int16 master_id                   |   int16 master_id                   |
+|               |   uint16 slave_position             |   int16 slave_position              |
+|               |   uint16 sdo_index                  |   uint16 sdo_index                  |
+|               |   uint8 sdo_subindex                |   uint8 sdo_subindex                |
+|               |   string sdo_data_type              |   string sdo_data_type              |
+|               |                                     |   string sdo_value                  |
++---------------+-------------------------------------+-------------------------------------+
+| **Response**  | .. code-block:: shell               | .. code-block:: shell               |
+|               |                                     |                                     |
+|               |   bool success                      |   bool success                      |
+|               |   string sdo_return_message         |   string sdo_return_message         |
+|               |   string sdo_return_value_string    |                                     |
+|               |   float64 sdo_return_value          |                                     |
++---------------+-------------------------------------+-------------------------------------+
+
+Usage
+-----
+
+Build and source the :code:`ethercat_manager` package and run:
+
+.. code-block:: shell
+
+  $ ros2 run ethercat_manager ethercat_sdo_srv_server

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/CMakeLists.txt

@@ -0,0 +1,89 @@
+cmake_minimum_required(VERSION 3.8)
+project(ethercat_generic_cia402_drive)
+
+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 REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(ethercat_interface REQUIRED)
+find_package(ethercat_generic_slave REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(yaml_cpp_vendor REQUIRED)
+
+file(GLOB_RECURSE PLUGINS_SRC src/*.cpp)
+add_library(ethercat_generic_cia402_drive ${PLUGINS_SRC})
+target_compile_features(ethercat_generic_cia402_drive PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+target_include_directories(ethercat_generic_cia402_drive PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(
+  ethercat_generic_cia402_drive
+  ethercat_interface
+  ethercat_generic_slave
+  pluginlib
+  yaml_cpp_vendor
+)
+
+pluginlib_export_plugin_description_file(ethercat_interface ethercat_plugins.xml)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  TARGETS ethercat_generic_cia402_drive
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(pluginlib REQUIRED)
+  find_package(ethercat_interface REQUIRED)
+  find_package(ethercat_generic_slave REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  # Test Load EtherCAT modules
+  ament_add_gmock(
+    test_load_generic_plugins
+    test/test_load_ec_modules.cpp
+  )
+  target_include_directories(test_load_generic_plugins PRIVATE include)
+  ament_target_dependencies(test_load_generic_plugins
+    pluginlib
+    ethercat_interface
+  )
+
+  # Test Generic EtherCAT CIA402 Drive Plugin
+  ament_add_gmock(
+    test_generic_ec_cia402_drive
+    test/test_generic_ec_cia402_drive.cpp
+  )
+  target_include_directories(test_generic_ec_cia402_drive PRIVATE include)
+  target_link_libraries(test_generic_ec_cia402_drive
+    ethercat_generic_cia402_drive
+  )
+  ament_target_dependencies(test_generic_ec_cia402_drive
+    pluginlib
+    ethercat_interface
+    ethercat_generic_slave
+  )
+endif()
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ethercat_generic_cia402_drive
+)
+ament_export_targets(
+  export_${PROJECT_NAME}
+)
+
+ament_package()

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/ethercat_plugins.xml

@@ -0,0 +1,5 @@
+<library path="ethercat_generic_cia402_drive">
+  <class name="ethercat_generic_plugins/EcCiA402Drive" type="ethercat_generic_plugins::EcCiA402Drive" base_class_type="ethercat_interface::EcSlave">
+    <description>Generic EtherCAT CiA402 Drive.</description>
+  </class>
+</library>

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/cia402_common_defs.hpp

@@ -0,0 +1,71 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ETHERCAT_GENERIC_PLUGINS__CIA402_COMMON_DEFS_HPP_
+#define ETHERCAT_GENERIC_PLUGINS__CIA402_COMMON_DEFS_HPP_
+
+#define CiA402D_RPDO_CONTROLWORD  ((uint16_t) 0x6040)
+#define CiA402D_RPDO_POSITION  ((uint16_t) 0x607a)
+#define CiA402D_RPDO_VELOCITY  ((uint16_t) 0x60ff)
+#define CiA402D_RPDO_EFFORT  ((uint16_t) 0x6071)
+#define CiA402D_RPDO_MODE_OF_OPERATION  ((uint16_t) 0x6060)
+
+#define CiA402D_TPDO_POSITION ((uint16_t) 0x6064)
+#define CiA402D_TPDO_STATUSWORD  ((uint16_t) 0x6041)
+#define CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY  ((uint16_t) 0x6061)
+
+#include <map>
+#include <string>
+
+enum DeviceState
+{
+  STATE_UNDEFINED = 0,
+  STATE_START = 1,
+  STATE_NOT_READY_TO_SWITCH_ON,
+  STATE_SWITCH_ON_DISABLED,
+  STATE_READY_TO_SWITCH_ON,
+  STATE_SWITCH_ON,
+  STATE_OPERATION_ENABLED,
+  STATE_QUICK_STOP_ACTIVE,
+  STATE_FAULT_REACTION_ACTIVE,
+  STATE_FAULT
+};
+
+enum ModeOfOperation
+{
+  MODE_NO_MODE                = 0,
+  MODE_PROFILED_POSITION      = 1,
+  MODE_PROFILED_VELOCITY      = 3,
+  MODE_PROFILED_TORQUE        = 4,
+  MODE_HOMING                 = 6,
+  MODE_INTERPOLATED_POSITION  = 7,
+  MODE_CYCLIC_SYNC_POSITION   = 8,
+  MODE_CYCLIC_SYNC_VELOCITY  = 9,
+  MODE_CYCLIC_SYNC_TORQUE     = 10
+};
+
+const std::map<DeviceState, std::string> DEVICE_STATE_STR = {
+  {STATE_START, "Start"},
+  {STATE_NOT_READY_TO_SWITCH_ON, "Not Ready to Switch On"},
+  {STATE_SWITCH_ON_DISABLED, "Switch on Disabled"},
+  {STATE_READY_TO_SWITCH_ON, "Ready to Switch On"},
+  {STATE_SWITCH_ON, "Switch On"},
+  {STATE_OPERATION_ENABLED, "Operation Enabled"},
+  {STATE_QUICK_STOP_ACTIVE, "Quick Stop Active"},
+  {STATE_FAULT_REACTION_ACTIVE, "Fault Reaction Active"},
+  {STATE_FAULT, "Fault"},
+  {STATE_UNDEFINED, "Undefined State"}
+};
+
+#endif  // ETHERCAT_GENERIC_PLUGINS__CIA402_COMMON_DEFS_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp

@@ -0,0 +1,79 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// 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.
+//
+// Author: Maciej Bednarczyk (macbednarczyk@gmail.com)
+
+#ifndef ETHERCAT_GENERIC_PLUGINS__GENERIC_EC_CIA402_DRIVE_HPP_
+#define ETHERCAT_GENERIC_PLUGINS__GENERIC_EC_CIA402_DRIVE_HPP_
+
+#include <vector>
+#include <string>
+#include <unordered_map>
+#include <limits>
+
+#include "yaml-cpp/yaml.h"
+#include "ethercat_interface/ec_slave.hpp"
+#include "ethercat_interface/ec_pdo_channel_manager.hpp"
+#include "ethercat_generic_plugins/generic_ec_slave.hpp"
+#include "ethercat_generic_plugins/cia402_common_defs.hpp"
+
+namespace ethercat_generic_plugins
+{
+
+class EcCiA402Drive : public GenericEcSlave
+{
+public:
+  EcCiA402Drive();
+  virtual ~EcCiA402Drive();
+  /** Returns true if drive has reached "operation enabled" state.
+   *  The transition through the state machine is handled automatically. */
+  bool initialized() const;
+
+  virtual void processData(size_t index, uint8_t * domain_address);
+
+  virtual bool setupSlave(
+    std::unordered_map<std::string, std::string> slave_paramters,
+    std::vector<double> * state_interface,
+    std::vector<double> * command_interface);
+
+  int8_t mode_of_operation_display_ = 0;
+  int8_t mode_of_operation_ = -1;
+
+protected:
+  uint32_t counter_ = 0;
+  uint16_t last_status_word_ = -1;
+  uint16_t status_word_ = 0;
+  uint16_t control_word_ = 0;
+  DeviceState last_state_ = STATE_START;
+  DeviceState state_ = STATE_START;
+  bool initialized_ = false;
+  bool auto_fault_reset_ = false;
+  bool auto_state_transitions_ = true;
+  bool fault_reset_ = false;
+  int fault_reset_command_interface_index_ = -1;
+  bool last_fault_reset_command_ = false;
+  double last_position_ = std::numeric_limits<double>::quiet_NaN();
+
+  /** returns device state based upon the status_word */
+  DeviceState deviceState(uint16_t status_word);
+  /** returns the control word that will take device from state to next desired state */
+  uint16_t transition(DeviceState state, uint16_t control_word);
+  /** set up of the drive configuration from yaml node*/
+  bool setup_from_config(YAML::Node drive_config);
+  /** set up of the drive configuration from yaml file*/
+  bool setup_from_config_file(std::string config_file);
+};
+}  // namespace ethercat_generic_plugins
+
+#endif  // ETHERCAT_GENERIC_PLUGINS__GENERIC_EC_CIA402_DRIVE_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/package.xml

@@ -0,0 +1,23 @@
+<?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>ethercat_generic_cia402_drive</name>
+  <version>1.2.0</version>
+  <description>Plugin implementations of generic EtherCAT modules for CiA402 drives.</description>
+  <maintainer email="mcbed.robotics@gamil.com">Maciej Bednarczyk</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <depend>ethercat_interface</depend>
+  <depend>ethercat_generic_slave</depend>
+  <depend>pluginlib</depend>
+  <depend>yaml_cpp_vendor</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp

@@ -0,0 +1,232 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// 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.
+//
+// Author: Maciej Bednarczyk (macbednarczyk@gmail.com)
+
+#include <numeric>
+
+#include "ethercat_generic_plugins/generic_ec_cia402_drive.hpp"
+
+namespace ethercat_generic_plugins
+{
+
+EcCiA402Drive::EcCiA402Drive()
+: GenericEcSlave() {}
+EcCiA402Drive::~EcCiA402Drive() {}
+
+bool EcCiA402Drive::initialized() const {return initialized_;}
+
+void EcCiA402Drive::processData(size_t index, uint8_t * domain_address)
+{
+  // Special case: ControlWord
+  if (pdo_channels_info_[index].index == CiA402D_RPDO_CONTROLWORD) {
+    if (is_operational_) {
+      if (fault_reset_command_interface_index_ >= 0) {
+        if (command_interface_ptr_->at(fault_reset_command_interface_index_) == 0) {
+          last_fault_reset_command_ = false;
+        }
+        if (last_fault_reset_command_ == false &&
+          command_interface_ptr_->at(fault_reset_command_interface_index_) != 0 &&
+          !std::isnan(command_interface_ptr_->at(fault_reset_command_interface_index_)))
+        {
+          last_fault_reset_command_ = true;
+          fault_reset_ = true;
+        }
+      }
+
+      if (auto_state_transitions_) {
+        pdo_channels_info_[index].default_value = transition(
+          state_,
+          pdo_channels_info_[index].ec_read(domain_address));
+      }
+    }
+  }
+
+  // setup current position as default position
+  if (pdo_channels_info_[index].index == CiA402D_RPDO_POSITION) {
+    if (mode_of_operation_display_ != ModeOfOperation::MODE_NO_MODE) {
+      pdo_channels_info_[index].default_value =
+        pdo_channels_info_[index].factor * last_position_ +
+        pdo_channels_info_[index].offset;
+    }
+    pdo_channels_info_[index].override_command =
+      (mode_of_operation_display_ != ModeOfOperation::MODE_CYCLIC_SYNC_POSITION) ? true : false;
+  }
+
+  // setup mode of operation
+  if (pdo_channels_info_[index].index == CiA402D_RPDO_MODE_OF_OPERATION) {
+    if (mode_of_operation_ >= 0 && mode_of_operation_ <= 10) {
+      pdo_channels_info_[index].default_value = mode_of_operation_;
+    }
+  }
+
+  pdo_channels_info_[index].ec_update(domain_address);
+
+  // get mode_of_operation_display_
+  if (pdo_channels_info_[index].index == CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY) {
+    mode_of_operation_display_ = pdo_channels_info_[index].last_value;
+  }
+
+  if (pdo_channels_info_[index].index == CiA402D_TPDO_POSITION) {
+    last_position_ = pdo_channels_info_[index].last_value;
+  }
+
+  // Special case: StatusWord
+  if (pdo_channels_info_[index].index == CiA402D_TPDO_STATUSWORD) {
+    status_word_ = pdo_channels_info_[index].last_value;
+  }
+
+
+  // CHECK FOR STATE CHANGE
+  if (index == all_channels_.size() - 1) {  // if last entry  in domain
+    if (status_word_ != last_status_word_) {
+      state_ = deviceState(status_word_);
+      if (state_ != last_state_) {
+        std::cout << "STATE: " << DEVICE_STATE_STR.at(state_)
+                  << " with status word :" << status_word_ << std::endl;
+      }
+    }
+    initialized_ = ((state_ == STATE_OPERATION_ENABLED) &&
+      (last_state_ == STATE_OPERATION_ENABLED)) ? true : false;
+
+    last_status_word_ = status_word_;
+    last_state_ = state_;
+    counter_++;
+  }
+}
+
+bool EcCiA402Drive::setupSlave(
+  std::unordered_map<std::string, std::string> slave_paramters,
+  std::vector<double> * state_interface,
+  std::vector<double> * command_interface)
+{
+  state_interface_ptr_ = state_interface;
+  command_interface_ptr_ = command_interface;
+  paramters_ = slave_paramters;
+
+  if (paramters_.find("slave_config") != paramters_.end()) {
+    if (!setup_from_config_file(paramters_["slave_config"])) {
+      return false;
+    }
+  } else {
+    std::cerr << "EcCiA402Drive: failed to find 'slave_config' tag in URDF." << std::endl;
+    return false;
+  }
+
+  setup_interface_mapping();
+  setup_syncs();
+
+  if (paramters_.find("mode_of_operation") != paramters_.end()) {
+    mode_of_operation_ = std::stod(paramters_["mode_of_operation"]);
+  }
+
+  if (paramters_.find("command_interface/reset_fault") != paramters_.end()) {
+    fault_reset_command_interface_index_ = std::stoi(paramters_["command_interface/reset_fault"]);
+  }
+
+  return true;
+}
+
+bool EcCiA402Drive::setup_from_config(YAML::Node drive_config)
+{
+  if (!GenericEcSlave::setup_from_config(drive_config)) {return false;}
+  // additional configuration parameters for CiA402 Drives
+  if (drive_config["auto_fault_reset"]) {
+    auto_fault_reset_ = drive_config["auto_fault_reset"].as<bool>();
+  }
+  if (drive_config["auto_state_transitions"]) {
+    auto_state_transitions_ = drive_config["auto_state_transitions"].as<bool>();
+  }
+  return true;
+}
+
+bool EcCiA402Drive::setup_from_config_file(std::string config_file)
+{
+  // Read drive configuration from YAML file
+  try {
+    slave_config_ = YAML::LoadFile(config_file);
+  } catch (const YAML::ParserException & ex) {
+    std::cerr << "EcCiA402Drive: failed to load drive configuration: " << ex.what() << std::endl;
+    return false;
+  } catch (const YAML::BadFile & ex) {
+    std::cerr << "EcCiA402Drive: failed to load drive configuration: " << ex.what() << std::endl;
+    return false;
+  }
+  if (!setup_from_config(slave_config_)) {
+    return false;
+  }
+  return true;
+}
+
+/** returns device state based upon the status_word */
+DeviceState EcCiA402Drive::deviceState(uint16_t status_word)
+{
+  if ((status_word & 0b01001111) == 0b00000000) {
+    return STATE_NOT_READY_TO_SWITCH_ON;
+  } else if ((status_word & 0b01001111) == 0b01000000) {
+    return STATE_SWITCH_ON_DISABLED;
+  } else if ((status_word & 0b01101111) == 0b00100001) {
+    return STATE_READY_TO_SWITCH_ON;
+  } else if ((status_word & 0b01101111) == 0b00100011) {
+    return STATE_SWITCH_ON;
+  } else if ((status_word & 0b01101111) == 0b00100111) {
+    return STATE_OPERATION_ENABLED;
+  } else if ((status_word & 0b01101111) == 0b00000111) {
+    return STATE_QUICK_STOP_ACTIVE;
+  } else if ((status_word & 0b01001111) == 0b00001111) {
+    return STATE_FAULT_REACTION_ACTIVE;
+  } else if ((status_word & 0b01001111) == 0b00001000) {
+    return STATE_FAULT;
+  }
+  return STATE_UNDEFINED;
+}
+
+/** returns the control word that will take device from state to next desired state */
+uint16_t EcCiA402Drive::transition(DeviceState state, uint16_t control_word)
+{
+  switch (state) {
+    case STATE_START:                     // -> STATE_NOT_READY_TO_SWITCH_ON (automatic)
+      return control_word;
+    case STATE_NOT_READY_TO_SWITCH_ON:    // -> STATE_SWITCH_ON_DISABLED (automatic)
+      return control_word;
+    case STATE_SWITCH_ON_DISABLED:        // -> STATE_READY_TO_SWITCH_ON
+      return (control_word & 0b01111110) | 0b00000110;
+    case STATE_READY_TO_SWITCH_ON:        // -> STATE_SWITCH_ON
+      return (control_word & 0b01110111) | 0b00000111;
+    case STATE_SWITCH_ON:                 // -> STATE_OPERATION_ENABLED
+      return (control_word & 0b01111111) | 0b00001111;
+    case STATE_OPERATION_ENABLED:         // -> GOOD
+      return control_word;
+    case STATE_QUICK_STOP_ACTIVE:         // -> STATE_OPERATION_ENABLED
+      return (control_word & 0b01111111) | 0b00001111;
+    case STATE_FAULT_REACTION_ACTIVE:     // -> STATE_FAULT (automatic)
+      return control_word;
+    case STATE_FAULT:                     // -> STATE_SWITCH_ON_DISABLED
+      if (auto_fault_reset_ || fault_reset_) {
+        fault_reset_ = false;
+        return (control_word & 0b11111111) | 0b10000000;     // automatic reset
+      } else {
+        return control_word;
+      }
+    default:
+      break;
+  }
+  return control_word;
+}
+
+}  // namespace ethercat_generic_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(ethercat_generic_plugins::EcCiA402Drive, ethercat_interface::EcSlave)

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp

@@ -0,0 +1,318 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <map>
+#include <limits>
+#include <pluginlib/class_loader.hpp>
+#include "ethercat_interface/ec_slave.hpp"
+#include "test_generic_ec_cia402_drive.hpp"
+
+const char test_drive_config[] =
+  R"(
+# Configuration file for Test drive
+vendor_id: 0x00000011
+product_id: 0x07030924
+assign_activate: 0x0321  # DC Synch register
+period: 100  # Hz
+auto_fault_reset: false  # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"
+sdo:  # sdo data to be transferred at drive startup
+  - {index: 0x60C2, sub_index: 1, type: int8, value: 10} # Set interpolation time for cyclic modes to 10 ms
+  - {index: 0x60C2, sub_index: 2, type: int8, value: -3} # Set base 10-3s
+rpdo:  # RxPDO
+  - index: 0x1607
+    channels:
+      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan}  # Target position
+      - {index: 0x60ff, sub_index: 0, type: int32, command_interface: velocity, default: 0}  # Target velocity
+      - {index: 0x6071, sub_index: 0, type: int16, command_interface: effort, default: -5}  # Target torque
+      - {index: 0x6072, sub_index: 0, type: int16, command_interface: ~, default: 1000}  # Max torque
+      - {index: 0x6040, sub_index: 0, type: uint16, command_interface: ~, default: 0}  # Control word
+      - {index: 0x6060, sub_index: 0, type: int8, command_interface: mode_of_operation, default: 8}  # Mode of operation
+tpdo:  # TxPDO
+  - index: 0x1a07
+    channels:
+      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position}  # Position actual value
+      - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity}  # Velocity actual value
+      - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort}  # Torque actual value
+      - {index: 0x6041, sub_index: 0, type: uint16, state_interface: ~}  # Status word
+      - {index: 0x6061, sub_index: 0, type: int8, state_interface: mode_of_operation}  # Mode of operation display
+  - index: 0x1a45
+    channels:
+      - {index: 0x2205, sub_index: 1, type: int16, state_interface: analog_input1}  # Analog input
+      - {index: 0x2205, sub_index: 2, type: int16, state_interface: analog_input2}  # Analog input
+)";
+
+void EcCiA402DriveTest::SetUp()
+{
+  plugin_ = std::make_unique<FriendEcCiA402Drive>();
+}
+
+void EcCiA402DriveTest::TearDown()
+{
+  plugin_.reset(nullptr);
+}
+
+TEST_F(EcCiA402DriveTest, SlaveSetupNoDriveConfig)
+{
+  SetUp();
+  std::vector<double> state_interface = {0};
+  std::vector<double> command_interface = {0};
+  std::unordered_map<std::string, std::string> slave_paramters;
+  // setup failed, 'drive_config' parameter not set
+  ASSERT_EQ(
+    plugin_->setupSlave(
+      slave_paramters,
+      &state_interface,
+      &command_interface
+    ),
+    false
+  );
+}
+
+TEST_F(EcCiA402DriveTest, SlaveSetupMissingFileDriveConfig)
+{
+  SetUp();
+  std::vector<double> state_interface = {0};
+  std::vector<double> command_interface = {0};
+  std::unordered_map<std::string, std::string> slave_paramters;
+  slave_paramters["drive_config"] = "drive_config.yaml";
+  // setup failed, 'drive_config.yaml' file not set
+  ASSERT_EQ(
+    plugin_->setupSlave(
+      slave_paramters,
+      &state_interface,
+      &command_interface
+    ),
+    false
+  );
+}
+
+TEST_F(EcCiA402DriveTest, SlaveSetupDriveFromConfig)
+{
+  SetUp();
+  ASSERT_EQ(
+    plugin_->setup_from_config(YAML::Load(test_drive_config)),
+    true
+  );
+  ASSERT_EQ(plugin_->vendor_id_, 0x00000011);
+  ASSERT_EQ(plugin_->product_id_, 0x07030924);
+  ASSERT_EQ(plugin_->assign_activate_, 0x0321);
+  ASSERT_EQ(plugin_->auto_fault_reset_, false);
+
+  ASSERT_EQ(plugin_->rpdos_.size(), 1);
+  ASSERT_EQ(plugin_->rpdos_[0].index, 0x1607);
+
+  ASSERT_EQ(plugin_->tpdos_.size(), 2);
+  ASSERT_EQ(plugin_->tpdos_[0].index, 0x1a07);
+  ASSERT_EQ(plugin_->tpdos_[1].index, 0x1a45);
+
+  ASSERT_EQ(plugin_->pdo_channels_info_[1].interface_name, "velocity");
+  ASSERT_EQ(plugin_->pdo_channels_info_[3].default_value, 1000);
+  ASSERT_TRUE(std::isnan(plugin_->pdo_channels_info_[0].default_value));
+  ASSERT_EQ(plugin_->pdo_channels_info_[4].interface_name, "null");
+  ASSERT_EQ(plugin_->pdo_channels_info_[12].interface_name, "analog_input2");
+  ASSERT_EQ(plugin_->pdo_channels_info_[4].data_type, "uint16");
+}
+
+TEST_F(EcCiA402DriveTest, SlaveSetupPdoChannels)
+{
+  SetUp();
+  plugin_->setup_from_config(YAML::Load(test_drive_config));
+  std::vector<ec_pdo_entry_info_t> channels(
+    plugin_->channels(),
+    plugin_->channels() + plugin_->all_channels_.size()
+  );
+
+  ASSERT_EQ(channels.size(), 13);
+  ASSERT_EQ(channels[0].index, 0x607a);
+  ASSERT_EQ(channels[11].index, 0x2205);
+  ASSERT_EQ(channels[11].subindex, 0x01);
+}
+
+TEST_F(EcCiA402DriveTest, SlaveSetupSyncs)
+{
+  SetUp();
+  plugin_->setup_from_config(YAML::Load(test_drive_config));
+  plugin_->setup_syncs();
+  std::vector<ec_sync_info_t> syncs(
+    plugin_->syncs(),
+    plugin_->syncs() + plugin_->syncSize()
+  );
+
+  ASSERT_EQ(syncs.size(), 5);
+  ASSERT_EQ(syncs[1].index, 1);
+  ASSERT_EQ(syncs[1].dir, EC_DIR_INPUT);
+  ASSERT_EQ(syncs[1].n_pdos, 0);
+  ASSERT_EQ(syncs[1].watchdog_mode, EC_WD_DISABLE);
+  ASSERT_EQ(syncs[2].dir, EC_DIR_OUTPUT);
+  ASSERT_EQ(syncs[2].n_pdos, 1);
+  ASSERT_EQ(syncs[3].index, 3);
+  ASSERT_EQ(syncs[3].dir, EC_DIR_INPUT);
+  ASSERT_EQ(syncs[3].n_pdos, 2);
+  ASSERT_EQ(syncs[3].watchdog_mode, EC_WD_DISABLE);
+}
+
+TEST_F(EcCiA402DriveTest, SlaveSetupDomains)
+{
+  SetUp();
+  plugin_->setup_from_config(YAML::Load(test_drive_config));
+  std::map<unsigned int, std::vector<unsigned int>> domains;
+  plugin_->domains(domains);
+
+  ASSERT_EQ(domains[0].size(), 13);
+  ASSERT_EQ(domains[0][0], 0);
+  ASSERT_EQ(domains[0][12], 12);
+}
+
+TEST_F(EcCiA402DriveTest, EcReadTPDOToStateInterface)
+{
+  SetUp();
+  std::unordered_map<std::string, std::string> slave_paramters;
+  std::vector<double> state_interface = {0, 0};
+  plugin_->state_interface_ptr_ = &state_interface;
+  slave_paramters["state_interface/effort"] = "1";
+  plugin_->paramters_ = slave_paramters;
+  plugin_->setup_from_config(YAML::Load(test_drive_config));
+  plugin_->setup_interface_mapping();
+  ASSERT_EQ(plugin_->pdo_channels_info_[8].interface_index, 1);
+  uint8_t domain_address[2];
+  EC_WRITE_S16(domain_address, 42);
+  plugin_->processData(8, domain_address);
+  ASSERT_EQ(plugin_->state_interface_ptr_->at(1), 42);
+}
+
+TEST_F(EcCiA402DriveTest, EcWriteRPDOFromCommandInterface)
+{
+  SetUp();
+  std::unordered_map<std::string, std::string> slave_paramters;
+  std::vector<double> command_interface = {0, 42};
+  plugin_->command_interface_ptr_ = &command_interface;
+  slave_paramters["command_interface/effort"] = "1";
+  plugin_->paramters_ = slave_paramters;
+  plugin_->setup_from_config(YAML::Load(test_drive_config));
+  plugin_->setup_interface_mapping();
+  ASSERT_EQ(plugin_->pdo_channels_info_[2].interface_index, 1);
+  plugin_->mode_of_operation_display_ = 10;
+  uint8_t domain_address[2];
+  plugin_->processData(2, domain_address);
+  ASSERT_EQ(plugin_->pdo_channels_info_[2].last_value, 42);
+  ASSERT_EQ(EC_READ_S16(domain_address), 42);
+}
+
+TEST_F(EcCiA402DriveTest, EcWriteRPDODefaultValue)
+{
+  SetUp();
+  plugin_->setup_from_config(YAML::Load(test_drive_config));
+  plugin_->setup_interface_mapping();
+  plugin_->mode_of_operation_display_ = 10;
+  uint8_t domain_address[2];
+  plugin_->processData(2, domain_address);
+  ASSERT_EQ(plugin_->pdo_channels_info_[2].last_value, -5);
+  ASSERT_EQ(EC_READ_S16(domain_address), -5);
+}
+
+// TEST_F(EcCiA402DriveTest, FaultReset)
+// {
+//   std::unordered_map<std::string, std::string> slave_paramters;
+//   std::vector<double> command_interface = {0, 1};
+//   plugin_->command_interface_ptr_ = &command_interface;
+//   plugin_->setup_from_config(YAML::Load(test_drive_config));
+//   plugin_->setup_interface_mapping();
+//   plugin_->fault_reset_command_interface_index_ = 1;
+//   plugin_->state_ = STATE_FAULT;
+//   plugin_->is_operational_ = true;
+//   uint8_t domain_address = 0;
+//   plugin_->pdo_channels_info_[4].data_type = "";
+//   ASSERT_FALSE(plugin_->last_fault_reset_command_);
+//   ASSERT_FALSE(plugin_->fault_reset_);
+//   ASSERT_EQ(plugin_->command_interface_ptr_->at(
+//     plugin_->fault_reset_command_interface_index_), 1);
+//   plugin_->processData(4, &domain_address);
+//   ASSERT_EQ(plugin_->pdo_channels_info_[4].default_value, 0b10000000);
+//   plugin_->pdo_channels_info_[4].last_value = 0;
+//   plugin_->processData(4, &domain_address);
+//   ASSERT_EQ(plugin_->pdo_channels_info_[4].default_value, 0b00000000);
+//   command_interface[1] = 0;
+//   plugin_->processData(4, &domain_address);
+//   ASSERT_EQ(plugin_->pdo_channels_info_[4].default_value, 0b00000000);
+//   command_interface[1] = 2;  plugin_->processData(4, &domain_address);
+//   ASSERT_EQ(plugin_->pdo_channels_info_[4].default_value, 0b10000000);
+// }
+
+TEST_F(EcCiA402DriveTest, SwitchModeOfOperation)
+{
+  std::unordered_map<std::string, std::string> slave_paramters;
+  std::vector<double> command_interface = {
+    std::numeric_limits<double>::quiet_NaN(),
+    std::numeric_limits<double>::quiet_NaN()};
+  slave_paramters["command_interface/mode_of_operation"] = "1";
+  plugin_->paramters_ = slave_paramters;
+  plugin_->command_interface_ptr_ = &command_interface;
+  plugin_->setup_from_config(YAML::Load(test_drive_config));
+  plugin_->setup_interface_mapping();
+  plugin_->is_operational_ = true;
+  uint8_t domain_address[2];
+  plugin_->processData(5, domain_address);
+  ASSERT_EQ(EC_READ_S8(domain_address), 8);
+  command_interface[1] = 9;
+  plugin_->processData(5, domain_address);
+  plugin_->processData(10, domain_address);
+  ASSERT_EQ(EC_READ_S8(domain_address), 9);
+  ASSERT_EQ(plugin_->mode_of_operation_display_, 9);
+}
+
+TEST_F(EcCiA402DriveTest, EcWriteDefaultTargetPosition)
+{
+  std::unordered_map<std::string, std::string> slave_paramters;
+  std::vector<double> command_interface = {
+    std::numeric_limits<double>::quiet_NaN(),
+    std::numeric_limits<double>::quiet_NaN()};
+  slave_paramters["command_interface/mode_of_operation"] = "1";
+  plugin_->paramters_ = slave_paramters;
+  plugin_->command_interface_ptr_ = &command_interface;
+  plugin_->setup_from_config(YAML::Load(test_drive_config));
+  plugin_->setup_interface_mapping();
+  plugin_->is_operational_ = true;
+  plugin_->mode_of_operation_display_ = 8;
+  uint8_t domain_address[4];
+  uint8_t domain_address_moo[2];
+
+  plugin_->processData(5, domain_address_moo);
+  plugin_->processData(10, domain_address_moo);
+  ASSERT_EQ(plugin_->mode_of_operation_display_, 8);
+
+  EC_WRITE_S32(domain_address, 123456);
+  plugin_->processData(6, domain_address);
+  ASSERT_EQ(plugin_->last_position_, 123456);
+
+  EC_WRITE_S32(domain_address, 0);
+  plugin_->processData(0, domain_address);
+  ASSERT_EQ(EC_READ_S32(domain_address), 123456);
+
+  command_interface[1] = 9;
+  plugin_->processData(5, domain_address_moo);
+  plugin_->processData(10, domain_address_moo);
+  ASSERT_EQ(plugin_->mode_of_operation_display_, 9);
+
+  EC_WRITE_S32(domain_address, 0);
+  plugin_->processData(0, domain_address);
+  ASSERT_EQ(EC_READ_S32(domain_address), 123456);
+
+  EC_WRITE_S32(domain_address, 654321);
+  plugin_->processData(6, domain_address);
+  ASSERT_EQ(plugin_->last_position_, 654321);
+
+  EC_WRITE_S32(domain_address, 0);
+  plugin_->processData(0, domain_address);
+  ASSERT_EQ(EC_READ_S32(domain_address), 654321);
+}

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.hpp

@@ -0,0 +1,52 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TEST_GENERIC_EC_CIA402_DRIVE_HPP_
+#define TEST_GENERIC_EC_CIA402_DRIVE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <unordered_map>
+
+#include "gmock/gmock.h"
+
+#include "ethercat_generic_plugins/generic_ec_cia402_drive.hpp"
+
+// subclassing and friending so we can access member variables
+class FriendEcCiA402Drive : public ethercat_generic_plugins::EcCiA402Drive
+{
+  FRIEND_TEST(EcCiA402DriveTest, SlaveSetupDriveFromConfig);
+  FRIEND_TEST(EcCiA402DriveTest, SlaveSetupPdoChannels);
+  FRIEND_TEST(EcCiA402DriveTest, SlaveSetupSyncs);
+  FRIEND_TEST(EcCiA402DriveTest, SlaveSetupDomains);
+  FRIEND_TEST(EcCiA402DriveTest, EcReadTPDOToStateInterface);
+  FRIEND_TEST(EcCiA402DriveTest, EcWriteRPDOFromCommandInterface);
+  FRIEND_TEST(EcCiA402DriveTest, EcWriteRPDODefaultValue);
+  // FRIEND_TEST(EcCiA402DriveTest, FaultReset);
+  FRIEND_TEST(EcCiA402DriveTest, SwitchModeOfOperation);
+  FRIEND_TEST(EcCiA402DriveTest, EcWriteDefaultTargetPosition);
+};
+
+class EcCiA402DriveTest : public ::testing::Test
+{
+public:
+  void SetUp();
+  void TearDown();
+
+protected:
+  std::unique_ptr<FriendEcCiA402Drive> plugin_;
+};
+
+#endif  // TEST_GENERIC_EC_CIA402_DRIVE_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_load_ec_modules.cpp

@@ -0,0 +1,26 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include <pluginlib/class_loader.hpp>
+#include "ethercat_interface/ec_slave.hpp"
+
+TEST(TestLoadEcCiA402Drive, load_ec_module)
+{
+  pluginlib::ClassLoader<ethercat_interface::EcSlave> ec_loader_{
+    "ethercat_interface", "ethercat_interface::EcSlave"};
+  ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_generic_plugins/EcCiA402Drive"));
+}

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_slave/CMakeLists.txt

@@ -0,0 +1,85 @@
+cmake_minimum_required(VERSION 3.8)
+project(ethercat_generic_slave)
+
+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 REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(ethercat_interface REQUIRED)
+find_package(pluginlib REQUIRED)
+find_package(yaml_cpp_vendor REQUIRED)
+
+file(GLOB_RECURSE PLUGINS_SRC src/*.cpp)
+add_library(ethercat_generic_slave ${PLUGINS_SRC})
+target_compile_features(ethercat_generic_slave PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+target_include_directories(ethercat_generic_slave PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>)
+ament_target_dependencies(
+  ethercat_generic_slave
+  ethercat_interface
+  pluginlib
+  yaml_cpp_vendor
+)
+
+pluginlib_export_plugin_description_file(ethercat_interface ethercat_plugins.xml)
+
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+install(
+  TARGETS ethercat_generic_slave
+  EXPORT export_${PROJECT_NAME}
+  ARCHIVE DESTINATION lib
+  LIBRARY DESTINATION lib
+  RUNTIME DESTINATION bin
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(pluginlib REQUIRED)
+  find_package(ethercat_interface REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  # Test Load EtherCAT modules
+  ament_add_gmock(
+    test_load_generic_plugins
+    test/test_load_ec_modules.cpp
+  )
+  target_include_directories(test_load_generic_plugins PRIVATE include)
+  ament_target_dependencies(test_load_generic_plugins
+    pluginlib
+    ethercat_interface
+  )
+
+  # Test Generic EtherCAT Slave Plugin
+  ament_add_gmock(
+    test_generic_ec_slave
+    test/test_generic_ec_slave.cpp
+  )
+  target_include_directories(test_generic_ec_slave PRIVATE include)
+  target_link_libraries(test_generic_ec_slave
+    ethercat_generic_slave
+  )
+  ament_target_dependencies(test_generic_ec_slave
+    pluginlib
+    ethercat_interface
+  )
+endif()
+
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ethercat_generic_slave
+)
+ament_export_targets(
+  export_${PROJECT_NAME}
+)
+
+ament_package()

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_slave/ethercat_plugins.xml

@@ -0,0 +1,5 @@
+<library path="ethercat_generic_slave">
+  <class name="ethercat_generic_plugins/GenericEcSlave" type="ethercat_generic_plugins::GenericEcSlave" base_class_type="ethercat_interface::EcSlave">
+    <description>Generic EtherCAT Slave.</description>
+  </class>
+</library>

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp

@@ -0,0 +1,74 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// 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.
+//
+// Author: Maciej Bednarczyk (macbednarczyk@gmail.com)
+
+#ifndef ETHERCAT_GENERIC_PLUGINS__GENERIC_EC_SLAVE_HPP_
+#define ETHERCAT_GENERIC_PLUGINS__GENERIC_EC_SLAVE_HPP_
+
+#include <vector>
+#include <string>
+#include <unordered_map>
+
+#include "yaml-cpp/yaml.h"
+#include "ethercat_interface/ec_slave.hpp"
+#include "ethercat_interface/ec_pdo_channel_manager.hpp"
+#include "ethercat_interface/ec_sync_manager.hpp"
+
+namespace ethercat_generic_plugins
+{
+
+class GenericEcSlave : public ethercat_interface::EcSlave
+{
+public:
+  GenericEcSlave();
+  virtual ~GenericEcSlave();
+  virtual int assign_activate_dc_sync();
+
+  virtual void processData(size_t index, uint8_t * domain_address);
+
+  virtual const ec_sync_info_t * syncs();
+  virtual size_t syncSize();
+  virtual const ec_pdo_entry_info_t * channels();
+  virtual void domains(DomainMap & domains) const;
+
+  virtual bool setupSlave(
+    std::unordered_map<std::string, std::string> slave_paramters,
+    std::vector<double> * state_interface,
+    std::vector<double> * command_interface);
+
+protected:
+  uint32_t counter_ = 0;
+  std::vector<ec_pdo_info_t> rpdos_;
+  std::vector<ec_pdo_info_t> tpdos_;
+  std::vector<ec_pdo_entry_info_t> all_channels_;
+  std::vector<ethercat_interface::EcPdoChannelManager> pdo_channels_info_;
+  std::vector<ethercat_interface::SMConfig> sm_configs_;
+  std::vector<ec_sync_info_t> syncs_;
+  std::vector<unsigned int> domain_map_;
+  YAML::Node slave_config_;
+  uint32_t assign_activate_ = 0;
+
+  /** set up of the drive configuration from yaml node*/
+  bool setup_from_config(YAML::Node slave_config);
+  /** set up of the drive configuration from yaml file*/
+  bool setup_from_config_file(std::string config_file);
+
+  void setup_syncs();
+
+  void setup_interface_mapping();
+};
+}  // namespace ethercat_generic_plugins
+
+#endif  // ETHERCAT_GENERIC_PLUGINS__GENERIC_EC_SLAVE_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_slave/package.xml

@@ -0,0 +1,22 @@
+<?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>ethercat_generic_slave</name>
+  <version>1.2.0</version>
+  <description>Plugin implementations of generic EtherCAT modules</description>
+  <maintainer email="mcbed.robotics@gamil.com">Maciej Bednarczyk</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake_ros</buildtool_depend>
+
+  <depend>ethercat_interface</depend>
+  <depend>pluginlib</depend>
+  <depend>yaml_cpp_vendor</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp

@@ -0,0 +1,263 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// 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.
+//
+// Author: Maciej Bednarczyk (macbednarczyk@gmail.com)
+
+#include <numeric>
+
+#include "ethercat_generic_plugins/generic_ec_slave.hpp"
+
+
+size_t type2bytes(std::string type)
+{
+  if (type == "int8" || type == "uint8") {
+    return 1;
+  } else if (type == "int16" || type == "uint16") {
+    return 2;
+  } else if (type == "int32" || type == "uint32") {
+    return 4;
+  } else if (type == "int64" || type == "uint64") {
+    return 8;
+  }
+}
+
+namespace ethercat_generic_plugins
+{
+
+GenericEcSlave::GenericEcSlave()
+: EcSlave(0, 0) {}
+GenericEcSlave::~GenericEcSlave() {}
+int GenericEcSlave::assign_activate_dc_sync() {return assign_activate_;}
+
+void GenericEcSlave::processData(size_t index, uint8_t * domain_address)
+{
+  pdo_channels_info_[domain_map_[index]].ec_update(domain_address);
+}
+
+const ec_sync_info_t * GenericEcSlave::syncs()
+{
+  return syncs_.data();
+}
+size_t GenericEcSlave::syncSize()
+{
+  return syncs_.size();
+}
+const ec_pdo_entry_info_t * GenericEcSlave::channels()
+{
+  return all_channels_.data();
+}
+void GenericEcSlave::domains(DomainMap & domains) const
+{
+  domains = {{0, domain_map_}};
+}
+
+void GenericEcSlave::setup_syncs()
+{
+  if (sm_configs_.size() == 0) {
+    syncs_.push_back({0, EC_DIR_OUTPUT, 0, NULL, EC_WD_DISABLE});
+    syncs_.push_back({1, EC_DIR_INPUT, 0, NULL, EC_WD_DISABLE});
+    syncs_.push_back({2, EC_DIR_OUTPUT, rpdos_.size(), rpdos_.data(), EC_WD_ENABLE});
+    syncs_.push_back({3, EC_DIR_INPUT, tpdos_.size(), tpdos_.data(), EC_WD_DISABLE});
+  } else {
+    for (auto & sm : sm_configs_) {
+      if (sm.pdo_name == "null") {
+        syncs_.push_back({sm.index, sm.type, 0, NULL, sm.watchdog});
+      } else if (sm.pdo_name == "rpdo") {
+        syncs_.push_back({sm.index, sm.type, rpdos_.size(), rpdos_.data(), sm.watchdog});
+      } else if (sm.pdo_name == "tpdo") {
+        syncs_.push_back({sm.index, sm.type, tpdos_.size(), tpdos_.data(), sm.watchdog});
+      }
+    }
+  }
+  syncs_.push_back({0xff});
+}
+
+bool GenericEcSlave::setupSlave(
+  std::unordered_map<std::string, std::string> slave_paramters,
+  std::vector<double> * state_interface,
+  std::vector<double> * command_interface)
+{
+  state_interface_ptr_ = state_interface;
+  command_interface_ptr_ = command_interface;
+  paramters_ = slave_paramters;
+
+  if (paramters_.find("slave_config") != paramters_.end()) {
+    if (!setup_from_config_file(paramters_["slave_config"])) {
+      return false;
+    }
+  } else {
+    std::cerr << "GenericEcSlave: failed to find 'slave_config' tag in URDF." << std::endl;
+    return false;
+  }
+
+  setup_interface_mapping();
+  setup_syncs();
+
+  return true;
+}
+
+bool GenericEcSlave::setup_from_config(YAML::Node slave_config)
+{
+  if (slave_config.size() != 0) {
+    if (slave_config["vendor_id"]) {
+      vendor_id_ = slave_config["vendor_id"].as<uint32_t>();
+    } else {
+      std::cerr << "GenericEcSlave: failed to load drive vendor ID." << std::endl;
+      return false;
+    }
+    if (slave_config["product_id"]) {
+      product_id_ = slave_config["product_id"].as<uint32_t>();
+    } else {
+      std::cerr << "GenericEcSlave: failed to load drive product ID." << std::endl;
+      return false;
+    }
+    if (slave_config["assign_activate"]) {
+      assign_activate_ = slave_config["assign_activate"].as<uint32_t>();
+    }
+
+    if (slave_config["sm"]) {
+      for (const auto & sm : slave_config["sm"]) {
+        ethercat_interface::SMConfig config;
+        if (config.load_from_config(sm)) {
+          sm_configs_.push_back(config);
+        }
+      }
+    }
+
+    if (slave_config["sdo"]) {
+      for (const auto & sdo : slave_config["sdo"]) {
+        ethercat_interface::SdoConfigEntry config;
+        if (config.load_from_config(sdo)) {
+          sdo_config.push_back(config);
+        }
+      }
+    }
+
+    auto channels_nbr = 0;
+
+    if (slave_config["rpdo"]) {
+      for (auto i = 0ul; i < slave_config["rpdo"].size(); i++) {
+        channels_nbr += slave_config["rpdo"][i]["channels"].size();
+      }
+    }
+    if (slave_config["tpdo"]) {
+      for (auto i = 0ul; i < slave_config["tpdo"].size(); i++) {
+        channels_nbr += slave_config["tpdo"][i]["channels"].size();
+      }
+    }
+
+    all_channels_.reserve(channels_nbr);
+    channels_nbr = 0;
+
+    if (slave_config["rpdo"]) {
+      for (auto i = 0ul; i < slave_config["rpdo"].size(); i++) {
+        auto rpdo_channels_size = slave_config["rpdo"][i]["channels"].size();
+        for (auto c = 0ul; c < rpdo_channels_size; c++) {
+          ethercat_interface::EcPdoChannelManager channel_info;
+          channel_info.pdo_type = ethercat_interface::RPDO;
+          channel_info.load_from_config(slave_config["rpdo"][i]["channels"][c]);
+          pdo_channels_info_.push_back(channel_info);
+          all_channels_.push_back(channel_info.get_pdo_entry_info());
+        }
+        rpdos_.push_back(
+          {
+            slave_config["rpdo"][i]["index"].as<uint16_t>(),
+            rpdo_channels_size,
+            all_channels_.data() + channels_nbr
+          }
+        );
+        channels_nbr += rpdo_channels_size;
+      }
+    }
+
+    if (slave_config["tpdo"]) {
+      for (auto i = 0ul; i < slave_config["tpdo"].size(); i++) {
+        auto tpdo_channels_size = slave_config["tpdo"][i]["channels"].size();
+
+        for (auto c = 0ul; c < tpdo_channels_size; c++) {
+          ethercat_interface::EcPdoChannelManager channel_info;
+          channel_info.pdo_type = ethercat_interface::TPDO;
+          channel_info.load_from_config(slave_config["tpdo"][i]["channels"][c]);
+          pdo_channels_info_.push_back(channel_info);
+          all_channels_.push_back(channel_info.get_pdo_entry_info());
+        }
+        tpdos_.push_back(
+          {
+            slave_config["tpdo"][i]["index"].as<uint16_t>(),
+            tpdo_channels_size,
+            all_channels_.data() + channels_nbr
+          }
+        );
+        channels_nbr += tpdo_channels_size;
+      }
+    }
+
+    // Remove gaps from domain mapping
+    for (auto i = 0ul; i < all_channels_.size(); i++) {
+      if (all_channels_[i].index != 0x0000) {
+        domain_map_.push_back(i);
+      }
+    }
+
+    return true;
+  } else {
+    std::cerr << "GenericEcSlave: failed to load slave configuration: empty configuration" <<
+      std::endl;
+    return false;
+  }
+}
+
+bool GenericEcSlave::setup_from_config_file(std::string config_file)
+{
+  // Read drive configuration from YAML file
+  try {
+    slave_config_ = YAML::LoadFile(config_file);
+  } catch (const YAML::ParserException & ex) {
+    std::cerr << "GenericEcSlave: failed to load drive configuration: " << ex.what() << std::endl;
+    return false;
+  } catch (const YAML::BadFile & ex) {
+    std::cerr << "GenericEcSlave: failed to load drive configuration: " << ex.what() << std::endl;
+    return false;
+  }
+  if (!setup_from_config(slave_config_)) {
+    return false;
+  }
+  return true;
+}
+
+void GenericEcSlave::setup_interface_mapping()
+{
+  for (auto & channel : pdo_channels_info_) {
+    if (channel.pdo_type == ethercat_interface::TPDO) {
+      if (paramters_.find("state_interface/" + channel.interface_name) != paramters_.end()) {
+        channel.interface_index =
+          std::stoi(paramters_["state_interface/" + channel.interface_name]);
+      }
+    }
+    if (channel.pdo_type == ethercat_interface::RPDO) {
+      if (paramters_.find("command_interface/" + channel.interface_name) != paramters_.end()) {
+        channel.interface_index = std::stoi(
+          paramters_["command_interface/" + channel.interface_name]);
+      }
+    }
+
+    channel.setup_interface_ptrs(state_interface_ptr_, command_interface_ptr_);
+  }
+}
+
+}  // namespace ethercat_generic_plugins
+
+#include <pluginlib/class_list_macros.hpp>
+
+PLUGINLIB_EXPORT_CLASS(ethercat_generic_plugins::GenericEcSlave, ethercat_interface::EcSlave)

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.cpp

@@ -0,0 +1,252 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <map>
+#include <pluginlib/class_loader.hpp>
+#include "ethercat_interface/ec_slave.hpp"
+#include "test_generic_ec_slave.hpp"
+
+const char test_slave_config[] =
+  R"(
+# Configuration file for Test Slave
+vendor_id: 0x00000011
+product_id: 0x07030924
+assign_activate: 0x0321  # DC Synch register
+sdo:  # sdo data to be transferred at slave startup
+  - {index: 0x60C2, sub_index: 1, type: int8, value: 10}
+  - {index: 0x60C2, sub_index: 2, type: int8, value: -3}
+  - {index: 0x6098, sub_index: 0, type: int8, value: 35}
+  - {index: 0x6099, sub_index: 0, type: int32, value: 0}
+rpdo:  # Receive PDO Mapping
+  - index: 0x1607
+    channels:
+      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan}
+      - {index: 0x60ff, sub_index: 0, type: int32, command_interface: velocity, default: 0}
+      - {index: 0x6071, sub_index: 0, type: int16, command_interface: effort, default: -5, factor: 2, offset: 10}
+      - {index: 0x6072, sub_index: 0, type: int16, command_interface: ~, default: 1000}
+      - {index: 0x6040, sub_index: 0, type: uint16, command_interface: ~, default: 0}
+      - {index: 0x6060, sub_index: 0, type: int8, command_interface: ~, default: 8}
+tpdo:  # Transmit PDO Mapping
+  - index: 0x1a07
+    channels:
+      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position}
+      - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity}
+      - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort, factor: 5, offset: 15}
+      - {index: 0x6041, sub_index: 0, type: uint16, state_interface: ~}
+      - {index: 0x6061, sub_index: 0, type: int8, state_interface: ~}
+  - index: 0x1a45
+    channels:
+      - {index: 0x2205, sub_index: 1, type: int16, state_interface: analog_input1}
+      - {index: 0x2205, sub_index: 2, type: int16, state_interface: analog_input2}
+sm:  # Sync Manager
+  - {index: 0, type: output, pdo: ~, watchdog: disable}
+  - {index: 1, type: input, pdo: ~, watchdog: disable}
+  - {index: 2, type: output, pdo: rpdo, watchdog: enable}
+  - {index: 3, type: input, pdo: tpdo, watchdog: disable}
+)";
+
+void GenericEcSlaveTest::SetUp()
+{
+  plugin_ = std::make_unique<FriendGenericEcSlave>();
+}
+
+void GenericEcSlaveTest::TearDown()
+{
+  plugin_.reset(nullptr);
+}
+
+TEST_F(GenericEcSlaveTest, SlaveSetupNoSlaveConfig)
+{
+  SetUp();
+  std::vector<double> state_interface = {0};
+  std::vector<double> command_interface = {0};
+  std::unordered_map<std::string, std::string> slave_paramters;
+  // setup failed, 'slave_config' parameter not set
+  ASSERT_EQ(
+    plugin_->setupSlave(
+      slave_paramters,
+      &state_interface,
+      &command_interface
+    ),
+    false
+  );
+}
+
+TEST_F(GenericEcSlaveTest, SlaveSetupMissingFileSlaveConfig)
+{
+  SetUp();
+  std::vector<double> state_interface = {0};
+  std::vector<double> command_interface = {0};
+  std::unordered_map<std::string, std::string> slave_paramters;
+  slave_paramters["slave_config"] = "slave_config.yaml";
+  // setup failed, 'slave_config.yaml' file not set
+  ASSERT_EQ(
+    plugin_->setupSlave(
+      slave_paramters,
+      &state_interface,
+      &command_interface
+    ),
+    false
+  );
+}
+
+TEST_F(GenericEcSlaveTest, SlaveSetupSlaveFromConfig)
+{
+  SetUp();
+  ASSERT_EQ(
+    plugin_->setup_from_config(YAML::Load(test_slave_config)),
+    true
+  );
+  ASSERT_EQ(plugin_->vendor_id_, 0x00000011);
+  ASSERT_EQ(plugin_->product_id_, 0x07030924);
+  ASSERT_EQ(plugin_->assign_activate_, 0x0321);
+
+  ASSERT_EQ(plugin_->rpdos_.size(), 1);
+  ASSERT_EQ(plugin_->rpdos_[0].index, 0x1607);
+
+  ASSERT_EQ(plugin_->tpdos_.size(), 2);
+  ASSERT_EQ(plugin_->tpdos_[0].index, 0x1a07);
+  ASSERT_EQ(plugin_->tpdos_[1].index, 0x1a45);
+
+  ASSERT_EQ(plugin_->pdo_channels_info_[1].interface_name, "velocity");
+  ASSERT_EQ(plugin_->pdo_channels_info_[2].factor, 2);
+  ASSERT_EQ(plugin_->pdo_channels_info_[2].offset, 10);
+  ASSERT_EQ(plugin_->pdo_channels_info_[3].default_value, 1000);
+  ASSERT_TRUE(std::isnan(plugin_->pdo_channels_info_[0].default_value));
+  ASSERT_EQ(plugin_->pdo_channels_info_[4].interface_name, "null");
+  ASSERT_EQ(plugin_->pdo_channels_info_[12].interface_name, "analog_input2");
+  ASSERT_EQ(plugin_->pdo_channels_info_[4].data_type, "uint16");
+}
+
+TEST_F(GenericEcSlaveTest, SlaveSetupPdoChannels)
+{
+  SetUp();
+  plugin_->setup_from_config(YAML::Load(test_slave_config));
+  std::vector<ec_pdo_entry_info_t> channels(
+    plugin_->channels(),
+    plugin_->channels() + plugin_->all_channels_.size()
+  );
+
+  ASSERT_EQ(channels.size(), 13);
+  ASSERT_EQ(channels[0].index, 0x607a);
+  ASSERT_EQ(channels[11].index, 0x2205);
+  ASSERT_EQ(channels[11].subindex, 0x01);
+}
+
+TEST_F(GenericEcSlaveTest, SlaveSetupSyncs)
+{
+  SetUp();
+  plugin_->setup_from_config(YAML::Load(test_slave_config));
+  plugin_->setup_syncs();
+  std::vector<ec_sync_info_t> syncs(
+    plugin_->syncs(),
+    plugin_->syncs() + plugin_->syncSize()
+  );
+
+  ASSERT_EQ(syncs.size(), 5);
+  ASSERT_EQ(syncs[1].index, 1);
+  ASSERT_EQ(syncs[1].dir, EC_DIR_INPUT);
+  ASSERT_EQ(syncs[1].n_pdos, 0);
+  ASSERT_EQ(syncs[1].watchdog_mode, EC_WD_DISABLE);
+  ASSERT_EQ(syncs[2].dir, EC_DIR_OUTPUT);
+  ASSERT_EQ(syncs[2].n_pdos, 1);
+  ASSERT_EQ(syncs[3].index, 3);
+  ASSERT_EQ(syncs[3].dir, EC_DIR_INPUT);
+  ASSERT_EQ(syncs[3].n_pdos, 2);
+  ASSERT_EQ(syncs[3].watchdog_mode, EC_WD_DISABLE);
+}
+
+TEST_F(GenericEcSlaveTest, SlaveSetupDomains)
+{
+  SetUp();
+  plugin_->setup_from_config(YAML::Load(test_slave_config));
+  std::map<unsigned int, std::vector<unsigned int>> domains;
+  plugin_->domains(domains);
+
+  ASSERT_EQ(domains[0].size(), 13);
+  ASSERT_EQ(domains[0][0], 0);
+  ASSERT_EQ(domains[0][12], 12);
+}
+
+TEST_F(GenericEcSlaveTest, EcReadTPDOToStateInterface)
+{
+  SetUp();
+  std::unordered_map<std::string, std::string> slave_paramters;
+  std::vector<double> state_interface = {0, 0};
+  plugin_->state_interface_ptr_ = &state_interface;
+  slave_paramters["state_interface/effort"] = "1";
+  plugin_->paramters_ = slave_paramters;
+  plugin_->setup_from_config(YAML::Load(test_slave_config));
+  plugin_->setup_interface_mapping();
+  ASSERT_EQ(plugin_->pdo_channels_info_[8].interface_index, 1);
+  uint8_t domain_address[2];
+  EC_WRITE_S16(domain_address, 42);
+  plugin_->processData(8, domain_address);
+  ASSERT_EQ(plugin_->state_interface_ptr_->at(1), 5 * 42 + 15);
+}
+
+TEST_F(GenericEcSlaveTest, EcWriteRPDOFromCommandInterface)
+{
+  SetUp();
+  std::unordered_map<std::string, std::string> slave_paramters;
+  std::vector<double> command_interface = {0, 42};
+  plugin_->command_interface_ptr_ = &command_interface;
+  slave_paramters["command_interface/effort"] = "1";
+  plugin_->paramters_ = slave_paramters;
+  plugin_->setup_from_config(YAML::Load(test_slave_config));
+  plugin_->setup_interface_mapping();
+  ASSERT_EQ(plugin_->pdo_channels_info_[2].interface_index, 1);
+  uint8_t domain_address[2];
+  plugin_->processData(2, domain_address);
+  ASSERT_EQ(plugin_->pdo_channels_info_[2].last_value, 2 * 42 + 10);
+  ASSERT_EQ(EC_READ_S16(domain_address), 2 * 42 + 10);
+}
+
+TEST_F(GenericEcSlaveTest, EcWriteRPDODefaultValue)
+{
+  SetUp();
+  plugin_->setup_from_config(YAML::Load(test_slave_config));
+  plugin_->setup_interface_mapping();
+  uint8_t domain_address[2];
+  plugin_->processData(2, domain_address);
+  ASSERT_EQ(plugin_->pdo_channels_info_[2].last_value, -5);
+  ASSERT_EQ(EC_READ_S16(domain_address), -5);
+}
+
+TEST_F(GenericEcSlaveTest, SlaveSetupSDOConfig)
+{
+  SetUp();
+  plugin_->setup_from_config(YAML::Load(test_slave_config));
+  ASSERT_EQ(plugin_->sdo_config[0].index, 0x60C2);
+  ASSERT_EQ(plugin_->sdo_config[0].sub_index, 1);
+  ASSERT_EQ(plugin_->sdo_config[1].sub_index, 2);
+  ASSERT_EQ(plugin_->sdo_config[0].data_size(), 1);
+  ASSERT_EQ(plugin_->sdo_config[0].data, 10);
+  ASSERT_EQ(plugin_->sdo_config[2].index, 0x6098);
+  ASSERT_EQ(plugin_->sdo_config[3].data_type, "int32");
+  ASSERT_EQ(plugin_->sdo_config[3].data_size(), 4);
+}
+
+TEST_F(GenericEcSlaveTest, SlaveSetupSyncManagerConfig)
+{
+  SetUp();
+  plugin_->setup_from_config(YAML::Load(test_slave_config));
+  ASSERT_EQ(plugin_->sm_configs_.size(), 4);
+  ASSERT_EQ(plugin_->sm_configs_[0].index, 0);
+  ASSERT_EQ(plugin_->sm_configs_[0].type, EC_DIR_OUTPUT);
+  ASSERT_EQ(plugin_->sm_configs_[0].watchdog, EC_WD_DISABLE);
+  ASSERT_EQ(plugin_->sm_configs_[0].pdo_name, "null");
+  ASSERT_EQ(plugin_->sm_configs_[2].pdo_name, "rpdo");
+  ASSERT_EQ(plugin_->sm_configs_[2].watchdog, EC_WD_ENABLE);
+}

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.hpp

@@ -0,0 +1,51 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef TEST_GENERIC_EC_SLAVE_HPP_
+#define TEST_GENERIC_EC_SLAVE_HPP_
+
+#include <memory>
+#include <string>
+#include <vector>
+#include <unordered_map>
+
+#include "gmock/gmock.h"
+
+#include "ethercat_generic_plugins/generic_ec_slave.hpp"
+
+// subclassing and friending so we can access member variables
+class FriendGenericEcSlave : public ethercat_generic_plugins::GenericEcSlave
+{
+  FRIEND_TEST(GenericEcSlaveTest, SlaveSetupSlaveFromConfig);
+  FRIEND_TEST(GenericEcSlaveTest, SlaveSetupPdoChannels);
+  FRIEND_TEST(GenericEcSlaveTest, SlaveSetupSyncs);
+  FRIEND_TEST(GenericEcSlaveTest, SlaveSetupDomains);
+  FRIEND_TEST(GenericEcSlaveTest, EcReadTPDOToStateInterface);
+  FRIEND_TEST(GenericEcSlaveTest, EcWriteRPDOFromCommandInterface);
+  FRIEND_TEST(GenericEcSlaveTest, EcWriteRPDODefaultValue);
+  FRIEND_TEST(GenericEcSlaveTest, SlaveSetupSDOConfig);
+  FRIEND_TEST(GenericEcSlaveTest, SlaveSetupSyncManagerConfig);
+};
+
+class GenericEcSlaveTest : public ::testing::Test
+{
+public:
+  void SetUp();
+  void TearDown();
+
+protected:
+  std::unique_ptr<FriendGenericEcSlave> plugin_;
+};
+
+#endif  // TEST_GENERIC_EC_SLAVE_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_generic_plugins/ethercat_generic_slave/test/test_load_ec_modules.cpp

@@ -0,0 +1,26 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include <pluginlib/class_loader.hpp>
+#include "ethercat_interface/ec_slave.hpp"
+
+TEST(TestLoadGenericEcSlave, load_ec_module)
+{
+  pluginlib::ClassLoader<ethercat_interface::EcSlave> ec_loader_{
+    "ethercat_interface", "ethercat_interface::EcSlave"};
+  ASSERT_NO_THROW(ec_loader_.createSharedInstance("ethercat_generic_plugins/GenericEcSlave"));
+}

HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/CMakeLists.txt

@@ -0,0 +1,81 @@
+cmake_minimum_required(VERSION 3.8)
+project(ethercat_interface)
+
+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 REQUIRED)
+find_package(ament_cmake_ros REQUIRED)
+find_package(rclcpp REQUIRED)
+
+# EtherLab
+set(ETHERLAB_DIR /usr/local)
+set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
+find_library(ETHERCAT_LIB ethercat HINTS ${ETHERLAB_DIR}/lib)
+
+ament_export_include_directories(
+  include
+  ${ETHERLAB_DIR}/include
+)
+
+add_library(
+  ${PROJECT_NAME}
+  SHARED
+  src/ec_master.cpp)
+
+target_include_directories(
+  ${PROJECT_NAME}
+  PRIVATE
+  include
+  ${ETHERLAB_DIR}/include
+)
+
+target_link_libraries(${PROJECT_NAME} ${ETHERCAT_LIB})
+
+ament_target_dependencies(
+  ${PROJECT_NAME}
+  rclcpp
+)
+
+# INSTALL
+install(
+  TARGETS ${PROJECT_NAME}
+  DESTINATION lib
+)
+install(
+  DIRECTORY include/
+  DESTINATION include
+)
+
+if(BUILD_TESTING)
+  # find_package(ament_cmake_gtest REQUIRED)
+  find_package(ament_lint_auto REQUIRED)
+  find_package(yaml_cpp_vendor REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+
+  # Test PdoChannelManager
+  ament_add_gmock(
+    test_ec_pdo_channel_manager
+    test/test_ec_pdo_channel_manager.cpp
+  )
+  target_include_directories(test_ec_pdo_channel_manager PRIVATE include ${ETHERLAB_DIR}/include)
+  ament_target_dependencies(test_ec_pdo_channel_manager
+    yaml_cpp_vendor
+  )
+endif()
+
+## EXPORTS
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ${PROJECT_NAME}
+  ${ETHERCAT_LIBRARY}
+)
+ament_export_dependencies(
+  rclcpp
+)
+ament_package()

HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_master.hpp

@@ -0,0 +1,177 @@
+// Copyright 2022 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ETHERCAT_INTERFACE__EC_MASTER_HPP_
+#define ETHERCAT_INTERFACE__EC_MASTER_HPP_
+
+#include <ecrt.h>
+
+#include <time.h>
+#include <string>
+#include <vector>
+#include <map>
+#include <chrono>
+#include "ethercat_interface/ec_slave.hpp"
+
+
+namespace ethercat_interface
+{
+
+class EcMaster
+{
+public:
+  explicit EcMaster(const int master = 0);
+  virtual ~EcMaster();
+
+  /** \brief add a slave device to the master
+    * alias and position can be found by running the following command
+    * /opt/etherlab/bin$ sudo ./ethercat slaves
+    * look for the "A B:C STATUS DEVICE" (e.g. B=alias, C=position)
+    */
+  void addSlave(uint16_t alias, uint16_t position, EcSlave * slave);
+
+  /** \brief configure slave using SDO
+    */
+  int configSlaveSdo(uint16_t slave_position, SdoConfigEntry sdo_config, uint32_t * abort_code);
+
+  /** call after adding all slaves, and before update */
+  bool activate();
+
+  /** perform one EtherCAT cycle, passing the domain to the slaves */
+  virtual void update(uint32_t domain = 0);
+
+  /** run a control loop of update() and user_callback(), blocking.
+   *  call activate and setThreadHighPriority/RealTime first. */
+  typedef void (* SIMPLECAT_CONTRL_CALLBACK)(void);
+  virtual void run(SIMPLECAT_CONTRL_CALLBACK user_callback);
+
+  /** stop the control loop. use within callback, or from a separate thread. */
+  virtual void stop() {running_ = false;}
+
+  /** time of last ethercat update, since calling run. stops if stop called.
+   *  returns actual time. use elapsedCycles()/frequency for discrete time at last update. */
+  virtual double elapsedTime();
+
+  /** number of EtherCAT updates since calling run. */
+  virtual uint64_t elapsedCycles();
+
+  /** add ctr-c exit callback.
+    * default exits the run loop and prints timing */
+  typedef void (* SIMPLECAT_EXIT_CALLBACK)(int);
+  static void setCtrlCHandler(SIMPLECAT_EXIT_CALLBACK user_callback = NULL);
+
+  /** set the thread to a priority of -19
+   *  priority range is -20 (highest) to 19 (lowest) */
+  static void setThreadHighPriority();
+
+  /** set the thread to real time (FIFO)
+   *  thread cannot be preempted.
+   *  set priority as 49 (kernel and interrupts are 50) */
+  static void setThreadRealTime();
+
+  void setCtrlFrequency(double frequency)
+  {
+    interval_ = 1000000000.0 / frequency;
+  }
+
+  uint32_t getInterval() {return interval_;}
+
+  void readData(uint32_t domain = 0);
+  void writeData(uint32_t domain = 0);
+
+private:
+  /** true if running */
+  volatile bool running_ = false;
+
+  /** start and current time */
+  std::chrono::time_point<std::chrono::system_clock> start_t_, curr_t_;
+
+  // EtherCAT Control
+
+  /** register a domain of the slave */
+  struct DomainInfo;
+  void registerPDOInDomain(
+    uint16_t alias, uint16_t position,
+    std::vector<uint32_t> & channel_indices,
+    DomainInfo * domain_info,
+    EcSlave * slave);
+
+  /** check for change in the domain state */
+  void checkDomainState(uint32_t domain);
+
+  /** check for change in the master state */
+  void checkMasterState();
+
+  /** check for change in the slave states */
+  void checkSlaveStates();
+
+  /** print warning message to terminal */
+  static void printWarning(const std::string & message);
+
+  /** EtherCAT master data */
+  ec_master_t * master_ = NULL;
+  ec_master_state_t master_state_ = {};
+
+  /** data for a single domain */
+  struct DomainInfo
+  {
+    explicit DomainInfo(ec_master_t * master);
+    ~DomainInfo();
+
+    ec_domain_t * domain = NULL;
+    ec_domain_state_t domain_state = {};
+    uint8_t * domain_pd = NULL;
+
+    /** domain pdo registration array.
+     *  do not modify after active(), or may invalidate */
+    std::vector<ec_pdo_entry_reg_t> domain_regs;
+
+    /** slave's pdo entries in the domain */
+    struct Entry
+    {
+      EcSlave * slave = NULL;
+      int num_pdos = 0;
+      uint32_t * offset = NULL;
+      uint32_t * bit_position = NULL;
+    };
+
+    std::vector<Entry> entries;
+  };
+
+  /** map from domain index to domain info */
+  std::map<uint32_t, DomainInfo *> domain_info_;
+
+  /** data needed to check slave state */
+  struct SlaveInfo
+  {
+    EcSlave * slave = NULL;
+    ec_slave_config_t * config = NULL;
+    ec_slave_config_state_t config_state = {0};
+  };
+
+  std::vector<SlaveInfo> slave_info_;
+
+  /** counter of control loops */
+  uint64_t update_counter_ = 0;
+
+  /** frequency to check for master or slave state change.
+   *  state checked every frequency_ control loops */
+  uint32_t check_state_frequency_ = 10;
+
+  uint32_t interval_;
+};
+
+}  // namespace ethercat_interface
+
+#endif  // ETHERCAT_INTERFACE__EC_MASTER_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp

@@ -0,0 +1,234 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// 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.
+//
+// Author: Maciej Bednarczyk (macbednarczyk@gmail.com)
+
+#ifndef ETHERCAT_INTERFACE__EC_PDO_CHANNEL_MANAGER_HPP_
+#define ETHERCAT_INTERFACE__EC_PDO_CHANNEL_MANAGER_HPP_
+
+#include <ecrt.h>
+#include <string>
+#include <vector>
+#include <limits>
+
+#include "yaml-cpp/yaml.h"
+
+namespace ethercat_interface
+{
+enum PdoType
+{
+  RPDO = 0,
+  TPDO = 1
+};
+
+class EcPdoChannelManager
+{
+public:
+  EcPdoChannelManager() {}
+  ~EcPdoChannelManager() {}
+  void setup_interface_ptrs(
+    std::vector<double> * state_interface,
+    std::vector<double> * command_interface)
+  {
+    command_interface_ptr_ = command_interface;
+    state_interface_ptr_ = state_interface;
+  }
+
+  ec_pdo_entry_info_t get_pdo_entry_info() {return {index, sub_index, type2bits(data_type)};}
+
+  double ec_read(uint8_t * domain_address)
+  {
+    if (data_type == "uint8") {
+      last_value = static_cast<double>(EC_READ_U8(domain_address));
+    } else if (data_type == "int8") {
+      last_value = static_cast<double>(EC_READ_S8(domain_address));
+    } else if (data_type == "uint16") {
+      last_value = static_cast<double>(EC_READ_U16(domain_address));
+    } else if (data_type == "int16") {
+      last_value = static_cast<double>(EC_READ_S16(domain_address));
+    } else if (data_type == "uint32") {
+      last_value = static_cast<double>(EC_READ_U32(domain_address));
+    } else if (data_type == "int32") {
+      last_value = static_cast<double>(EC_READ_S32(domain_address));
+    } else if (data_type == "uint64") {
+      last_value = static_cast<double>(EC_READ_U64(domain_address));
+    } else if (data_type == "int64") {
+      last_value = static_cast<double>(EC_READ_S64(domain_address));
+    } else if (data_type == "bool") {
+      last_value = (EC_READ_U8(domain_address) & data_mask) ? 1 : 0;
+    } else {
+      last_value = static_cast<double>(EC_READ_U8(domain_address) & data_mask);
+    }
+    last_value = factor * last_value + offset;
+    return last_value;
+  }
+
+  void ec_write(uint8_t * domain_address, double value)
+  {
+    if (data_type == "uint8") {
+      EC_WRITE_U8(domain_address, static_cast<uint8_t>(value));
+    } else if (data_type == "int8") {
+      EC_WRITE_S8(domain_address, static_cast<int8_t>(value));
+    } else if (data_type == "uint16") {
+      EC_WRITE_U16(domain_address, static_cast<uint16_t>(value));
+    } else if (data_type == "int16") {
+      EC_WRITE_S16(domain_address, static_cast<int16_t>(value));
+    } else if (data_type == "uint32") {
+      EC_WRITE_U32(domain_address, static_cast<uint32_t>(value));
+    } else if (data_type == "int32") {
+      EC_WRITE_S32(domain_address, static_cast<int32_t>(value));
+    } else if (data_type == "uint64") {
+      EC_WRITE_U64(domain_address, static_cast<uint64_t>(value));
+    } else if (data_type == "int64") {
+      EC_WRITE_S64(domain_address, static_cast<int64_t>(value));
+    } else {
+      buffer_ = EC_READ_U8(domain_address);
+      if (popcount(data_mask) == 1) {
+        buffer_ &= ~(data_mask);
+        if (value) {buffer_ |= data_mask;}
+      } else if (data_mask != 0) {
+        buffer_ = 0;
+        buffer_ |= (static_cast<uint8_t>(value) & data_mask);
+      }
+      EC_WRITE_U8(domain_address, buffer_);
+    }
+    last_value = value;
+  }
+
+  void ec_update(uint8_t * domain_address)
+  {
+    // update state interface
+    if (pdo_type == TPDO) {
+      ec_read(domain_address);
+      if (interface_index >= 0) {
+        state_interface_ptr_->at(interface_index) = last_value;
+      }
+    } else if (pdo_type == RPDO && allow_ec_write) {
+      if (interface_index >= 0 &&
+        !std::isnan(command_interface_ptr_->at(interface_index)) &&
+        !override_command)
+      {
+        ec_write(domain_address, factor * command_interface_ptr_->at(interface_index) + offset);
+      } else {
+        if (!std::isnan(default_value)) {
+          ec_write(domain_address, default_value);
+        }
+      }
+    }
+  }
+
+  bool load_from_config(YAML::Node channel_config)
+  {
+    // index
+    if (channel_config["index"]) {
+      index = channel_config["index"].as<uint16_t>();
+    } else {
+      std::cerr << "missing channel index info" << std::endl;
+    }
+    // sub_index
+    if (channel_config["sub_index"]) {
+      sub_index = channel_config["sub_index"].as<uint8_t>();
+    } else {
+      std::cerr << "channel " << index << ": missing channel info" << std::endl;
+    }
+    // data type
+    if (channel_config["type"]) {
+      data_type = channel_config["type"].as<std::string>();
+    } else {
+      std::cerr << "channel " << index << ": missing channel data type info" << std::endl;
+    }
+
+    if (pdo_type == RPDO) {
+      // interface name
+      if (channel_config["command_interface"]) {
+        interface_name = channel_config["command_interface"].as<std::string>();
+      }
+      // default value
+      if (channel_config["default"]) {
+        default_value = channel_config["default"].as<double>();
+      }
+
+    } else if (pdo_type == TPDO) {
+      // interface name
+      if (channel_config["state_interface"]) {
+        interface_name = channel_config["state_interface"].as<std::string>();
+      }
+    }
+
+    // factor
+    if (channel_config["factor"]) {
+      factor = channel_config["factor"].as<double>();
+    }
+    // offset
+    if (channel_config["offset"]) {
+      offset = channel_config["offset"].as<double>();
+    }
+    // mask
+    if (channel_config["mask"]) {
+      data_mask = channel_config["mask"].as<uint8_t>();
+    }
+
+    return true;
+  }
+
+  uint8_t type2bits(std::string type)
+  {
+    if (type == "bool") {
+      return 1;
+    } else if (type == "int16" || type == "uint16") {
+      return 16;
+    } else if (type == "int8" || type == "uint8") {
+      return 8;
+    } else if (type == "int16" || type == "uint16") {
+      return 16;
+    } else if (type == "int32" || type == "uint32") {
+      return 32;
+    } else if (type == "int64" || type == "uint64") {
+      return 64;
+    } else if (type.find("bit") != std::string::npos) {
+      std::string n_bits = type.substr(type.find("bit") + 3);
+      return static_cast<uint8_t>(std::stoi(n_bits));
+    }
+    return -1;
+  }
+
+  PdoType pdo_type;
+  uint16_t index;
+  uint8_t sub_index;
+  std::string data_type;
+  std::string interface_name;
+  uint8_t data_mask = 255;
+  double default_value = std::numeric_limits<double>::quiet_NaN();
+  int interface_index = -1;
+  double last_value = std::numeric_limits<double>::quiet_NaN();
+  bool allow_ec_write = true;
+  bool override_command = false;
+  double factor = 1;
+  double offset = 0;
+
+private:
+  std::vector<double> * command_interface_ptr_;
+  std::vector<double> * state_interface_ptr_;
+  uint8_t buffer_ = 0;
+
+  int popcount(uint8_t x)
+  {
+    int count = 0;
+    for (; x != 0; x >>= 1) {if (x & 1) {count++;}}
+    return count;
+  }
+};
+
+}  // namespace ethercat_interface
+#endif  // ETHERCAT_INTERFACE__EC_PDO_CHANNEL_MANAGER_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp

@@ -0,0 +1,117 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// 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.
+//
+// Author: Maciej Bednarczyk (macbednarczyk@gmail.com)
+
+#ifndef ETHERCAT_INTERFACE__EC_SDO_MANAGER_HPP_
+#define ETHERCAT_INTERFACE__EC_SDO_MANAGER_HPP_
+
+#include <ecrt.h>
+#include <string>
+#include <vector>
+#include <limits>
+
+#include "yaml-cpp/yaml.h"
+
+namespace ethercat_interface
+{
+
+class SdoConfigEntry
+{
+public:
+  SdoConfigEntry() {}
+  ~SdoConfigEntry() {}
+
+  void buffer_write(uint8_t * buffer)
+  {
+    if (data_type == "uint8") {
+      EC_WRITE_U8(buffer, static_cast<uint8_t>(data));
+    } else if (data_type == "int8") {
+      EC_WRITE_S8(buffer, static_cast<int8_t>(data));
+    } else if (data_type == "uint16") {
+      EC_WRITE_U16(buffer, static_cast<uint16_t>(data));
+    } else if (data_type == "int16") {
+      EC_WRITE_S16(buffer, static_cast<int16_t>(data));
+    } else if (data_type == "uint32") {
+      EC_WRITE_U32(buffer, static_cast<uint32_t>(data));
+    } else if (data_type == "int32") {
+      EC_WRITE_S32(buffer, static_cast<int32_t>(data));
+    } else if (data_type == "uint64") {
+      EC_WRITE_U64(buffer, static_cast<uint64_t>(data));
+    } else if (data_type == "int64") {
+      EC_WRITE_S64(buffer, static_cast<int64_t>(data));
+    }
+  }
+
+  bool load_from_config(YAML::Node sdo_config)
+  {
+    // index
+    if (sdo_config["index"]) {
+      index = sdo_config["index"].as<uint16_t>();
+    } else {
+      std::cerr << "missing sdo index info" << std::endl;
+      return false;
+    }
+    // sub_index
+    if (sdo_config["sub_index"]) {
+      sub_index = sdo_config["sub_index"].as<uint8_t>();
+    } else {
+      std::cerr << "sdo " << index << ": missing sdo info" << std::endl;
+      return false;
+    }
+    // data type
+    if (sdo_config["type"]) {
+      data_type = sdo_config["type"].as<std::string>();
+    } else {
+      std::cerr << "sdo " << index << ": missing sdo data type info" << std::endl;
+      return false;
+    }
+    // value
+    if (sdo_config["value"]) {
+      data = sdo_config["value"].as<int>();
+    } else {
+      std::cerr << "sdo " << index << ": missing sdo value" << std::endl;
+      return false;
+    }
+
+    return true;
+  }
+
+  size_t data_size()
+  {
+    return type2bytes(data_type);
+  }
+
+  uint16_t index;
+  uint8_t sub_index;
+  std::string data_type;
+  int data;
+
+private:
+  size_t type2bytes(std::string type)
+  {
+    if (type == "int8" || type == "uint8") {
+      return 1;
+    } else if (type == "int16" || type == "uint16") {
+      return 2;
+    } else if (type == "int32" || type == "uint32") {
+      return 4;
+    } else if (type == "int64" || type == "uint64") {
+      return 8;
+    }
+  }
+};
+
+}  // namespace ethercat_interface
+#endif  // ETHERCAT_INTERFACE__EC_SDO_MANAGER_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_slave.hpp

@@ -0,0 +1,76 @@
+// Copyright 2022 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ETHERCAT_INTERFACE__EC_SLAVE_HPP_
+#define ETHERCAT_INTERFACE__EC_SLAVE_HPP_
+
+#include <ecrt.h>
+#include <map>
+#include <vector>
+#include <unordered_map>
+#include <iostream>
+#include <cmath>
+#include <string>
+
+#include "ethercat_interface/ec_sdo_manager.hpp"
+
+namespace ethercat_interface
+{
+
+class EcSlave
+{
+public:
+  EcSlave(uint32_t vendor_id, uint32_t product_id)
+  : vendor_id_(vendor_id),
+    product_id_(product_id) {}
+  virtual ~EcSlave() {}
+  /** read or write data to the domain */
+  virtual void processData(size_t /*index*/, uint8_t * /*domain_address*/) {}
+  /** a pointer to syncs. return &syncs[0] */
+  virtual const ec_sync_info_t * syncs() {return NULL;}
+  virtual bool initialized() {return true;}
+  virtual void set_state_is_operational(bool value) {is_operational_ = value;}
+  /** Assign activate DC synchronization. return activate word*/
+  virtual int assign_activate_dc_sync() {return 0x00;}
+  /** number of elements in the syncs array. */
+  virtual size_t syncSize() {return 0;}
+  /** a pointer to all PDO entries */
+  virtual const ec_pdo_entry_info_t * channels() {return NULL;}
+  /** a map from domain index to pdo indices in that domain.
+  *  map<domain index, vector<channels_ indices> > */
+  typedef std::map<unsigned int, std::vector<unsigned int>> DomainMap;
+  virtual void domains(DomainMap & /*domains*/) const {}
+  virtual bool setupSlave(
+    std::unordered_map<std::string, std::string> slave_paramters,
+    std::vector<double> * state_interface,
+    std::vector<double> * command_interface)
+  {
+    state_interface_ptr_ = state_interface;
+    command_interface_ptr_ = command_interface;
+    paramters_ = slave_paramters;
+    return true;
+  }
+  uint32_t vendor_id_;
+  uint32_t product_id_;
+
+  std::vector<SdoConfigEntry> sdo_config;
+
+protected:
+  std::vector<double> * state_interface_ptr_;
+  std::vector<double> * command_interface_ptr_;
+  std::unordered_map<std::string, std::string> paramters_;
+  bool is_operational_ = false;
+};
+}  // namespace ethercat_interface
+#endif  // ETHERCAT_INTERFACE__EC_SLAVE_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/include/ethercat_interface/ec_sync_manager.hpp

@@ -0,0 +1,86 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// 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.
+//
+// Author: Maciej Bednarczyk (macbednarczyk@gmail.com)
+
+#ifndef ETHERCAT_INTERFACE__EC_SYNC_MANAGER_HPP_
+#define ETHERCAT_INTERFACE__EC_SYNC_MANAGER_HPP_
+
+#include <ecrt.h>
+#include <string>
+#include <vector>
+#include <limits>
+
+#include "yaml-cpp/yaml.h"
+
+namespace ethercat_interface
+{
+
+class SMConfig
+{
+public:
+  SMConfig() {}
+  ~SMConfig() {}
+
+  bool load_from_config(YAML::Node sm_config)
+  {
+    // index
+    if (sm_config["index"]) {
+      index = sm_config["index"].as<uint8_t>();
+    } else {
+      std::cerr << "missing sdo index info" << std::endl;
+      return false;
+    }
+    // type
+    if (sm_config["type"]) {
+      if (sm_config["type"].as<std::string>() == "input") {
+        type = EC_DIR_INPUT;
+      } else if (sm_config["type"].as<std::string>() == "output") {
+        type = EC_DIR_OUTPUT;
+      } else {
+        std::cerr << "sm " << index << ": type should be input/output" << std::endl;
+        return false;
+      }
+    } else {
+      std::cerr << "sm " << index << ": missing type info" << std::endl;
+      return false;
+    }
+    // pdo name
+    if (sm_config["pdo"]) {
+      if (sm_config["pdo"].as<std::string>() == "rpdo") {
+        pdo_name = "rpdo";
+      } else if (sm_config["pdo"].as<std::string>() == "tpdo") {
+        pdo_name = "tpdo";
+      }
+    }
+    // watchdog
+    if (sm_config["watchdog"]) {
+      if (sm_config["watchdog"].as<std::string>() == "enable") {
+        watchdog = EC_WD_ENABLE;
+      } else if (sm_config["watchdog"].as<std::string>() == "disable") {
+        watchdog = EC_WD_DISABLE;
+      }
+    }
+
+    return true;
+  }
+
+  uint8_t index;
+  ec_direction_t type;
+  std::string pdo_name = "null";
+  ec_watchdog_mode_t watchdog = EC_WD_DEFAULT;
+};
+
+}  // namespace ethercat_interface
+#endif  // ETHERCAT_INTERFACE__EC_SYNC_MANAGER_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/package.xml

@@ -0,0 +1,20 @@
+<?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>ethercat_interface</name>
+  <version>1.2.0</version>
+  <description>Base implementations of EC interfaces</description>
+  <maintainer email="mcbed.robotics@gamil.com">Maciej Bednarczyk</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>pluginlib</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/src/ec_master.cpp

@@ -0,0 +1,514 @@
+// Copyright 2022 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include "ethercat_interface/ec_master.hpp"
+#include "ethercat_interface/ec_slave.hpp"
+
+#include <unistd.h>
+#include <sys/resource.h>
+#include <pthread.h>
+#include <sched.h>
+#include <signal.h>
+#include <time.h>
+#include <sys/mman.h>
+#include <string.h>
+#include <iostream>
+#include <sstream>
+
+#define EC_NEWTIMEVAL2NANO(TV) \
+  (((TV).tv_sec - 946684800ULL) * 1000000000ULL + (TV).tv_nsec)
+
+namespace ethercat_interface
+{
+
+EcMaster::DomainInfo::DomainInfo(ec_master_t * master)
+{
+  domain = ecrt_master_create_domain(master);
+  if (domain == NULL) {
+    printWarning("Failed to create domain");
+    return;
+  }
+
+  const ec_pdo_entry_reg_t empty = {0};
+  domain_regs.push_back(empty);
+}
+
+
+EcMaster::DomainInfo::~DomainInfo()
+{
+  for (Entry & entry : entries) {
+    delete[] entry.offset;
+    delete[] entry.bit_position;
+  }
+}
+
+
+EcMaster::EcMaster(const int master)
+{
+  master_ = ecrt_request_master(master);
+  if (master_ == NULL) {
+    printWarning("Failed to obtain master.");
+    return;
+  }
+  interval_ = 0;
+}
+
+EcMaster::~EcMaster()
+{
+  for (SlaveInfo & slave : slave_info_) {
+    //
+  }
+  for (auto & domain : domain_info_) {
+    if (domain.second != NULL) {
+      delete domain.second;
+    }
+  }
+}
+
+void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave * slave)
+{
+  // configure slave in master
+
+  SlaveInfo slave_info;
+  slave_info.slave = slave;
+  slave_info.config = ecrt_master_slave_config(
+    master_, alias, position,
+    slave->vendor_id_,
+    slave->product_id_);
+  if (slave_info.config == NULL) {
+    printWarning("Add slave. Failed to get slave configuration.");
+    return;
+  }
+
+  // check and setup dc
+
+  if (slave->assign_activate_dc_sync()) {
+    struct timespec t;
+    clock_gettime(CLOCK_MONOTONIC, &t);
+    ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t));
+    ecrt_slave_config_dc(
+      slave_info.config,
+      slave->assign_activate_dc_sync(),
+      interval_,
+      interval_ - (t.tv_nsec % (interval_)),
+      0,
+      0);
+  }
+
+  slave_info_.push_back(slave_info);
+
+  // check if slave has pdos
+  size_t num_syncs = slave->syncSize();
+  const ec_sync_info_t * syncs = slave->syncs();
+  if (num_syncs > 0) {
+    // configure pdos in slave
+    int pdos_status = ecrt_slave_config_pdos(slave_info.config, num_syncs, syncs);
+    if (pdos_status) {
+      printWarning("Add slave. Failed to configure PDOs");
+      return;
+    }
+  } else {
+    printWarning(
+      "Add slave. Sync size is zero for " +
+      std::to_string(alias) + ":" + std::to_string(position));
+  }
+
+  // check if slave registered any pdos for the domain
+  EcSlave::DomainMap domain_map;
+  slave->domains(domain_map);
+  for (auto & iter : domain_map) {
+    // get the domain info, create if necessary
+    uint32_t domain_index = iter.first;
+    DomainInfo * domain_info = NULL;
+    if (domain_info_.count(domain_index)) {
+      domain_info = domain_info_.at(domain_index);
+    }
+    if (domain_info == NULL) {
+      domain_info = new DomainInfo(master_);
+      domain_info_[domain_index] = domain_info;
+    }
+
+    registerPDOInDomain(
+      alias, position,
+      iter.second, domain_info,
+      slave);
+  }
+}
+
+int EcMaster::configSlaveSdo(
+  uint16_t slave_position, SdoConfigEntry sdo_config,
+  uint32_t * abort_code)
+{
+  uint8_t buffer[8];
+  sdo_config.buffer_write(buffer);
+  int ret = ecrt_master_sdo_download(
+    master_,
+    slave_position,
+    sdo_config.index,
+    sdo_config.sub_index,
+    buffer,
+    sdo_config.data_size(),
+    abort_code
+  );
+  return ret;
+}
+
+void EcMaster::registerPDOInDomain(
+  uint16_t alias, uint16_t position,
+  std::vector<uint32_t> & channel_indices,
+  DomainInfo * domain_info,
+  EcSlave * slave)
+{
+  // expand the size of the domain
+  uint32_t num_pdo_regs = channel_indices.size();
+  size_t start_index = domain_info->domain_regs.size() - 1;  // empty element at end
+  domain_info->domain_regs.resize(domain_info->domain_regs.size() + num_pdo_regs);
+
+  // create a new entry in the domain
+  DomainInfo::Entry domain_entry;
+  domain_entry.slave = slave;
+  domain_entry.num_pdos = num_pdo_regs;
+  domain_entry.offset = new uint32_t[num_pdo_regs];
+  domain_entry.bit_position = new uint32_t[num_pdo_regs];
+  domain_info->entries.push_back(domain_entry);
+
+  EcSlave::DomainMap domain_map;
+  slave->domains(domain_map);
+
+  // add to array of pdos registrations
+  const ec_pdo_entry_info_t * pdo_regs = slave->channels();
+  for (size_t i = 0; i < num_pdo_regs; ++i) {
+    // create pdo entry in the domain
+    ec_pdo_entry_reg_t & pdo_reg = domain_info->domain_regs[start_index + i];
+    pdo_reg.alias = alias;
+    pdo_reg.position = position;
+    pdo_reg.vendor_id = slave->vendor_id_;
+    pdo_reg.product_code = slave->product_id_;
+    pdo_reg.index = pdo_regs[channel_indices[i]].index;
+    pdo_reg.subindex = pdo_regs[channel_indices[i]].subindex;
+    pdo_reg.offset = &(domain_entry.offset[i]);
+    pdo_reg.bit_position = &(domain_entry.bit_position[i]);
+
+
+    // print the domain pdo entry
+    std::cout << "{" << pdo_reg.alias << ", " << pdo_reg.position;
+    std::cout << ", 0x" << std::hex << pdo_reg.vendor_id;
+    std::cout << ", 0x" << std::hex << pdo_reg.product_code;
+    std::cout << ", 0x" << std::hex << pdo_reg.index;
+    std::cout << ", 0x" << std::hex << static_cast<int>(pdo_reg.subindex);
+    std::cout << "}" << std::dec << std::endl;
+  }
+
+  // set the last element to null
+  ec_pdo_entry_reg_t empty = {0};
+  domain_info->domain_regs.back() = empty;
+}
+
+bool EcMaster::activate()
+{
+  // register domain
+  for (auto & iter : domain_info_) {
+    DomainInfo * domain_info = iter.second;
+    if (domain_info == NULL) {
+      throw std::runtime_error("Null domain info: " + std::to_string(iter.first));
+    }
+    bool domain_status = ecrt_domain_reg_pdo_entry_list(
+      domain_info->domain,
+      &(domain_info->domain_regs[0]));
+    if (domain_status) {
+      printWarning("Activate. Failed to register domain PDO entries.");
+      return false;
+    }
+  }
+  // set application time
+  struct timespec t;
+  clock_gettime(CLOCK_MONOTONIC, &t);
+  ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t));
+
+  // activate master
+  bool activate_status = ecrt_master_activate(master_);
+  if (activate_status) {
+    printWarning("Activate. Failed to activate master.");
+    return false;
+  }
+
+  // retrieve domain data
+  for (auto & iter : domain_info_) {
+    DomainInfo * domain_info = iter.second;
+    if (domain_info == NULL) {
+      throw std::runtime_error("Null domain info: " + std::to_string(iter.first));
+    }
+    domain_info->domain_pd = ecrt_domain_data(domain_info->domain);
+    if (domain_info->domain_pd == NULL) {
+      printWarning("Activate. Failed to retrieve domain process data.");
+      return false;
+    }
+  }
+  return true;
+}
+
+void EcMaster::update(uint32_t domain)
+{
+  // receive process data
+  ecrt_master_receive(master_);
+
+  DomainInfo * domain_info = domain_info_.at(domain);
+  if (domain_info == NULL) {
+    throw std::runtime_error("Null domain info: " + std::to_string(domain));
+  }
+
+  ecrt_domain_process(domain_info->domain);
+
+  // check process data state (optional)
+  checkDomainState(domain);
+
+  // check for master and slave state change
+  if (update_counter_ % check_state_frequency_ == 0) {
+    checkMasterState();
+    checkSlaveStates();
+  }
+
+  // read and write process data
+  for (DomainInfo::Entry & entry : domain_info->entries) {
+    for (int i = 0; i < entry.num_pdos; ++i) {
+      (entry.slave)->processData(i, domain_info->domain_pd + entry.offset[i]);
+    }
+  }
+
+  struct timespec t;
+
+  clock_gettime(CLOCK_REALTIME, &t);
+  ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t));
+  ecrt_master_sync_reference_clock(master_);
+  ecrt_master_sync_slave_clocks(master_);
+
+  // send process data
+  ecrt_domain_queue(domain_info->domain);
+  ecrt_master_send(master_);
+
+  ++update_counter_;
+}
+
+void EcMaster::readData(uint32_t domain)
+{
+  // receive process data
+  ecrt_master_receive(master_);
+
+  DomainInfo * domain_info = domain_info_.at(domain);
+  if (domain_info == NULL) {
+    throw std::runtime_error("Null domain info: " + std::to_string(domain));
+  }
+
+  ecrt_domain_process(domain_info->domain);
+
+  // check process data state (optional)
+  checkDomainState(domain);
+
+  // check for master and slave state change
+  if (update_counter_ % check_state_frequency_ == 0) {
+    checkMasterState();
+    checkSlaveStates();
+  }
+
+  // read and write process data
+  for (DomainInfo::Entry & entry : domain_info->entries) {
+    for (int i = 0; i < entry.num_pdos; ++i) {
+      (entry.slave)->processData(i, domain_info->domain_pd + entry.offset[i]);
+    }
+  }
+
+  ++update_counter_;
+}
+
+void EcMaster::writeData(uint32_t domain)
+{
+  DomainInfo * domain_info = domain_info_.at(domain);
+  if (domain_info == NULL) {
+    throw std::runtime_error("Null domain info: " + std::to_string(domain));
+  }
+
+  // read and write process data
+  for (DomainInfo::Entry & entry : domain_info->entries) {
+    for (int i = 0; i < entry.num_pdos; ++i) {
+      (entry.slave)->processData(i, domain_info->domain_pd + entry.offset[i]);
+    }
+  }
+
+  struct timespec t;
+
+  clock_gettime(CLOCK_REALTIME, &t);
+  ecrt_master_application_time(master_, EC_NEWTIMEVAL2NANO(t));
+  ecrt_master_sync_reference_clock(master_);
+  ecrt_master_sync_slave_clocks(master_);
+
+  // send process data
+  ecrt_domain_queue(domain_info->domain);
+  ecrt_master_send(master_);
+}
+
+void EcMaster::setCtrlCHandler(SIMPLECAT_EXIT_CALLBACK user_callback)
+{
+  // ctrl c handler
+  struct sigaction sigIntHandler;
+  sigIntHandler.sa_handler = user_callback;
+  sigemptyset(&sigIntHandler.sa_mask);
+  sigIntHandler.sa_flags = 0;
+  sigaction(SIGINT, &sigIntHandler, NULL);
+}
+
+void EcMaster::run(SIMPLECAT_CONTRL_CALLBACK user_callback)
+{
+  // start after one second
+  struct timespec t;
+  clock_gettime(CLOCK_MONOTONIC, &t);
+  t.tv_sec++;
+
+  running_ = true;
+  start_t_ = std::chrono::system_clock::now();
+  while (running_) {
+    // wait until next shot
+    clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL);
+
+    // update EtherCAT bus
+    this->update();
+
+    // get actual time
+    curr_t_ = std::chrono::system_clock::now();
+
+    // user callback
+    user_callback();
+
+    // calculate next shot. carry over nanoseconds into microseconds.
+    t.tv_nsec += interval_;
+    while (t.tv_nsec >= 1000000000) {
+      t.tv_nsec -= 1000000000;
+      t.tv_sec++;
+    }
+  }
+}
+
+double EcMaster::elapsedTime()
+{
+  std::chrono::duration<double> elapsed_seconds = curr_t_ - start_t_;
+  return elapsed_seconds.count() - 1.0;  // started after 1 second
+}
+
+uint64_t EcMaster::elapsedCycles()
+{
+  return update_counter_;
+}
+
+void EcMaster::setThreadHighPriority()
+{
+  pid_t pid = getpid();
+  int priority_status = setpriority(PRIO_PROCESS, pid, -19);
+  if (priority_status) {
+    printWarning("setThreadHighPriority. Failed to set priority.");
+    return;
+  }
+}
+
+void EcMaster::setThreadRealTime()
+{
+  /* Declare ourself as a real time task, priority 49.
+      PRREMPT_RT uses priority 50
+      for kernel tasklets and interrupt handler by default */
+  struct sched_param param;
+  param.sched_priority = 49;
+  // pthread_t this_thread = pthread_self();
+  if (sched_setscheduler(0, SCHED_FIFO, &param) == -1) {
+    perror("sched_setscheduler failed");
+    exit(-1);
+  }
+
+  /* Lock memory */
+  if (mlockall(MCL_CURRENT | MCL_FUTURE) == -1) {
+    perror("mlockall failed");
+    exit(-2);
+  }
+
+  /* Pre-fault our stack
+      8*1024 is the maximum stack size
+      which is guaranteed safe to access without faulting */
+  int MAX_SAFE_STACK = 8 * 1024;
+  unsigned char dummy[MAX_SAFE_STACK];
+  memset(dummy, 0, MAX_SAFE_STACK);
+}
+
+void EcMaster::checkDomainState(uint32_t domain)
+{
+  DomainInfo * domain_info = domain_info_.at(domain);
+  if (domain_info == NULL) {
+    throw std::runtime_error("Null domain info: " + std::to_string(domain));
+  }
+
+  ec_domain_state_t ds;
+  ecrt_domain_state(domain_info->domain, &ds);
+
+  if (ds.working_counter != domain_info->domain_state.working_counter) {
+    printf("Domain: WC %u.\n", ds.working_counter);
+  }
+  if (ds.wc_state != domain_info->domain_state.wc_state) {
+    printf("Domain: State %u.\n", ds.wc_state);
+  }
+  domain_info->domain_state = ds;
+}
+
+
+void EcMaster::checkMasterState()
+{
+  ec_master_state_t ms;
+  ecrt_master_state(master_, &ms);
+
+  if (ms.slaves_responding != master_state_.slaves_responding) {
+    printf("%u slave(s).\n", ms.slaves_responding);
+  }
+  if (ms.al_states != master_state_.al_states) {
+    printf("Master AL states: 0x%02X.\n", ms.al_states);
+  }
+  if (ms.link_up != master_state_.link_up) {
+    printf("Link is %s.\n", ms.link_up ? "up" : "down");
+  }
+  master_state_ = ms;
+}
+
+
+void EcMaster::checkSlaveStates()
+{
+  for (SlaveInfo & slave : slave_info_) {
+    ec_slave_config_state_t s;
+    ecrt_slave_config_state(slave.config, &s);
+
+    if (s.al_state != slave.config_state.al_state) {
+      // this spams the terminal at initialization.
+      printf("Slave: State 0x%02X.\n", s.al_state);
+    }
+    if (s.online != slave.config_state.online) {
+      printf("Slave: %s.\n", s.online ? "online" : "offline");
+    }
+    if (s.operational != slave.config_state.operational) {
+      printf("Slave: %soperational.\n", s.operational ? "" : "Not ");
+      slave.slave->set_state_is_operational(s.operational ? true : false);
+    }
+    slave.config_state = s;
+  }
+}
+
+
+void EcMaster::printWarning(const std::string & message)
+{
+  std::cout << "WARNING. Master. " << message << std::endl;
+}
+
+}  // namespace ethercat_interface

HiveCoreR0/src/ethercat_driver_ros2/ethercat_interface/test/test_ec_pdo_channel_manager.cpp

@@ -0,0 +1,144 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <gtest/gtest.h>
+#include <memory>
+
+#include "ethercat_interface/ec_pdo_channel_manager.hpp"
+#include "yaml-cpp/yaml.h"
+
+TEST(TestEcPdoChannelManager, LoadFromConfig)
+{
+  const char channel_config[] =
+    R"(
+      {index: 0x6071, sub_index: 0, type: int16, command_interface: effort, default: -5, factor: 2, offset: 10}
+    )";
+  YAML::Node config = YAML::Load(channel_config);
+  ethercat_interface::EcPdoChannelManager pdo_manager;
+  pdo_manager.pdo_type = ethercat_interface::PdoType::RPDO;
+  pdo_manager.load_from_config(config);
+
+  ASSERT_EQ(pdo_manager.index, 0x6071);
+  ASSERT_EQ(pdo_manager.sub_index, 0);
+  ASSERT_EQ(pdo_manager.data_type, "int16");
+  ASSERT_EQ(pdo_manager.interface_name, "effort");
+  ASSERT_EQ(pdo_manager.default_value, -5);
+  ASSERT_EQ(pdo_manager.factor, 2);
+  ASSERT_EQ(pdo_manager.offset, 10);
+}
+
+TEST(TestEcPdoChannelManager, EcReadS16)
+{
+  const char channel_config[] =
+    R"(
+      {index: 0x6071, sub_index: 0, type: int16, command_interface: effort, default: -5, factor: 2, offset: 10}
+    )";
+  YAML::Node config = YAML::Load(channel_config);
+  ethercat_interface::EcPdoChannelManager pdo_manager;
+  pdo_manager.pdo_type = ethercat_interface::PdoType::RPDO;
+  pdo_manager.load_from_config(config);
+
+  uint8_t buffer[16];
+  EC_WRITE_S16(buffer, 42);
+  ASSERT_EQ(pdo_manager.ec_read(buffer), 2 * 42 + 10);
+}
+
+TEST(TestEcPdoChannelManager, EcReadWriteBit2)
+{
+  const char channel_config[] =
+    R"(
+      {index: 0x6071, sub_index: 0, type: bit2, mask: 3}
+    )";
+  YAML::Node config = YAML::Load(channel_config);
+  ethercat_interface::EcPdoChannelManager pdo_manager;
+  pdo_manager.pdo_type = ethercat_interface::PdoType::RPDO;
+  pdo_manager.load_from_config(config);
+
+  ASSERT_EQ(pdo_manager.data_type, "bit2");
+  ASSERT_EQ(pdo_manager.data_mask, 3);
+  ASSERT_EQ(pdo_manager.type2bits(pdo_manager.data_type), 2);
+
+  uint8_t buffer[1];
+  EC_WRITE_U8(buffer, 0);
+  ASSERT_EQ(pdo_manager.ec_read(buffer), 0);
+  EC_WRITE_U8(buffer, 3);
+  ASSERT_EQ(pdo_manager.ec_read(buffer), 3);
+  EC_WRITE_U8(buffer, 5);
+  ASSERT_EQ(pdo_manager.ec_read(buffer), 1);
+
+  pdo_manager.ec_write(buffer, 0);
+  ASSERT_EQ(EC_READ_U8(buffer), 0);
+  pdo_manager.ec_write(buffer, 2);
+  ASSERT_EQ(EC_READ_U8(buffer), 2);
+  pdo_manager.ec_write(buffer, 5);
+  ASSERT_EQ(EC_READ_U8(buffer), 1);
+}
+
+TEST(TestEcPdoChannelManager, EcReadWriteBoolMask1)
+{
+  const char channel_config[] =
+    R"(
+      {index: 0x6071, sub_index: 0, type: bool, mask: 1}
+    )";
+  YAML::Node config = YAML::Load(channel_config);
+  ethercat_interface::EcPdoChannelManager pdo_manager;
+  pdo_manager.pdo_type = ethercat_interface::PdoType::RPDO;
+  pdo_manager.load_from_config(config);
+
+  ASSERT_EQ(pdo_manager.data_type, "bool");
+  ASSERT_EQ(pdo_manager.data_mask, 1);
+  ASSERT_EQ(pdo_manager.type2bits(pdo_manager.data_type), 1);
+
+  uint8_t buffer[1];
+  EC_WRITE_U8(buffer, 3);
+  ASSERT_EQ(pdo_manager.ec_read(buffer), 1);
+  EC_WRITE_U8(buffer, 0);
+  ASSERT_EQ(pdo_manager.ec_read(buffer), 0);
+
+  pdo_manager.ec_write(buffer, 0);
+  ASSERT_EQ(EC_READ_U8(buffer), 0);
+  pdo_manager.ec_write(buffer, 5);
+  ASSERT_EQ(EC_READ_U8(buffer), 1);
+}
+
+TEST(TestEcPdoChannelManager, EcReadWriteBoolMask5)
+{
+  const char channel_config[] =
+    R"(
+      {index: 0x6071, sub_index: 0, type: bool, mask: 5}
+    )";
+  YAML::Node config = YAML::Load(channel_config);
+  ethercat_interface::EcPdoChannelManager pdo_manager;
+  pdo_manager.pdo_type = ethercat_interface::PdoType::RPDO;
+  pdo_manager.load_from_config(config);
+
+  ASSERT_EQ(pdo_manager.data_type, "bool");
+  ASSERT_EQ(pdo_manager.data_mask, 5);
+  ASSERT_EQ(pdo_manager.type2bits(pdo_manager.data_type), 1);
+
+  uint8_t buffer[1];
+  EC_WRITE_U8(buffer, 7);
+  ASSERT_EQ(pdo_manager.ec_read(buffer), 1);
+  EC_WRITE_U8(buffer, 0);
+  ASSERT_EQ(pdo_manager.ec_read(buffer), 0);
+
+  pdo_manager.ec_write(buffer, 0);
+  ASSERT_EQ(EC_READ_U8(buffer), 0);
+  pdo_manager.ec_write(buffer, 3);
+  ASSERT_EQ(EC_READ_U8(buffer), 1);
+  pdo_manager.ec_write(buffer, 7);
+  ASSERT_EQ(EC_READ_U8(buffer), 5);
+  pdo_manager.ec_write(buffer, 5);
+  ASSERT_EQ(EC_READ_U8(buffer), 5);
+}

HiveCoreR0/src/ethercat_driver_ros2/ethercat_manager/CMakeLists.txt

@@ -0,0 +1,51 @@
+cmake_minimum_required(VERSION 3.5)
+project(ethercat_manager)
+
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(ethercat_msgs REQUIRED)
+
+# EtherLab
+set(ETHERLAB_DIR /usr/local/etherlab)
+set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
+
+find_library(ETHERCAT_LIB ethercat HINTS ${ETHERLAB_DIR}/lib)
+
+ament_export_include_directories(
+  include
+  ${ETHERLAB_DIR}/include
+)
+
+add_executable(
+  ethercat_sdo_srv_server
+  src/ethercat_sdo_srv_server.cpp)
+target_include_directories(
+  ethercat_sdo_srv_server
+  PRIVATE
+  include
+  ${ETHERLAB_DIR}/include
+)
+target_link_libraries(ethercat_sdo_srv_server ${ETHERCAT_LIB})
+ament_target_dependencies(ethercat_sdo_srv_server rclcpp ethercat_msgs)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+install(TARGETS
+  ethercat_sdo_srv_server
+  DESTINATION lib/${PROJECT_NAME})
+
+  ## EXPORTS
+ament_export_include_directories(
+  include
+)
+ament_export_libraries(
+  ethercat_sdo_srv_server
+  ${ETHERCAT_LIBRARY}
+)
+ament_export_dependencies(
+  rclcpp
+)
+ament_package()

HiveCoreR0/src/ethercat_driver_ros2/ethercat_manager/include/ethercat_manager/data_convertion_tools.hpp

@@ -0,0 +1,421 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// 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.
+//
+// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com)
+
+#ifndef ETHERCAT_MANAGER__DATA_CONVERTION_TOOLS_HPP_
+#define ETHERCAT_MANAGER__DATA_CONVERTION_TOOLS_HPP_
+
+#include <stdint.h>
+#include <string>
+#include <stdexcept>
+#include <ostream>
+#include <iostream>
+#include "rclcpp/rclcpp.hpp"
+
+namespace ethercat_manager
+{
+
+class SizeException
+  : public std::runtime_error
+{
+public:
+  explicit SizeException(const std::string & msg)
+  : std::runtime_error(msg) {}
+};
+
+struct DataType
+{
+  const char * name;
+  uint16_t code;
+  size_t byteSize;
+};
+
+static const DataType dataTypes[] = {
+  {"bool", 0x0001, 1},
+  {"int8", 0x0002, 1},
+  {"int16", 0x0003, 2},
+  {"int32", 0x0004, 4},
+  {"uint8", 0x0005, 1},
+  {"uint16", 0x0006, 2},
+  {"uint32", 0x0007, 4},
+  {"float", 0x0008, 4},
+  {"string", 0x0009, 0},           // a. k. a. visible_string
+  {"octet_string", 0x000a, 0},
+  {"unicode_string", 0x000b, 0},
+  // ... not implemented yet
+  {"int24", 0x0010, 3},
+  {"double", 0x0011, 8},
+  {"int40", 0x0012, 5},
+  {"int48", 0x0013, 6},
+  {"int56", 0x0014, 7},
+  {"int64", 0x0015, 8},
+  {"uint24", 0x0016, 3},
+  // reserved        0x0017
+  {"uint40", 0x0018, 5},
+  {"uint48", 0x0019, 6},
+  {"uint56", 0x001a, 7},
+  {"uint64", 0x001b, 8},
+  // reserved        0x001c-0x001f
+  {"sm8", 0xfffb, 1},              // sign-and-magnitude coding
+  {"sm16", 0xfffc, 2},             // sign-and-magnitude coding
+  {"sm32", 0xfffd, 4},             // sign-and-magnitude coding
+  {"sm64", 0xfffe, 8},             // sign-and-magnitude coding
+  {"raw", 0xffff, 0},
+  {}
+};
+
+static const DataType * get_data_type(const std::string & str)
+{
+  const DataType * d;
+  for (d = dataTypes; d->name; d++) {
+    if (str == d->name) {
+      return d;
+    }
+  }
+  return NULL;
+}
+
+static const DataType * get_data_type(uint16_t code)
+{
+  const DataType * d;
+  for (d = dataTypes; d->name; d++) {
+    if (code == d->code) {
+      return d;
+    }
+  }
+  return NULL;
+}
+
+static void buffer2raw(std::ostream & o, const uint8_t * data, size_t size)
+{
+  o << std::hex << std::setfill('0');
+  while (size--) {
+    o << " 0x" << std::setw(2) << (unsigned int) *data++;
+  }
+}
+
+static size_t data2buffer(
+  const DataType * type, const std::string & source, void * target,
+  size_t targetSize)
+{
+  std::stringstream str;
+  size_t dataSize = type->byteSize;
+
+  memset(target, 0, targetSize);
+
+  str << source;
+  str >> resetiosflags(std::ios::basefield);    // guess base from prefix
+  str.exceptions(std::ios::failbit);
+
+  switch (type->code) {
+    case 0x0001:     // bool
+      {
+        int16_t val;         // uint8_t is interpreted as char
+        str >> val;
+        if (val > 1 || val < 0) {
+          throw  std::ios::failure("Value out of range");
+        }
+        *reinterpret_cast<uint8_t *>(target) = val;
+        break;
+      }
+    case 0x0002:     // int8
+      {
+        int16_t val;         // uint8_t is interpreted as char
+        str >> val;
+        if (val > 127 || val < -128) {
+          throw  std::ios::failure("Value out of range");
+        }
+        *reinterpret_cast<uint8_t *>(target) = val;
+        break;
+      }
+    case 0x0003:     // int16
+      {
+        int16_t val;
+        str >> val;
+        *reinterpret_cast<int16_t *>(target) = cpu_to_le16(val);
+        break;
+      }
+    case 0x0004:     // int32
+      {
+        int32_t val;
+        str >> val;
+        *reinterpret_cast<int32_t *>(target) = cpu_to_le32(val);
+        break;
+      }
+    case 0x0005:     // uint8
+      {
+        uint16_t val;         // uint8_t is interpreted as char
+        str >> val;
+        if (val > 0xff) {
+          throw  std::ios::failure("Value out of range");
+        }
+        *reinterpret_cast<uint8_t *>(target) = val;
+        break;
+      }
+    case 0x0006:     // uint16
+      {
+        uint16_t val;
+        str >> val;
+        *reinterpret_cast<uint16_t *>(target) = cpu_to_le16(val);
+        break;
+      }
+    case 0x0007:     // uint32
+      {
+        uint32_t val;
+        str >> val;
+        *reinterpret_cast<uint32_t *>(target) = cpu_to_le32(val);
+        break;
+      }
+    case 0x0008:     // float
+      {
+        float val;
+        str >> val;
+        *reinterpret_cast<uint32_t *>(target) =
+          cpu_to_le32(*reinterpret_cast<uint32_t *>(reinterpret_cast<void *>( &val)));
+        break;
+      }
+    case 0x0009:     // std::string
+    case 0x000a:     // octet_string
+    case 0x000b:     // unicode_string
+      dataSize = str.str().size();
+      if (dataSize > targetSize) {
+        std::stringstream err;
+        err << "String too large ("
+            << dataSize << " > " << targetSize << ")";
+        throw SizeException(err.str());
+      }
+      str.read(reinterpret_cast<char *>(target), dataSize);
+      break;
+    case 0x0011:     // double
+      {
+        double val;
+        str >> val;
+        *reinterpret_cast<uint64_t *>(target) =
+          cpu_to_le64(*reinterpret_cast<uint64_t *>(reinterpret_cast<void *>(&val)));
+        break;
+      }
+      break;
+    case 0x0015:     // int64
+      {
+        int64_t val;
+        str >> val;
+        *reinterpret_cast<int64_t *>(target) = cpu_to_le64(val);
+        break;
+      }
+      break;
+    case 0x001b:     // uint64
+      {
+        uint64_t val;
+        str >> val;
+        *reinterpret_cast<uint64_t *>(target) = cpu_to_le64(val);
+        break;
+      }
+      break;
+
+    case 0x0010:     // int24
+    case 0x0012:     // int40
+    case 0x0013:     // int48
+    case 0x0014:     // int56
+    case 0x0016:     // uint24
+    case 0x0018:     // uint40
+    case 0x0019:     // uint48
+    case 0x001a:     // uint56
+      {
+        std::stringstream err;
+        err << "Non-native integer type " << type->name
+            << " is not yet implemented.";
+        throw std::runtime_error(err.str());
+      }
+
+    case 0xfffb:     // sm8
+    case 0xfffc:     // sm16
+    case 0xfffd:     // sm32
+    case 0xfffe:     // sm64
+      {
+        std::stringstream err;
+        err << "Sign-and-magitude types not yet"
+          " implemented for input direction.";
+        throw std::runtime_error(err.str());
+      }
+
+    default:
+      {
+        std::stringstream err;
+        err << "Unknown data type 0x" << std::hex << type->code;
+        throw std::runtime_error(err.str());
+      }
+  }
+
+  return dataSize;
+}
+
+static void buffer2data(
+  std::ostream & o, double & value,
+  const DataType * type, void * data, size_t dataSize)
+{
+  uint16_t typeCode;
+
+  if (type) {
+    if (type->byteSize && dataSize != type->byteSize) {
+      std::stringstream err;
+      err << "Data type mismatch. Expected " << type->name
+          << " with " << type->byteSize << " byte, but got "
+          << dataSize << " byte.";
+      throw SizeException(err.str());
+    }
+    typeCode = type->code;
+  } else {
+    typeCode = 0xffff;     // raw data
+  }
+
+  o << std::setfill('0');
+
+  switch (typeCode) {
+    case 0x0001:     // bool
+      {
+        int val = static_cast<int>(*reinterpret_cast<int8_t *>(data));
+        o << "0x" << std::hex << std::setw(2) << val;
+        value = static_cast<double>(val);
+      }
+      break;
+    case 0x0002:     // int8
+      {
+        int val = static_cast<int>(*reinterpret_cast<int8_t *>(data));
+        o << "0x" << std::hex << std::setw(2) << val;
+        value = static_cast<double>(val);
+      }
+      break;
+    case 0x0003:     // int16
+      {
+        int16_t val = le16_to_cpup(data);
+        o << "0x" << std::hex << std::setw(4) << val;
+        value = static_cast<double>(val);
+      }
+      break;
+    case 0x0004:     // int32
+      {
+        int32_t val = le32_to_cpup(data);
+        o << "0x" << std::hex << std::setw(8) << val;
+        value = static_cast<double>(val);
+      }
+      break;
+    case 0x0005:     // uint8
+      {
+        unsigned int val = static_cast<unsigned int>(*reinterpret_cast<uint8_t *>(data));
+        o << "0x" << std::hex << std::setw(2) << val;
+        value = static_cast<double>(val);
+      }
+      break;
+    case 0x0006:     // uint16
+      {
+        uint16_t val = le16_to_cpup(data);
+        o << "0x" << std::hex << std::setw(4) << val;
+        value = static_cast<double>(val);
+      }
+      break;
+    case 0x0007:     // uint32
+      {
+        uint32_t val = le32_to_cpup(data);
+        o << "0x" << std::hex << std::setw(8) << val;
+        value = static_cast<double>(val);
+      }
+      break;
+    case 0x0008:     // float
+      {
+        uint32_t val = le32_to_cpup(data);
+        float fval = *reinterpret_cast<float *>(reinterpret_cast<void *>(&val));
+        o << fval;
+        value = static_cast<double>(val);
+      }
+      break;
+    case 0x0009:     // string
+      o << std::string(reinterpret_cast<const char *>(data), dataSize);
+      break;
+    case 0x000a:     // octet_string
+      o << std::string(reinterpret_cast<const char *>(data), dataSize) << std::flush;
+      break;
+    case 0x000b:     // unicode_string
+                     // FIXME encoding
+      o << std::string(reinterpret_cast<const char *>(data), dataSize);
+      break;
+    case 0x0011:     // double
+      {
+        uint64_t val = le64_to_cpup(data);
+        double fval = *reinterpret_cast<double *>(reinterpret_cast<void *>(&val));
+        o << fval;
+        value = static_cast<double>(val);
+      }
+      break;
+    case 0x0015:     // int64
+      {
+        int64_t val = le64_to_cpup(data);
+        o << "0x" << std::hex << std::setw(16) << val;
+        value = static_cast<double>(val);
+      }
+      break;
+    case 0x001b:     // uint64
+      {
+        uint64_t val = le64_to_cpup(data);
+        o << "0x" << std::hex << std::setw(16) << val;
+        value = static_cast<double>(val);
+      }
+      break;
+    case 0xfffb:     // sm8
+      {
+        int8_t val = *reinterpret_cast<uint8_t *>(data);
+        int8_t smval = val < 0 ? (val & 0x7f) * -1 : val;
+
+        o << "0x" << std::hex << std::setw(2) << static_cast<int>(val);
+        value = static_cast<double>(static_cast<int>(smval));
+      }
+      break;
+    case 0xfffc:     // sm16
+      {
+        int16_t val = le16_to_cpup(data);
+        int16_t smval = val < 0 ? (val & 0x7fff) * -1 : val;
+
+        o << "0x" << std::hex << std::setw(4) << val;
+        value = static_cast<double>(smval);
+      }
+      break;
+    case 0xfffd:     // sm32
+      {
+        int32_t val = le32_to_cpup(data);
+        int32_t smval = val < 0 ? (val & 0x7fffffffUL) * -1 : val;
+
+        o << "0x" << std::hex << std::setw(8) << val;
+        value = static_cast<double>(smval);
+      }
+      break;
+    case 0xfffe:     // sm64
+      {
+        int64_t val = le64_to_cpup(data);
+        int64_t smval =
+          val < 0 ? (val & 0x7fffffffffffffffULL) * -1 : val;
+
+        o << "0x" << std::hex << std::setw(16) << val;
+        value = static_cast<double>(smval);
+      }
+      break;
+
+    default:
+      buffer2raw(o, (const uint8_t *) data, dataSize);       // FIXME
+      break;
+  }
+}
+
+}  // namespace ethercat_manager
+
+#endif  // ETHERCAT_MANAGER__DATA_CONVERTION_TOOLS_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_manager/include/ethercat_manager/ec_master_async.hpp

@@ -0,0 +1,175 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// 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.
+//
+// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com)
+
+#ifndef ETHERCAT_MANAGER__EC_MASTER_ASYNC_HPP_
+#define ETHERCAT_MANAGER__EC_MASTER_ASYNC_HPP_
+
+#include <sys/stat.h>
+#include <fcntl.h>
+#include <sys/ioctl.h>
+#include <stdio.h>
+#include <ecrt.h>
+#include <errno.h>
+#include <sstream>
+#include <map>
+#include <string>
+#include "rclcpp/rclcpp.hpp"
+#include "ethercat_manager/ec_master_async_io.hpp"
+
+
+namespace ethercat_manager
+{
+
+class MasterException
+  : public std::runtime_error
+{
+public:
+  explicit MasterException(const std::string & msg)
+  : std::runtime_error(msg) {}
+};
+
+class EcMasterAsync
+{
+public:
+  explicit EcMasterAsync(uint16_t master_index = 0)
+  {
+    index_ = master_index;
+    mcount_ = 0;
+    fd_ = -1;
+  }
+  ~EcMasterAsync() {close();}
+
+  void close()
+  {
+    if (fd_ != -1) {
+      ::close(fd_);
+      fd_ = -1;
+    }
+  }
+
+  enum Permissions {Read, ReadWrite};
+
+  void open(Permissions perm)
+  {
+    std::stringstream deviceName;
+
+    if (fd_ == -1) {  // not already open
+      ec_ioctl_module_t module_data;
+      deviceName << "/dev/EtherCAT" << index_;
+
+      if ((fd_ = ::open(
+          deviceName.str().c_str(),
+          perm == ReadWrite ? O_RDWR : O_RDONLY)) == -1)
+      {
+        std::stringstream err;
+        err << "Failed to open master device " << deviceName.str() << ": "
+            << strerror(errno);
+        throw MasterException(err.str());
+      }
+
+      getModule(&module_data);
+      if (module_data.ioctl_version_magic != EC_IOCTL_VERSION_MAGIC) {
+        std::stringstream err;
+        err << "ioctl() version magic is differing: "
+            << deviceName.str() << ": " << module_data.ioctl_version_magic
+            << ", ethercat tool: " << EC_IOCTL_VERSION_MAGIC;
+        throw MasterException(err.str());
+      }
+      mcount_ = module_data.master_count;
+    }
+  }
+
+  void sdo_download(ec_ioctl_slave_sdo_download_t * data)
+  {
+    if (ioctl(fd_, EC_IOCTL_SLAVE_SDO_DOWNLOAD, data) < 0) {
+      std::stringstream err;
+      if (errno == EIO && data->abort_code) {
+        err << "SDO transfer aborted: " << abort_code_map_.find(data->abort_code)->second;
+        throw MasterException(err.str());
+      } else {
+        err << "Failed to download SDO: " << strerror(errno);
+        throw MasterException(err.str());
+      }
+    }
+  }
+
+  void sdo_upload(ec_ioctl_slave_sdo_upload_t * data)
+  {
+    if (ioctl(fd_, EC_IOCTL_SLAVE_SDO_UPLOAD, data) < 0) {
+      std::stringstream err;
+      if (errno == EIO && data->abort_code) {
+        err << "SDO transfer aborted: " << abort_code_map_.find(data->abort_code)->second;
+        throw MasterException(err.str());
+      } else {
+        err << "Failed to upload SDO: " << strerror(errno);
+        throw MasterException(err.str());
+      }
+    }
+  }
+
+  void getModule(ec_ioctl_module_t * data)
+  {
+    if (ioctl(fd_, EC_IOCTL_MODULE, data) < 0) {
+      std::stringstream err;
+      err << "Failed to get module information: " << strerror(errno);
+      throw MasterException(err.str());
+    }
+  }
+
+private:
+  unsigned int index_;
+  unsigned int mcount_;
+  int fd_;
+
+  const std::map<uint32_t, std::string> abort_code_map_ = {
+    {0x05030000, "Toggle bit not changed"},
+    {0x05040000, "SDO protocol timeout"},
+    {0x05040001, "Client/Server command specifier not valid or unknown"},
+    {0x05040005, "Out of memory"},
+    {0x06010000, "Unsupported access to an object"},
+    {0x06010001, "Attempt to read a write-only object"},
+    {0x06010002, "Attempt to write a read-only object"},
+    {0x06020000, "This object does not exist in the object directory"},
+    {0x06040041, "The object cannot be mapped into the PDO"},
+    {0x06040042, "The number and length of the objects to be mapped would"
+      " exceed the PDO length"},
+    {0x06040043, "General parameter incompatibility reason"},
+    {0x06040047, "General internal incompatibility in device"},
+    {0x06060000, "Access failure due to a hardware error"},
+    {0x06070010, "Data type does not match, length of service parameter does"
+      " not match"},
+    {0x06070012, "Data type does not match, length of service parameter too"
+      " high"},
+    {0x06070013, "Data type does not match, length of service parameter too"
+      " low"},
+    {0x06090011, "Subindex does not exist"},
+    {0x06090030, "Value range of parameter exceeded"},
+    {0x06090031, "Value of parameter written too high"},
+    {0x06090032, "Value of parameter written too low"},
+    {0x06090036, "Maximum value is less than minimum value"},
+    {0x08000000, "General error"},
+    {0x08000020, "Data cannot be transferred or stored to the application"},
+    {0x08000021, "Data cannot be transferred or stored to the application"
+      " because of local control"},
+    {0x08000022, "Data cannot be transferred or stored to the application"
+      " because of the present device state"},
+    {0x08000023, "Object dictionary dynamic generation fails or no object"
+      " dictionary is present"},
+    {}
+  };
+};
+}  // namespace ethercat_manager
+#endif  // ETHERCAT_MANAGER__EC_MASTER_ASYNC_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_manager/include/ethercat_manager/ec_master_async_io.hpp

@@ -0,0 +1,62 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ETHERCAT_MANAGER__EC_MASTER_ASYNC_IO_HPP_
+#define ETHERCAT_MANAGER__EC_MASTER_ASYNC_IO_HPP_
+
+#define EC_IOCTL_TYPE 0xa4
+#define EC_IO(nr)  _IO(EC_IOCTL_TYPE, nr)
+#define EC_IOR(nr, type)  _IOR(EC_IOCTL_TYPE, nr, type)
+#define EC_IOW(nr, type)  _IOW(EC_IOCTL_TYPE, nr, type)
+#define EC_IOWR(nr, type)  _IOWR(EC_IOCTL_TYPE, nr, type)
+#define EC_IOCTL_VERSION_MAGIC 31
+#define EC_IOCTL_MODULE  EC_IOR(0x00, ec_ioctl_module_t)
+#define EC_IOCTL_SLAVE_SDO_UPLOAD  EC_IOWR(0x0e, ec_ioctl_slave_sdo_upload_t)
+#define EC_IOCTL_SLAVE_SDO_DOWNLOAD  EC_IOWR(0x0f, ec_ioctl_slave_sdo_download_t)
+
+typedef struct
+{
+  uint32_t ioctl_version_magic;
+  uint32_t master_count;
+} ec_ioctl_module_t;
+
+typedef struct
+{
+  // inputs
+  uint16_t slave_position;
+  uint16_t sdo_index;
+  uint8_t sdo_entry_subindex;
+  size_t target_size;
+  uint8_t * target;
+
+  // outputs
+  size_t data_size;
+  uint32_t abort_code;
+} ec_ioctl_slave_sdo_upload_t;
+
+typedef struct
+{
+  // inputs
+  uint16_t slave_position;
+  uint16_t sdo_index;
+  uint8_t sdo_entry_subindex;
+  uint8_t complete_access;
+  size_t data_size;
+  uint8_t * data;
+
+  // outputs
+  uint32_t abort_code;
+} ec_ioctl_slave_sdo_download_t;
+
+#endif  // ETHERCAT_MANAGER__EC_MASTER_ASYNC_IO_HPP_

HiveCoreR0/src/ethercat_driver_ros2/ethercat_manager/package.xml

@@ -0,0 +1,21 @@
+<?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>ethercat_manager</name>
+  <version>1.2.0</version>
+  <description>Components to interact with EtherCAT slave modules</description>
+  <maintainer email="mcbed.robotics@gmail.com">Maciej Bednarczyk</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>pluginlib</depend>
+  <depend>ethercat_msgs</depend>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

HiveCoreR0/src/ethercat_driver_ros2/ethercat_manager/src/ethercat_sdo_srv_server.cpp

@@ -0,0 +1,192 @@
+// Copyright 2023 ICUBE Laboratory, University of Strasbourg
+//
+// 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.
+//
+// Author: Maciej Bednarczyk (mcbed.robotics@gmail.com)
+
+#include <iostream>
+#include <iomanip>
+#include <memory>
+
+#include "rclcpp/rclcpp.hpp"
+#include "ethercat_msgs/srv/get_sdo.hpp"
+#include "ethercat_msgs/srv/set_sdo.hpp"
+#include "ethercat_manager/ec_master_async.hpp"
+#include "ethercat_manager/data_convertion_tools.hpp"
+
+
+namespace ethercat_manager
+{
+void upload(
+  const std::shared_ptr<ethercat_msgs::srv::GetSdo::Request> request,
+  std::shared_ptr<ethercat_msgs::srv::GetSdo::Response> response)
+{
+  ec_ioctl_slave_sdo_upload_t data;
+  std::stringstream return_stream, data_stream;
+  const DataType * data_type = NULL;
+  double data_value = std::numeric_limits<double>::quiet_NaN();
+  response->sdo_return_value = data_value;
+
+  data.sdo_index = request->sdo_index;
+  data.sdo_entry_subindex = request->sdo_subindex;
+  data.slave_position = request->slave_position;
+
+  if (!(data_type = get_data_type(request->sdo_data_type))) {
+    return_stream << "Invalid data type '" << request->sdo_data_type << "'!";
+    response->sdo_return_message = return_stream.str();
+    response->success = false;
+    RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str());
+    return;
+  }
+
+  data.target_size = data_type->byteSize;
+  data.target = new uint8_t[data.target_size + 1];
+
+  EcMasterAsync master(request->master_id);
+  try {
+    master.open(EcMasterAsync::Read);
+  } catch (MasterException & e) {
+    return_stream << e.what();
+    response->success = false;
+    RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str());
+    response->sdo_return_message = return_stream.str();
+    return;
+  }
+
+  try {
+    master.sdo_upload(&data);
+  } catch (MasterException & e) {
+    return_stream << e.what();
+    response->success = false;
+    RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str());
+    response->sdo_return_message = return_stream.str();
+    return;
+  }
+
+  master.close();
+
+  try {
+    buffer2data(data_stream, data_value, data_type, data.target, data.data_size);
+  } catch (SizeException & e) {
+    delete[] data.target;
+    return_stream << e.what();
+    response->success = false;
+    RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str());
+    response->sdo_return_message = return_stream.str();
+    return;
+  }
+  return_stream << "SDO upload done successfully";
+  response->success = true;
+  response->sdo_return_value_string = data_stream.str();
+  response->sdo_return_value = data_value;
+  response->sdo_return_message = return_stream.str();
+
+  delete[] data.target;
+  RCLCPP_INFO(rclcpp::get_logger("ethercat_sdo_srv_server"), return_stream.str().c_str());
+}
+
+void download(
+  const std::shared_ptr<ethercat_msgs::srv::SetSdo::Request> request,
+  std::shared_ptr<ethercat_msgs::srv::SetSdo::Response> response)
+{
+  ec_ioctl_slave_sdo_download_t data;
+  std::stringstream return_stream, data_stream;
+  const DataType * data_type = NULL;
+  data.complete_access = 0;
+  data.sdo_index = request->sdo_index;
+  data.sdo_entry_subindex = request->sdo_subindex;
+
+  data.slave_position = request->slave_position;
+  if (!(data_type = get_data_type(request->sdo_data_type))) {
+    return_stream << "Invalid data type '" << request->sdo_data_type << "'!";
+    response->success = false;
+    response->sdo_return_message = return_stream.str();
+    RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str());
+    return;
+  }
+
+  data.data_size = request->sdo_value.size();
+  data.data = new uint8_t[data.data_size + 1];
+
+  try {
+    data.data_size = data2buffer(
+      data_type, request->sdo_value, data.data, data.data_size);
+  } catch (SizeException & e) {
+    delete[] data.data;
+    return_stream << e.what();
+    response->success = false;
+    response->sdo_return_message = return_stream.str();
+    RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str());
+    return;
+  } catch (std::ios::failure & e) {
+    delete[] data.data;
+    return_stream << "Invalid value for type '" << data_type->name << "'!";
+    response->success = false;
+    response->sdo_return_message = return_stream.str();
+    RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str());
+    return;
+  }
+
+  EcMasterAsync master(request->master_id);
+  try {
+    master.open(EcMasterAsync::ReadWrite);
+  } catch (MasterException & e) {
+    return_stream << e.what();
+    response->success = false;
+    RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str());
+    response->sdo_return_message = return_stream.str();
+    return;
+  }
+
+  try {
+    master.sdo_download(&data);
+  } catch (MasterException & e) {
+    return_stream << e.what();
+    response->success = false;
+    RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str());
+    response->sdo_return_message = return_stream.str();
+    return;
+  }
+
+  master.close();
+
+  return_stream << "SDO download done successfully";
+  response->success = true;
+  response->sdo_return_message = return_stream.str();
+
+  delete[] data.data;
+  RCLCPP_INFO(rclcpp::get_logger("ethercat_sdo_srv_server"), return_stream.str().c_str());
+}
+}  // namespace ethercat_manager
+
+
+int main(int argc, char ** argv)
+{
+  rclcpp::init(argc, argv);
+
+  std::shared_ptr<rclcpp::Node> node = rclcpp::Node::make_shared("ethercat_sdo_srv_server");
+
+  rclcpp::Service<ethercat_msgs::srv::GetSdo>::SharedPtr service_get_sdo =
+    node->create_service<ethercat_msgs::srv::GetSdo>(
+    "ethercat_manager/get_sdo",
+    &ethercat_manager::upload);
+
+  rclcpp::Service<ethercat_msgs::srv::SetSdo>::SharedPtr service_set_sdo =
+    node->create_service<ethercat_msgs::srv::SetSdo>(
+    "ethercat_manager/set_sdo",
+    &ethercat_manager::download);
+
+  rclcpp::spin(node);
+  rclcpp::shutdown();
+  return 0;
+}

HiveCoreR0/src/ethercat_driver_ros2/ethercat_msgs/CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.8)
+project(ethercat_msgs)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+find_package(ament_cmake REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+  "srv/SetSdo.srv"
+  "srv/GetSdo.srv"
+  DEPENDENCIES
+)
+
+if(BUILD_TESTING)
+  find_package(ament_lint_auto REQUIRED)
+  ament_lint_auto_find_test_dependencies()
+endif()
+
+ament_package()

HiveCoreR0/src/ethercat_driver_ros2/ethercat_msgs/package.xml

@@ -0,0 +1,18 @@
+<?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>ethercat_msgs</name>
+  <version>1.2.0</version>
+  <description>Interfaces for the EtherCAT Driver stack</description>
+  <maintainer email="mcbed.robotics@gmail.com">mcbed</maintainer>
+  <license>Apache-2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_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>

HiveCoreR0/src/ethercat_driver_ros2/ethercat_msgs/srv/GetSdo.srv

@@ -0,0 +1,10 @@
+int16 master_id
+uint16 slave_position
+uint16 sdo_index
+uint8 sdo_subindex
+string sdo_data_type
+---
+bool success
+string sdo_return_message
+string sdo_return_value_string
+float64 sdo_return_value

HiveCoreR0/src/ethercat_driver_ros2/ethercat_msgs/srv/SetSdo.srv

@@ -0,0 +1,9 @@
+int16 master_id
+int16 slave_position
+int16 sdo_index
+int16 sdo_subindex
+string sdo_data_type
+string sdo_value
+---
+bool success
+string sdo_return_message

HiveCoreR0/src/ethercat_driver_ros2_examples/.pre-commit-config.yaml

@@ -0,0 +1,145 @@
+# To use:
+#
+#     pre-commit run -a
+#
+# Or:
+#
+#     pre-commit install  # (runs every time you commit in git)
+#
+# To update this file:
+#
+#     pre-commit autoupdate
+#
+# See https://github.com/pre-commit/pre-commit
+
+repos:
+  # Standard hooks
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v4.4.0
+    hooks:
+      - id: check-added-large-files
+      - id: check-ast
+      - id: check-case-conflict
+      - id: check-docstring-first
+      - id: check-merge-conflict
+      - id: check-symlinks
+      - id: check-xml
+      - id: check-yaml
+      - id: debug-statements
+      - id: end-of-file-fixer
+      - id: mixed-line-ending
+      - id: trailing-whitespace
+        exclude_types: [rst]
+      - id: fix-byte-order-marker
+
+
+  # Python hooks
+  - repo: https://github.com/asottile/pyupgrade
+    rev: v3.3.1
+    hooks:
+    -   id: pyupgrade
+        args: [--py36-plus]
+
+  # PyDocStyle
+  - repo: https://github.com/PyCQA/pydocstyle
+    rev: 6.3.0
+    hooks:
+    - id: pydocstyle
+      args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"]
+
+  - repo: https://github.com/pycqa/flake8
+    rev: 6.0.0
+    hooks:
+    - id: flake8
+      args: ["--extend-ignore=E501"]
+
+  # Uncrustify
+  - repo: local
+    hooks:
+      - id: ament_uncrustify
+        name: ament_uncrustify
+        description: Uncrustify.
+        stages: [commit]
+        entry: ament_uncrustify
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+        args: ["--reformat"]
+
+  # CPP hooks
+  - repo: local
+    hooks:
+      - id: clang-format
+        name: clang-format
+        description: Format files with ClangFormat.
+        entry: clang-format-14
+        language: system
+        files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
+        args: ['-fallback-style=none', '-i']
+
+  # - repo: local
+  #   hooks:
+  #     - id: ament_cppcheck
+  #       name: ament_cppcheck
+  #       description: Static code analysis of C/C++ files.
+  #       stages: [commit]
+  #       entry: ament_cppcheck
+  #       language: system
+  #       files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+
+  # Maybe use https://github.com/cpplint/cpplint instead
+  - repo: local
+    hooks:
+      - id: ament_cpplint
+        name: ament_cpplint
+        description: Static code analysis of C/C++ files.
+        stages: [commit]
+        entry: ament_cpplint
+        language: system
+        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
+        args: ["--linelength=100", "--filter=-whitespace/newline"]
+
+  # Cmake hooks
+  - repo: local
+    hooks:
+      - id: ament_lint_cmake
+        name: ament_lint_cmake
+        description: Check format of CMakeLists.txt files.
+        stages: [commit]
+        entry: ament_lint_cmake
+        language: system
+        files: CMakeLists\.txt$
+
+  # Copyright
+  - repo: local
+    hooks:
+      - id: ament_copyright
+        name: ament_copyright
+        description: Check if copyright notice is available in all files.
+        stages: [commit]
+        entry: ament_copyright
+        language: system
+
+  # Docs - RestructuredText hooks
+  - repo: https://github.com/PyCQA/doc8
+    rev: v1.1.1
+    hooks:
+      - id: doc8
+        args: ['--max-line-length=100', '--ignore=D001']
+        exclude: CHANGELOG\.rst$
+
+  - repo: https://github.com/pre-commit/pygrep-hooks
+    rev: v1.10.0
+    hooks:
+      - id: rst-backticks
+        exclude: CHANGELOG\.rst$
+      - id: rst-directive-colons
+      - id: rst-inline-touching-normal
+
+  # Spellcheck in comments and docs
+  # skipping of *.svg files is not working...
+  - repo: https://github.com/codespell-project/codespell
+    rev: v2.2.2
+    hooks:
+      - id: codespell
+        args: ['--write-changes']
+        exclude: CHANGELOG\.rst|\.(svg|pyc)$

HiveCoreR0/src/ethercat_driver_ros2_examples/CONTRIBUTING.md

@@ -0,0 +1,51 @@
+# Contributing Guidelines
+Thank you for your interest in contributing to `ethercat_driver_ros2_examples`. Whether it's a bug report, new feature, correction, or additional documentation, we greatly value feedback and contributions from the community.
+
+Please read through this document before submitting any issues or pull requests to ensure we have all the necessary information to effectively respond to your bug report or contribution.
+
+
+## Reporting Bugs/Feature Requests
+We welcome you to use the GitHub issue tracker to report bugs or suggest features.
+
+When filing an issue, please check [existing open][issues], or [recently closed][closed-issues], issues to make sure somebody else hasn't already reported the issue.
+
+Please try to include as much information as you can. Details like these are incredibly useful:
+
+* A reproducible test case or series of steps
+* The version of our code being used
+* Any modifications you've made relevant to the bug
+* Anything unusual about your environment or deployment
+
+
+## Contributing via Pull Requests
+Contributions via pull requests are much appreciated. Before sending us a pull request, please ensure that:
+
+1. You are working against the latest source on the *main* branch.
+2. You check existing open, and recently merged, pull requests to make sure someone else hasn't addressed the problem already.
+3. You open an issue to discuss any significant work - we would hate for your time to be wasted.
+
+To send us a pull request, please:
+
+1. Fork the repository.
+2. Modify the source; please focus on the specific change you are contributing.
+  If you also reformat all the code, it will be hard for us to focus on your change.
+3. Ensure local tests pass.
+4. Commit to your fork using clear commit messages.
+5. Send a pull request, answering any default questions in the pull request interface.
+6. Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation.
+
+GitHub provides additional documentation on [forking a repository](https://help.github.com/articles/fork-a-repo/) and [creating a pull request](https://help.github.com/articles/creating-a-pull-request/).
+
+
+## Licensing
+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.
+~~~

HiveCoreR0/src/ethercat_driver_ros2_examples/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.

HiveCoreR0/src/ethercat_driver_ros2_examples/README.md

@@ -0,0 +1,23 @@
+# ethercat_driver_ros2_examples
+[![Licence](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://opensource.org/licenses/Apache-2.0)
+[![Build](https://github.com/ICube-Robotics/ethercat_driver_ros2_examples/actions/workflows/ci.yml/badge.svg)](https://github.com/ICube-Robotics/ethercat_driver_ros2_examples/actions/workflows/ci.yml)
+
+Example repository for the [EtherCAT Driver ROS2 stack](https://github.com/ICube-Robotics/ethercat_driver_ros2).
+
+This repository contains the following example packages :
+- EtherCAT compatible CIA402 motor drive (example using Maxon EPOS3)
+- Custom Force Sensor EtherCAT slave using strain gauges and EasyCAT
+- Example configuration files for the some EtherCAT modules
+
+## Contacts ##
+![icube](https://icube.unistra.fr/fileadmin/templates/DUN/icube/images/logo.png)
+
+[ICube Laboratory](https://icube.unistra.fr), [University of Strasbourg](https://www.unistra.fr/), France
+
+__Manuel Yguel:__ [yguel@unistra.fr](mailto:yguel@unistra.fr), @github: [yguel](https://github.com/yguel)
+
+__Laurent Barbé:__ [laurent.barbe@unistra.fr](mailto:laurent.barbe@unistra.fr)
+
+__Philippe Zanne:__ [philippe.zanne@unistra.fr](mailto:philippe.zanne@unistra.fr)
+
+__Maciej Bednarczyk:__ [m.bednarczyk@unistra.fr](mailto:m.bednarczyk@unistra.fr), @github: [mcbed](https://github.com/mcbed)

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_driver_ros2_examples.repos

@@ -0,0 +1,5 @@
+repositories:
+  ethercat_driver_ros2:
+    type: git
+    url: https://github.com/ICube-Robotics/ethercat_driver_ros2.git
+    version: main

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_driver_ros2_examples/CMakeLists.txt

@@ -0,0 +1,4 @@
+cmake_minimum_required(VERSION 3.8)
+project(ethercat_driver_ros2_examples NONE)
+find_package(ament_cmake REQUIRED)
+ament_package()

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_driver_ros2_examples/package.xml

@@ -0,0 +1,16 @@
+<?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>ethercat_driver_ros2_examples</name>
+  <version>0.0.1</version>
+  <description>Meta-package aggregating the ethercat_driver_ros2_examples packages and documentation</description>
+  <maintainer email="mcbed.robotics@gmail.com">Maciej Bednarczyk</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <depend>ethercat_force_sensor</depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_force_sensor/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.8)
+project(ethercat_force_sensor)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(ethercat_driver REQUIRED)
+find_package(ethercat_interface REQUIRED)
+find_package(ethercat_generic_slave REQUIRED)
+
+install(
+  DIRECTORY config description launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_force_sensor/README.md

@@ -0,0 +1,56 @@
+# ethercat_force_sensor
+
+This repository contains the instructions for building a custom Force Sensor EtherCAT slave using strain gauges and EasyCAT and use it in ROS2 with [EtherCAT Driver ROS2 stack](https://github.com/ICube-Robotics/ethercat_driver_ros2).
+
+## Required Hardware ##
+| | |
+|---|---
+| [Arduino Uno](https://store.arduino.cc/products/arduino-uno-rev3) | <img src="https://cdn.shopify.com/s/files/1/0438/4735/2471/products/A000066_01.iso_643x483.jpg?v=1629815860"  width="30%">
+| [Load Cell / Wheatstone Amplifier Shield (2ch)](https://eu.robotshop.com/products/strain-gauge-load-cell-amplifier-shield-2ch) |<img src="https://cdn.shopify.com/s/files/1/0573/1486/9416/products/strain-gauge-load-cell-amplifier-shield-2ch_53cd0820-70a7-4f4f-95c8-a95b3a1deff1_600x.jpg?v=1681189038"  width="30%"> |
+| [EasyCAT Shield for Arduino](https://www.bausano.net/en/hardware/easycat.html)  | <img src="https://www.bausano.net/images/EasyCAT.jpg" width="30%">
+| [5 Kg Micro Load Cell](https://eu.robotshop.com/products/micro-load-cell-5-kg)  | <img src="https://cdn.shopify.com/s/files/1/0573/1486/9416/products/micro-load-cell-5-kg_1_0e957087-2532-406e-90a7-35aeebde0a6a_600x.jpg?v=1680837537" width="30%">
+
+
+
+## EasyCAT configuration ##
+Use the [EasyConfigurator](https://www.bausano.net/images/arduino-easycat/EasyConfigurator_UserManual.pdf) tool to load the [ForceSensor](config/easycat_config/ForceSensor.prj) file and write the configuration to the slave EEPROM.
+
+Open the [ForceSensor](config/easycat_config/ForceSensor.ino) Arduino project in the [Arduino IDE](https://www.arduino.cc/en/software) and upload it to the Arduino board.
+
+## Usage ##
+***Required setup : Ubuntu 22.04 LTS***
+
+1.  Install `ros2` packages. The current development is based of `ros2 humble`. Installation steps are described [here](https://docs.ros.org/en/humble/Installation.html).
+2. Source your `ros2` environment:
+    ```shell
+    source /opt/ros/humble/setup.bash
+    ```
+    **NOTE**: The ros2 environment needs to be sources in every used terminal. If only one distribution of ros2 is used, it can be added to the `~/.bashrc` file.
+3. Install `colcon` and its extensions :
+    ```shell
+    sudo apt install python3-colcon-common-extensions
+     ```
+3. Create a new ros2 workspace:
+    ```shell
+    mkdir ~/ros2_ws/src
+    ```
+4. Pull relevant packages, install dependencies, compile, and source the workspace by using:
+    ```shell
+    cd ~/ros2_ws
+    git clone https://github.com/ICube-Robotics/ethercat_driver_ros2_examples.git src/ethercat_driver_ros2_examples
+    vcs import src < ethercat_driver_ros2_examples.repos
+    rosdep install --ignore-src --from-paths . -y -r
+    colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install --packages-select ethercat_force_sensor
+    source install/setup.bash
+    ```
+5. Run the system:
+    ```shell
+    ros2 launch ethercat_force_sensor force_sensor.launch.py
+    ```
+
+## Contacts ##
+![icube](https://icube.unistra.fr/fileadmin/templates/DUN/icube/images/logo.png)
+
+[ICube Laboratory](https://icube.unistra.fr), [University of Strasbourg](https://www.unistra.fr/), France
+
+__Maciej Bednarczyk:__ [m.bednarczyk@unistra.fr](mailto:m.bednarczyk@unistra.fr), @github: [mcbed](https://github.com/mcbed)

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_force_sensor/config/controllers.yaml

@@ -0,0 +1,6 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 100  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_force_sensor/config/easycat_config/ForceSensor.h

@@ -0,0 +1,50 @@
+// Copyright 2023, ICube Laboratory, University of Strasbourg
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef ETHERCAT_FORCE_SENSOR__CONFIG__EASYCAT_CONFIG__FORCESENSOR_H_
+#define ETHERCAT_FORCE_SENSOR__CONFIG__EASYCAT_CONFIG__FORCESENSOR_H_
+
+//-------------------------------------------------------------------//
+//                                                                   //
+//     This file has been created by the Easy Configurator tool      //
+//                                                                   //
+//     Easy Configurator project ForceSensor.prj
+//                                                                   //
+//-------------------------------------------------------------------//
+
+
+#define CUST_BYTE_NUM_OUT       0
+#define CUST_BYTE_NUM_IN        4
+#define TOT_BYTE_NUM_ROUND_OUT  0
+#define TOT_BYTE_NUM_ROUND_IN   4
+
+//---- output buffer ----
+typedef union {
+  uint8_t Byte[TOT_BYTE_NUM_ROUND_OUT];
+  struct
+  {
+  } Cust;
+} PROCBUFFER_OUT;
+
+//---- input buffer ----
+typedef union {
+  uint8_t Byte[TOT_BYTE_NUM_ROUND_IN];
+  struct
+  {
+    int16_t force_0;
+    int16_t force_1;
+  } Cust;
+} PROCBUFFER_IN;
+
+#endif  // ETHERCAT_FORCE_SENSOR__CONFIG__EASYCAT_CONFIG__FORCESENSOR_H_

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_force_sensor/config/easycat_config/ForceSensor.ino

@@ -0,0 +1,94 @@
+#define CUSTOM
+#include "ForceSensor.h"
+#include "EasyCAT.h"                // EasyCAT library to interface the LAN9252
+#include <SPI.h>                    // SPI library
+
+EasyCAT EASYCAT(8);                 // EasyCAT istantiation
+
+const int Ana0 = A0;                // analog input 0
+const int Ana1 = A1;                // analog input 1
+
+unsigned long Millis = 0;
+unsigned long PreviousMillis = 0;
+
+
+//---- setup ---------------------------------------------------------------------------------------
+
+void setup()
+{
+  Serial.begin(9600);                                             // serial line initialization
+                                                                  //(used only for debug)
+
+  Serial.print ("\nEasyCAT - Generic EtherCAT slave\n");          // print the banner
+
+                                                                  //---- initialize the EasyCAT board -----
+
+  if (EASYCAT.Init() == true)                                     // initialization
+  {                                                               // successfully completed
+    Serial.print ("initialized");                                 //
+  }                                                               //
+
+  else                                                            // initialization failed
+  {                                                               // the EasyCAT board was not recognized
+    Serial.print ("initialization failed");                       //
+                                                                  // The most common reason is that the SPI
+                                                                  // chip select chosen on the board doesn't
+                                                                  // match the one chosen by the firmware
+
+    pinMode(13, OUTPUT);                                          // stay in loop for ever
+                                                                  // with the Arduino led blinking
+    while(1)                                                      //
+    {                                                             //
+      digitalWrite (13, LOW);                                     //
+      delay(500);                                                 //
+      digitalWrite (13, HIGH);                                    //
+      delay(500);                                                 //
+    }                                                             //
+  }
+
+  PreviousMillis = millis();
+}
+
+
+//---- main loop ----------------------------------------------------------------------------------------
+
+void loop()                                             // In the main loop we must call ciclically the
+{                                                       // EasyCAT task and our application
+                                                        //
+                                                        // This allows the bidirectional exachange of the data
+                                                        // between the EtherCAT master and our application
+                                                        //
+                                                        // The EasyCAT cycle and the Master cycle are asynchronous
+                                                        //
+
+  EASYCAT.MainTask();                                   // execute the EasyCAT task
+
+  Application();                                        // user applications
+
+}
+
+//---- user application ------------------------------------------------------------------------------
+
+void Application ()
+
+{
+  uint16_t Analog0;
+  uint16_t Analog1;
+
+  Millis = millis();                                    // As an example for this application
+  if (Millis - PreviousMillis >= 10)                    // we choose a cycle time of 10 mS
+  {                                                     //
+    PreviousMillis = Millis;                            //
+
+                                                        // --- analog inputs management ---
+
+    Analog0 = analogRead(Ana0);                         // read analog input 0
+    Analog0 = Analog0 << 6;                             // normalize it on 16 bits
+    EASYCAT.BufferIn.Cust.force_0 = Analog0;           // and put the result into
+                                                        // input Byte 0
+
+    Analog1 = analogRead(Ana1);                         // read analog input 1
+    Analog1 = Analog1 << 6;                             // normalize it on 16 bits
+    EASYCAT.BufferIn.Cust.force_1 = Analog1;           // and put the result into
+  }
+}

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_force_sensor/config/easycat_config/ForceSensor.prj

@@ -0,0 +1,14 @@
+ForceSensor
+OUTPUTS_TAG
+INPUTS_TAG
+force_0
+int16_t
+force_1
+int16_t
+INFO_TAG
+0x0000079A
+AB&T
+0xDEADBEEF
+0x00000001
+ForceSensor
+END_TAG

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_force_sensor/config/easycat_config/ForceSensor.xml

@@ -0,0 +1,131 @@
+<?xml version="1.0"?>
+<EtherCATInfo xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance" xsi:noNamespaceSchemaLocation="EtherCATInfo.xsd" Version="1.6">
+
+
+<!--*************************************************************************************** -->
+<!--                                                                                        -->
+<!-- AB&T Tecnologie Informatiche - Ivrea Italy                                             -->
+<!-- http://www.bausano.net                                                                 -->
+<!-- https://www.ethercat.org/en/products/791FFAA126AD43859920EA64384AD4FD.htm              -->
+<!--                                                                                        -->
+<!--*************************************************************************************** -->
+
+<!--*************************************************************************************** -->
+<!--                                                                                        -->
+<!-- This software is distributed as an example, "AS IS", in the hope that it could         -->
+<!-- be useful, WITHOUT ANY WARRANTY of any kind, express or implied, included, but         -->
+<!-- not limited,  to the warranties of merchantability, fitness for a particular           -->
+<!-- purpose, and non infringiment. In no event shall the authors be liable for any         -->
+<!-- claim damages or other liability, arising from, or in connection with this software.   -->
+<!--                                                                                        -->
+<!--*************************************************************************************** -->
+
+
+<!-- ************************************************************************************** -->
+<!--                                                                                        -->
+<!--     This file has been created by the EasyCAT configuration tool                       -->
+<!--                                                                                        -->
+<!--     Easy Configurator project ForceSensor.prj                                              -->
+<!--                                                                                        -->
+<!-- ************************************************************************************** -->
+
+
+
+	<Vendor>
+		<Id>#x0000079A</Id>
+	  	<Name>AB&amp;T</Name>
+	    <ImageData16x14>424dd6020000000000003600000028000000100000000e0000000100180000000000a0020000c40e0000c40e000000000000000000004cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb1224cb122ffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff241cedffffff241cedffffff241ced241cedffffffffffffffffff241ced241ced241cedffffff241cedffffffffffff241cedffffff241cedffffff241cedffffff241cedffffff241cedffffff241cedffffffffffff241cedffffffffffff241cedffffff241cedffffff241cedffffff241cedffffff241cedffffff241cedffffffffffff241cedffffffffffff241cedffffff241cedffffff241cedffffff241cedffffffffffff241cedffffffffffffffffff241cedffffffffffff241ced241ced241cedffffff241ced241cedffffffffffff241cedffffff241cedffffffffffff241cedffffffffffff241cedffffff241cedffffff241cedffffff241cedffffff241cedffffff241cedffffffffffff241cedffffffffffff241cedffffff241cedffffff241cedffffff241cedffffffffffff241cedffffffffffffffffff241cedffffffffffff241cedffffff241cedffffff241cedffffff241cedffffffffffffffffffffffffffffffffffff241cedffffffffffffffffff241cedffffffffffffffffff241cedffffffffffffffffffffffffffffffffffff241ced241ced241cedffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff</ImageData16x14>
+	</Vendor>
+  <Descriptions>
+    <Groups>
+      <Group SortOrder="0">
+        <Type>SSC_Device</Type>
+        <Name LcId="1033">EasyCAT</Name>
+        <ImageData16x14>424dd6020000000000003600000028000000100000000e0000000100180000000000a0020000c40e0000c40e00000000000000000000241ced241ced241ced241cedffffff241cedffffffffffffffffff241cedffffffffffffffffff241cedffffffffffff241cedffffffffffffffffffffffff241cedffffffffffffffffff241cedffffffffffffffffff241cedffffffffffff241cedffffffffffffffffffffffff241ced241ced241ced241ced241cedffffffffffffffffff241cedffffffffffff241cedffffffffffffffffffffffff241cedffffffffffffffffff241cedffffffffffffffffff241cedffffffffffff241cedffffffffffffffffffffffff241ced241cedffffff241ced241cedffffff241cedffffff241cedffffff241ced241ced241ced241ced241cedffffffffffff241ced241ced241cedffffffffffff241ced241ced241ced241ced241cedffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff241ced241ced241cedffffffffffff241ced241ced241cedffffff241ced241ced241cedffffff241ced241ced241ced241cedffffffffffffffffff241cedffffffffffff241cedffffffffffffffffff241cedffffffffffffffffff241ced241cedffffffffffffffffffffffff241ced241cedffffffffffff241ced241ced241cedffffff241ced241ced241ced241ced241cedffffffffffffffffffffffffffffff241cedffffff241cedffffffffffffffffff241cedffffff241ced241cedffffffffffffffffffffffff241ced241ced241cedffffff241ced241ced241cedffffff241cedffffff241ced241cedffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff241ced241ced241cedffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffffff</ImageData16x14>
+      </Group>
+    </Groups>
+    <Devices>
+
+<!-- ************************************************************************************** -->
+<!--                                                                                        -->
+<!--     Custom device                                                                      -->
+<!--                                                                                        -->
+<!-- ************************************************************************************** -->
+
+      <Device Physics="YY">
+        <Type ProductCode="#xDEADBEEF" RevisionNo="#x00000001" CheckRevisionNo="EQ_OR_G">ForceSensor</Type>
+        <Name LcId="1033"><![CDATA[ForceSensor]]></Name>
+        <GroupType>SSC_Device</GroupType>
+        <Fmmu>Inputs</Fmmu>
+
+	    <!-- input base physical address -->
+        <Sm StartAddress="#x1200" ControlByte="#x20" Enable="1">Inputs</Sm>
+
+<!-- ***************** input PDO ********************************************************** -->
+
+        <TxPdo Fixed="1" Mandatory="1" Sm="0">
+          <Index>#x1A00</Index>
+          <Name>Inputs</Name>
+          <Entry>
+            <Index>#x6</Index>
+            <SubIndex>1</SubIndex>
+            <BitLen>16</BitLen>
+            <Name>force_0</Name>
+            <DataType>INT</DataType>
+          </Entry>
+          <Entry>
+            <Index>#x6</Index>
+            <SubIndex>2</SubIndex>
+            <BitLen>16</BitLen>
+            <Name>force_1</Name>
+            <DataType>INT</DataType>
+          </Entry>
+        </TxPdo>
+
+
+<!-- ***************** Synchronization ***************************************************** -->
+
+		<Dc>                                                        <!-- DC not used(default)-->
+			<OpMode>
+				<Name>SM_Sync or Async</Name>
+				<Desc>SM_Sync or Async</Desc>
+				<AssignActivate>#x0000</AssignActivate>
+			</OpMode>
+
+			<OpMode>                                                <!-- DC used             -->
+				<Name>DC_Sync</Name>
+				<Desc>DC_Sync</Desc>
+				<AssignActivate>#x300</AssignActivate>
+				<CycleTimeSync0 Factor="1">0</CycleTimeSync0>
+				<ShiftTimeSync0>2000200000</ShiftTimeSync0>         <!-- 10nS unit           -->
+			</OpMode>
+		</Dc>
+
+<!-- *************************************************************************************** -->
+
+        <Eeprom>
+          <ByteSize>4096</ByteSize>
+          <ConfigData>8003006EFF00FF000000</ConfigData>
+                                                 <!-- 0x140   0x80 PDI type LAN9252 Spi      -->
+
+                                                 <!-- 0x141   0x03 device emulation          -->
+                                                 <!--         enhanced link detection        -->
+
+                                                 <!-- 0x150   0x00 not used for LAN9252 Spi  -->
+
+                                                 <!-- 0x151   0x6E map Sync0 to AL event     -->
+                                                 <!--         Sync0/Latch0 assigned to Sync0 -->
+                                                 <!--         Sync1/Latch1 assigned to Sync1 -->
+                                                 <!--         Sync0/1 push/pull active high  -->
+
+                                                 <!-- 0x982-3 0x00FF Sync0/1 length = 2.5uS  -->
+
+                                                 <!-- 0x152   0xFF all GPIO set to out       -->
+                                                 <!-- 0x153   0x00 reserved                  -->
+
+                                                 <!-- 0x12-13 0x0000 alias address           -->
+        </Eeprom>
+      </Device>
+    </Devices>
+  </Descriptions>
+</EtherCATInfo>

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_force_sensor/config/easycat_config/log_configuration.txt

@@ -0,0 +1,81 @@
+
+Easy Configurator, configuration engine  V_3_3
+
+log created on Thu Apr 13 11:45:58 2023
+
+Analyzing the project file
+
+Easy Configurator project ForceSensor.prj
+
+OUTPUTS_TAG
+INPUTS_TAG
+Line 5 --- force_0                 int16_t
+Line 6 --- force_1                 int16_t
+INFO_TAG
+0x0000079A
+AB&T
+0xDEADBEEF
+0x00000001
+ForceSensor
+ForceSensor
+END_TAG
+
+The ForceSensor.prj configuration file is correct
+
+ByteNumOut 0
+ByteNumIn  4
+PdoNumOut  0
+PdoNumIn   2
+
+Creating the ForceSensor.xml file
+
+PROJECT_TAG   ForceSensor.prj
+VENDOID_TAG   0000079A
+VEN_NAM_TAG   AB&T
+PRO_COD_TAG   DEADBEEF
+REVISIO_TAG   00000001
+DEV_NAM_TAG   ForceSensor
+DEV_NAM_TAG   ForceSensor
+
+FIRST_TAG
+
+Input entries
+          <Entry>
+            <Index>#x6</Index>
+            <SubIndex>1</SubIndex>
+            <BitLen>16</BitLen>
+            <Name>force_0</Name>
+            <DataType>INT</DataType>
+          </Entry>
+          <Entry>
+            <Index>#x6</Index>
+            <SubIndex>2</SubIndex>
+            <BitLen>16</BitLen>
+            <Name>force_1</Name>
+            <DataType>INT</DataType>
+          </Entry>
+
+LAST_TAG
+
+The ForceSensor.xml file has been created
+
+Creating the ForceSensor.h file
+
+#define CUST_BYTE_NUM_OUT	0
+#define CUST_BYTE_NUM_IN	4
+#define TOT_BYTE_NUM_ROUND_OUT	0
+#define TOT_BYTE_NUM_ROUND_IN	4
+
+
+Output entries
+
+Input entries
+		int16_t     force_0;
+		int16_t     force_1;
+
+The ForceSensor.h file has been created
+
+Creating the ForceSensor.bin file
+
+
+The ForceSensor.bin file has been created

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_force_sensor/config/slave_config.yaml

@@ -0,0 +1,10 @@
+# Configuration file for EasyCAT based ForceSensor Slave
+vendor_id: 0x0000079a
+product_id: 0xdeadbeef
+tpdo:  # TxPDO
+  - index: 0x1a00
+    channels:
+      - {index: 0x0006, sub_index: 1, type: int16, state_interface: force.0}
+      - {index: 0x0006, sub_index: 2, type: int16, state_interface: force.1}
+sm:  # Sync Manager
+  - {index: 0, type: input, pdo: tpdo, watchdog: disable}

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_force_sensor/description/config/force_sensor.config.xacro

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="force_sensor">
+
+  <link name="world"/>
+
+  <xacro:include filename="$(find ethercat_force_sensor)/description/ros2_control/force_sensor.ros2_control.xacro" />
+
+  <xacro:force_sensor/>
+</robot>

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_force_sensor/description/ros2_control/force_sensor.ros2_control.xacro

@@ -0,0 +1,27 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="force_sensor">
+
+    <ros2_control name="force_sensor" type="system">
+      <hardware>
+        <plugin>ethercat_driver/EthercatDriver</plugin>
+        <param name="master_id">0</param>
+        <param name="control_frequency">100</param>
+      </hardware>
+
+      <sensor name="force_sensor">
+        <state_interface name="force.0"/>
+        <state_interface name="force.1"/>
+        <ec_module name="force_sensor">
+          <plugin>ethercat_generic_plugins/GenericEcSlave</plugin>
+          <param name="alias">0</param>
+          <param name="position">0</param>
+          <param name="slave_config">$(find ethercat_force_sensor)/config/slave_config.yaml</param>
+        </ec_module>
+      </sensor>
+    </ros2_control>
+
+  </xacro:macro>
+
+</robot>

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_force_sensor/launch/force_sensor.launch.py

@@ -0,0 +1,81 @@
+# Copyright 2023 ICube Laboratory, University of Strasbourg
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'description_file',
+            default_value='force_sensor.config.xacro',
+            description='URDF/XACRO description file with the sensor.',
+        )
+    )
+
+    description_file = LaunchConfiguration('description_file')
+
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("ethercat_force_sensor"),
+                    "description/config",
+                    description_file,
+                ]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("ethercat_force_sensor"),
+            "config",
+            "controllers.yaml",
+        ]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_description, robot_controllers],
+        output="both",
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
+    )
+
+    nodes = [
+        control_node,
+        joint_state_broadcaster_spawner,
+    ]
+
+    return LaunchDescription(
+        declared_arguments +
+        nodes)

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_force_sensor/package.xml

@@ -0,0 +1,18 @@
+<?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>ethercat_force_sensor</name>
+  <version>0.0.1</version>
+  <description> Package with instructions for building a custom Force Sensor EtherCAT slave using strain gauges and EasyCAT and use it in ROS2</description>
+  <maintainer email="mcbed.robotics@gmail.com">Maciej Bednarczyk</maintainer>
+  <license>Apache License 2.0</license>
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <exec_depend>ethercat_driver</exec_depend>
+  <exec_depend>ethercat_interface</exec_depend>
+  <exec_depend>ethercat_generic_slave</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/CMakeLists.txt

@@ -0,0 +1,14 @@
+cmake_minimum_required(VERSION 3.8)
+project(ethercat_motor_drive)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(ethercat_driver REQUIRED)
+find_package(ethercat_interface REQUIRED)
+find_package(ethercat_generic_cia402_drive REQUIRED)
+
+install(
+  DIRECTORY config description launch
+  DESTINATION share/${PROJECT_NAME}
+)
+
+ament_package()

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/README.md

@@ -0,0 +1,45 @@
+# ethercat_motor_drive
+
+This repository contains the instructions for running an EtherCAT compatible motor drive and use it in ROS2 with [EtherCAT Driver ROS2 stack](https://github.com/ICube-Robotics/ethercat_driver_ros2).
+
+## Required Hardware ##
+- Maxon EPOS3 motor drive
+- Compatible DC motor with encoder
+
+## Usage ##
+***Required setup : Ubuntu 22.04 LTS***
+
+1.  Install `ros2` packages. The current development is based of `ros2 humble`. Installation steps are described [here](https://docs.ros.org/en/humble/Installation.html).
+2. Source your `ros2` environment:
+    ```shell
+    source /opt/ros/humble/setup.bash
+    ```
+    **NOTE**: The ros2 environment needs to be sources in every used terminal. If only one distribution of ros2 is used, it can be added to the `~/.bashrc` file.
+3. Install `colcon` and its extensions :
+    ```shell
+    sudo apt install python3-colcon-common-extensions
+     ```
+3. Create a new ros2 workspace:
+    ```shell
+    mkdir ~/ros2_ws/src
+    ```
+4. Pull relevant packages, install dependencies, compile, and source the workspace by using:
+    ```shell
+    cd ~/ros2_ws
+    git clone https://github.com/ICube-Robotics/ethercat_driver_ros2_examples.git src/ethercat_driver_ros2_examples
+    vcs import src < ethercat_driver_ros2_examples.repos
+    rosdep install --ignore-src --from-paths . -y -r
+    colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --symlink-install --packages-select ethercat_motor_drive
+    source install/setup.bash
+    ```
+5. Run the system:
+    ```shell
+    ros2 launch ethercat_motor_drive motor_drive.launch.py
+    ```
+
+## Contacts ##
+![icube](https://icube.unistra.fr/fileadmin/templates/DUN/icube/images/logo.png)
+
+[ICube Laboratory](https://icube.unistra.fr), [University of Strasbourg](https://www.unistra.fr/), France
+
+__Maciej Bednarczyk:__ [m.bednarczyk@unistra.fr](mailto:m.bednarczyk@unistra.fr), @github: [mcbed](https://github.com/mcbed)

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/config/controllers.yaml

@@ -0,0 +1,35 @@
+controller_manager:
+  ros__parameters:
+    update_rate: 1000  # Hz
+
+    joint_state_broadcaster:
+      type: joint_state_broadcaster/JointStateBroadcaster
+
+    trajectory_controller:
+      type: joint_trajectory_controller/JointTrajectoryController
+
+    velocity_controller:
+      type: velocity_controllers/JointGroupVelocityController
+
+    effort_controller:
+      type: effort_controllers/JointGroupEffortController
+
+
+trajectory_controller:
+  ros__parameters:
+    command_interfaces:
+      - position
+    state_interfaces:
+      - position
+    joints:
+      - joint_1
+
+velocity_controller:
+  ros__parameters:
+    joints:
+      - joint_1
+
+effort_controller:
+  ros__parameters:
+    joints:
+      - joint_1

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/config/ino_sv660n_config.yaml

@@ -0,0 +1,23 @@
+# Configuration file for Maxon EPOS3 drive
+vendor_id: 0x5a65726f
+product_id: 0x00029252
+assign_activate: 0x0300  # DC Synch register
+auto_fault_reset: false  # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"
+
+rpdo:  # RxPDO = receive PDO Mapping
+  - index: 0x1600
+    channels:
+      - {index: 0x6040, sub_index: 0, type: uint16, default: 0}  # Control word
+      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan}  # Target position
+      - {index: 0x60FE, sub_index: 0, type: int32, default: 8}  # Mode of operation
+     
+      
+tpdo:  # TxPDO = transmit PDO Mapping
+  - index: 0x1a00
+    channels:
+      - {index: 0x6041, sub_index: 0, type: uint16}  # Status word
+      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position}  # Position actual value
+      # - {index: 0x60FD, sub_index: 0, type: int32, default: .nan}  # Velocity actual value
+      - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity}  # Velocity actual value
+      - {index: 0x6077, sub_index: 0, type: int16, state_interface: effort}  # Torque actual value
+      # - {index: 0x603F, sub_index: 0, type: int16}  # Mode of operation display

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/config/maxon_epos3_config.yaml

@@ -0,0 +1,39 @@
+# Configuration file for Maxon EPOS3 drive
+vendor_id: 0x000000fb
+product_id: 0x64400000
+assign_activate: 0x0300 # DC Synch register
+auto_fault_reset: false # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"
+sdo: # sdo data to be transferred at drive startup
+  - { index: 0x60C2, sub_index: 1, type: int8, value: 10 } # Set interpolation time for cyclic modes to 10 ms
+  - { index: 0x60C2, sub_index: 2, type: int8, value: -3 } # Set base 10-3s
+rpdo: # RxPDO = receive PDO 4 Mapping
+  - index: 0x1603
+    channels:
+      - { index: 0x6040, sub_index: 0, type: uint16, default: 0 } # Control word
+      - {
+          index: 0x607a,
+          sub_index: 0,
+          type: int32,
+          command_interface: position,
+          default: .nan,
+        } # Target position
+      - { index: 0x60ff, sub_index: 0, type: int32, default: 0 } # Target velocity
+      - { index: 0x6071, sub_index: 0, type: int16, default: 0 } # Target torque
+      - { index: 0x60b0, sub_index: 0, type: int32, default: 0 } # Offset position
+      - { index: 0x60b1, sub_index: 0, type: int32, default: 0 } # Offset velocity
+      - { index: 0x60b2, sub_index: 0, type: int16, default: 0 } # Offset torque
+      - { index: 0x6060, sub_index: 0, type: int8, default: 8 } # Mode of operation
+      - { index: 0x2078, sub_index: 1, type: uint16, default: 0 } # Digital Output Functionalities
+      - { index: 0x60b8, sub_index: 0, type: uint16, default: 0 } # Touch Probe Function
+tpdo: # TxPDO = transmit PDO 4 Mapping
+  - index: 0x1a03
+    channels:
+      - { index: 0x6041, sub_index: 0, type: uint16 } # Status word
+      - { index: 0x6064, sub_index: 0, type: int32, state_interface: position } # Position actual value
+      - { index: 0x606c, sub_index: 0, type: int32, state_interface: velocity } # Velocity actual value
+      - { index: 0x6077, sub_index: 0, type: int16, state_interface: effort } # Torque actual value
+      - { index: 0x6061, sub_index: 0, type: int8 } # Mode of operation display
+      - { index: 0x2071, sub_index: 1, type: int16 } # Digital Input Functionalities State
+      - { index: 0x60b9, sub_index: 0, type: int16 } # Touch Probe Status
+      - { index: 0x60ba, sub_index: 0, type: int32 } # Touch Probe Position 1 Positive Value
+      - { index: 0x60bb, sub_index: 0, type: int32 } # Touch Probe Position 1 Negative Value

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/config/maxon_epos4_50_5_config.yaml

@@ -0,0 +1,39 @@
+# Configuration file for Maxon EPOS4 50/5 drive
+vendor_id: 0x000000fb
+product_id: 0x63500000 # Product code
+assign_activate: 0x0300 # DC Synch register
+auto_fault_reset: false # true = automatic fault reset, false = fault reset on rising edge command interface "reset_fault"
+sdo: # sdo data to be transferred at drive startup
+  - { index: 0x60C2, sub_index: 1, type: int8, value: 10 } # Set interpolation time for cyclic modes to 10 ms
+  - { index: 0x60C2, sub_index: 2, type: int8, value: -3 } # Set base 10-3s
+rpdo: # RxPDO = receive PDO 4 Mapping
+  - index: 0x1603
+    channels:
+      - { index: 0x6040, sub_index: 0, type: uint16, default: 0 } # Control word
+      - {
+          index: 0x607a,
+          sub_index: 0,
+          type: int32,
+          command_interface: position,
+          default: .nan,
+        } # Target position
+      - { index: 0x60ff, sub_index: 0, type: int32, default: 0 } # Target velocity
+      - { index: 0x6071, sub_index: 0, type: int16, default: 0 } # Target torque
+      - { index: 0x60b0, sub_index: 0, type: int32, default: 0 } # Offset position
+      - { index: 0x60b1, sub_index: 0, type: int32, default: 0 } # Offset velocity
+      - { index: 0x60b2, sub_index: 0, type: int16, default: 0 } # Offset torque
+      - { index: 0x6060, sub_index: 0, type: int8, default: 8 } # Mode of operation
+      - { index: 0x3150, sub_index: 1, type: uint16, default: 0 } # Digital Output Properties
+      - { index: 0x60b8, sub_index: 0, type: uint16, default: 0 } # Touch Probe Function
+tpdo: # TxPDO = transmit PDO 4 Mapping
+  - index: 0x1a03
+    channels:
+      - { index: 0x6041, sub_index: 0, type: uint16 } # Status word
+      - { index: 0x6064, sub_index: 0, type: int32, state_interface: position } # Position actual value
+      - { index: 0x606c, sub_index: 0, type: int32, state_interface: velocity } # Velocity actual value
+      - { index: 0x6077, sub_index: 0, type: int16, state_interface: effort } # Torque actual value
+      - { index: 0x6061, sub_index: 0, type: int8 } # Mode of operation display
+      - { index: 0x3141, sub_index: 1, type: int16 } # Digital Input Properties
+      - { index: 0x60b9, sub_index: 0, type: int16 } # Touch Probe Status
+      - { index: 0x60ba, sub_index: 0, type: int32 } # Touch Probe Position 1 Positive edge
+      - { index: 0x60bb, sub_index: 0, type: int32 } # Touch Probe Position 1 Negative edge

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/config/zeroErr_config.yaml

@@ -0,0 +1,33 @@
+# Configuration file for Maxon EPOS3 drive
+vendor_id: 0x5a65726f
+product_id: 0x00029252
+assign-activate: 0x0300   #DC synch register
+auto_fault_reset: true
+auto_enable_set: false
+
+sdo:  # sdo data to be transferred at drive startup
+  - {index: 0x1C33, sub_index: 1, type: uint16, value: 2} # Set 0x1C33:01h = 0x02  DC设置
+
+rpdo: #PxPDO = receive PDO Mapping
+  - index: 0x1600
+    channels:
+      - {index: 0x607a, sub_index: 0, type: int32, command_interface: position, default: .nan, factor: 1.0, offset: 0} 
+      #target position	
+      - {index: 0x60ff, sub_index: 0, type: int32, default: 0, factor: 1.0}	#target velocity
+      - {index: 0x6071, sub_index: 0, type: int16, default: 0, factor: 1.0}	#target torque
+      - {index: 0x6072, sub_index: 0, type: uint16, default: 500}	# Max torque
+      - {index: 0x6040, sub_index: 0, type: uint16, default: 0}	# Control word
+      - {index: 0x6060, sub_index: 0, type: uint8, default: 8}	# Control operation
+      - {index: 0xf0ff, sub_index: 0, type: uint8}	# Dummy byte
+    
+tpdo:  #TxPDO = transmit PDO Mapping
+  - index: 0x1a00
+    channels:
+      - {index: 0x6064, sub_index: 0, type: int32, state_interface: position, factor: 1.0, offset: 0} #Position actual value	
+      - {index: 0x60F4, sub_index: 0, type: int32}	#Position Following error actual value
+      - {index: 0x606c, sub_index: 0, type: int32, state_interface: velocity, factor: 1.0 }	# Velocity actual value
+      - {index: 0x6077, sub_index: 0, type: int16, factor: 1.0 }	# Torque actual value
+      - {index: 0x6041, sub_index: 0, type: uint16}	# State word
+      - {index: 0x603f, sub_index: 0, type: uint16}	# Error code
+      - {index: 0x6061, sub_index: 0, type: uint8, state_interface: mode_of_operation}	        # Mode of operation display
+      - {index: 0xf0ff, sub_index: 0, type: uint8}	        # dummy byte

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/description/config/motor_drive.config.xacro

@@ -0,0 +1,9 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="motor_drive">
+
+  <link name="world"/>
+
+  <xacro:include filename="$(find ethercat_motor_drive)/description/ros2_control/motor_drive.ros2_control.xacro" />
+
+  <xacro:motor_drive/>
+</robot>

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/description/ros2_control/motor_drive.ros2_control.xacro

@@ -0,0 +1,30 @@
+<?xml version="1.0"?>
+<robot xmlns:xacro="http://www.ros.org/wiki/xacro">
+
+  <xacro:macro name="motor_drive">
+
+    <ros2_control name="motor_drive" type="system">
+      <hardware>
+        <plugin>ethercat_driver/EthercatDriver</plugin>
+        <param name="master_id">0</param>
+        <param name="control_frequency">1000</param>
+      </hardware>
+
+      <joint name="joint_1">
+        <state_interface name="position"/>
+        <state_interface name="velocity"/>
+        <command_interface name="position"/>
+        <command_interface name="reset_fault"/>
+        <ec_module name="SV660N">
+          <plugin>ethercat_generic_plugins/EcCiA402Drive</plugin>
+          <param name="alias">0</param>
+          <param name="position">0</param>
+          <param name="mode_of_operation">8</param>
+          <param name="slave_config">$(find ethercat_motor_drive)/config/zeroErr_config.yaml</param>
+        </ec_module>
+      </joint>
+    </ros2_control>
+
+  </xacro:macro>
+
+</robot>

HiveCoreR0/src/ethercat_driver_ros2_examples/ethercat_motor_drive/launch/motor_drive.launch.py

@@ -0,0 +1,113 @@
+# Copyright 2023 ICube Laboratory, University of Strasbourg
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from launch import LaunchDescription
+from launch.substitutions import Command, FindExecutable, PathJoinSubstitution, LaunchConfiguration
+from launch.actions import DeclareLaunchArgument
+from launch.actions import DeclareLaunchArgument, TimerAction
+
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+
+
+def generate_launch_description():
+
+    # Declare arguments
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            'description_file',
+            default_value='motor_drive.config.xacro',
+            description='URDF/XACRO description file with the axis.',
+        )
+    )
+
+    description_file = LaunchConfiguration('description_file')
+
+    # Get URDF via xacro
+    robot_description_content = Command(
+        [
+            PathJoinSubstitution([FindExecutable(name="xacro")]),
+            " ",
+            PathJoinSubstitution(
+                [
+                    FindPackageShare("ethercat_motor_drive"),
+                    "description/config",
+                    description_file,
+                ]
+            ),
+        ]
+    )
+    robot_description = {"robot_description": robot_description_content}
+
+    robot_controllers = PathJoinSubstitution(
+        [
+            FindPackageShare("ethercat_motor_drive"),
+            "config",
+            "controllers.yaml",
+        ]
+    )
+
+    control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[robot_description, robot_controllers],
+        output="both",
+    )
+    robot_state_pub_node = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        output="both",
+        parameters=[robot_description],
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["joint_state_broadcaster", "-c", "/controller_manager"],
+    )
+
+    trajectory_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["trajectory_controller", "-c", "/controller_manager"],
+    )
+
+    # velocity_controller_spawner = Node(
+    #     package="controller_manager",
+    #     executable="spawner",
+    #     arguments=["velocity_controller", "-c", "/controller_manager"],
+    # )
+
+    # effort_controller_spawner = Node(
+    #     package="controller_manager",
+    #     executable="spawner",
+    #     arguments=["effort_controller", "-c", "/controller_manager"],
+    # )
+
+    nodes = [
+        control_node,
+        robot_state_pub_node,
+        joint_state_broadcaster_spawner,
+        TimerAction(
+            period=30.0,  # 1 minutes delay
+            actions=[trajectory_controller_spawner],
+        ),
+        # velocity_controller_spawner,
+        # effort_controller_spawner,
+    ]
+
+    return LaunchDescription(
+        declared_arguments +
+        nodes)