From 43e06e8c4930e39183d46a223cf95838860d901f Mon Sep 17 00:00:00 2001 From: Manuel YGUEL Date: Tue, 4 Jun 2024 15:17:37 +0200 Subject: [PATCH 01/11] Transfer added to the master enabling safety (FsoE) Fix warnings: no more warnings when compiling. Fix warnings Remove warnings. Fix formatting Finish fixing warnings. Fix memory leak in exception catching. Reformat according to ament_uncrustify. Fix memory leak in exception catching. Finish fixing memory-leak Fix error in memory leak fix. [skip CI] [WIP] safety Fix memory leak in exception catching. First step for safety prototype. Parse the safety_estop.ros2_control.xacro correctly. [WIP] Automatic update of config file path by cmake for testing. [WIP] [WIP] Safety almost done. [WIP] Safety almost done. Change fsoe_config to YAML file YAML modification: tests pass. Set registerTransferInDomain after activating master. [skip ci] [WIP] All test passes. Add documentation in progress for safety. Add safety doc Update doc in safety code Add printing for place in process data from a slave position,index,sub_index Better output for slave operational info. Add binary option to display frames. Example of displaying just control_word and control_status for debugging [Bad] add finishProcessData virtual method to test our hypothesis. Add skip word in EcGenericSlave config. Fix bug to access pdos in generic_ec_cia402_drive Fix bug to access pdos in generic_ec_cia402_drive Let us have state_interfaces in rpdos and command interfaces in tpdos and in any kind of domain map we suppose we can have both. Remove logs for control word. [WIP] Major refactoring of ec_pdo_channel_manager to handle group of interfaces associated with a single PDO [WIP] Major refactoring of ec_pdo_channel_manager to handle group of interfaces associated with a single PDO day 2 [WIP] compile but do not link [WIP] Compile and links ok [WIP] Tests ok, warning one test check has been changed. TODO: check change in operation. [WIP] Add loadConfig group test. [WIP] Update example [WIP] Add more tests that pass [WIP] Fix linter. [WIP] Good output Improve tests for ec_pdo_channel_manager Refactoring: only one master incorporating transfers, renaming safety elements to transfer elements Remove standard outputs, every message is now output on the EthercatDriver message logs. --- ethercat_driver/CMakeLists.txt | 101 +++- ethercat_driver/ethercat_driver_plugin.xml | 9 +- .../configurations/beckhoff_ek1914.yaml | 93 ++++ .../configurations/beckhoff_el1918.yaml | 41 ++ .../configurations/estop_ethercat_safety.yaml | 11 + .../configurations/safety.ros2_control.xacro | 52 ++ .../safety_estop.ros2_control.template.xacro | 38 ++ .../test_config_ethercat_safety.yaml | 27 + .../update_slave_config_path.py | 68 +++ .../ethercat_driver/ethercat_driver.hpp | 60 ++- ethercat_driver/package.xml | 2 + ethercat_driver/src/ethercat_driver.cpp | 463 ++++++++++++++++-- .../testHelper_ethercat_safety_driver.hpp | 34 ++ .../test/test_ethercat_safety_driver.cpp | 227 +++++++++ .../developer_guide/domain_construction.rst | 98 ++++ .../sphinx/developer_guide/new_plugin.rst | 4 +- .../sphinx/developer_guide/safety.rst | 83 ++++ .../generic_ec_cia402_drive.hpp | 8 +- .../ethercat_generic_cia402_drive/package.xml | 4 +- .../src/generic_ec_cia402_drive.cpp | 102 ++-- .../test/test_generic_ec_cia402_drive.cpp | 66 +-- .../ethercat_generic_slave/CMakeLists.txt | 27 +- .../generic_ec_slave.hpp | 7 +- .../ethercat_generic_slave/package.xml | 2 +- .../src/generic_ec_slave.cpp | 136 +++-- .../test/test_generic_ec_slave.cpp | 48 +- ethercat_interface/CMakeLists.txt | 10 +- .../include/ethercat_interface/ec_master.hpp | 206 ++++++-- .../ec_pdo_channel_manager.hpp | 377 +++++++------- ...ec_pdo_group_interface_channel_manager.hpp | 278 +++++++++++ ...c_pdo_single_interface_channel_manager.hpp | 224 +++++++++ .../ethercat_interface/ec_sdo_manager.hpp | 1 + .../include/ethercat_interface/ec_slave.hpp | 42 +- .../ethercat_interface/ec_transfer.hpp | 54 ++ ethercat_interface/src/ec_master.cpp | 260 ++++++++-- .../src/ec_pdo_channel_manager.cpp | 309 ++++++++++++ ...ec_pdo_group_interface_channel_manager.cpp | 438 +++++++++++++++++ ...c_pdo_single_interface_channel_manager.cpp | 190 +++++++ .../test/test_ec_pdo_channel_manager.cpp | 297 ++++++++++- .../src/ethercat_sdo_srv_server.cpp | 35 +- 40 files changed, 4011 insertions(+), 521 deletions(-) create mode 100644 ethercat_driver/examples/configurations/beckhoff_ek1914.yaml create mode 100644 ethercat_driver/examples/configurations/beckhoff_el1918.yaml create mode 100644 ethercat_driver/examples/configurations/estop_ethercat_safety.yaml create mode 100644 ethercat_driver/examples/configurations/safety.ros2_control.xacro create mode 100644 ethercat_driver/examples/configurations/safety_estop.ros2_control.template.xacro create mode 100644 ethercat_driver/examples/configurations/test_config_ethercat_safety.yaml create mode 100644 ethercat_driver/examples/configurations/update_slave_config_path.py create mode 100644 ethercat_driver/test/testHelper_ethercat_safety_driver.hpp create mode 100644 ethercat_driver/test/test_ethercat_safety_driver.cpp create mode 100644 ethercat_driver_ros2/sphinx/developer_guide/domain_construction.rst create mode 100644 ethercat_driver_ros2/sphinx/developer_guide/safety.rst create mode 100644 ethercat_interface/include/ethercat_interface/ec_pdo_group_interface_channel_manager.hpp create mode 100644 ethercat_interface/include/ethercat_interface/ec_pdo_single_interface_channel_manager.hpp create mode 100644 ethercat_interface/include/ethercat_interface/ec_transfer.hpp create mode 100644 ethercat_interface/src/ec_pdo_channel_manager.cpp create mode 100644 ethercat_interface/src/ec_pdo_group_interface_channel_manager.cpp create mode 100644 ethercat_interface/src/ec_pdo_single_interface_channel_manager.cpp diff --git a/ethercat_driver/CMakeLists.txt b/ethercat_driver/CMakeLists.txt index a97e0e4a..65134a45 100644 --- a/ethercat_driver/CMakeLists.txt +++ b/ethercat_driver/CMakeLists.txt @@ -7,21 +7,24 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(ament_cmake_ros REQUIRED) + find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) find_package(ethercat_interface REQUIRED) +find_package(yaml_cpp_vendor REQUIRED) -add_library( - ${PROJECT_NAME} - SHARED - src/ethercat_driver.cpp) +file(GLOB_RECURSE PLUGINS_SRC src/*.cpp) -target_include_directories( - ${PROJECT_NAME} - PRIVATE - include +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 + $ + $ ) ament_target_dependencies( @@ -31,19 +34,24 @@ ament_target_dependencies( rclcpp rclcpp_lifecycle ethercat_interface + yaml_cpp_vendor ) # 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") +target_compile_definitions(${PROJECT_NAME} 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 + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install( DIRECTORY include/ @@ -51,7 +59,81 @@ install( ) if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) + find_package(ethercat_interface REQUIRED) + ament_lint_auto_find_test_dependencies() + + # Ensure test data is copied to the build directory + set(CFG_FILES + safety.ros2_control.xacro + safety_estop.ros2_control.template.xacro + estop_ethercat_safety.yaml + test_config_ethercat_safety.yaml + beckhoff_ek1914.yaml + beckhoff_el1918.yaml + ) + + set(CFG_IN ${CMAKE_CURRENT_SOURCE_DIR}/examples/configurations) + set(CFG_DST ${CMAKE_CURRENT_BINARY_DIR}/test_configurations) + list(TRANSFORM CFG_FILES PREPEND "${CFG_IN}" OUTPUT_VARIABLE CFG_SRC) + + # Ensure the destination directory exists + file(MAKE_DIRECTORY ${CFG_DST}) + + # List to keep track of the output files + set(CFG_OUT) + + foreach(file ${CFG_FILES}) + set(src_file "${CMAKE_CURRENT_SOURCE_DIR}/examples/configurations/${file}") + set(dst_file "${CFG_DST}/${file}") + + add_custom_command( + OUTPUT ${dst_file} + COMMAND ${CMAKE_COMMAND} -E copy ${src_file} ${dst_file} + DEPENDS ${src_file} + COMMENT "Copying ${src_file} to ${dst_file}" + ) + + list(APPEND CFG_OUT ${dst_file}) + endforeach() + # message(WARNING "Copying ${CFG_SRC} to ${CFG_DST}") + + # Create a custom target to group the copy commands + add_custom_target(copy_safety_config ALL DEPENDS ${CFG_OUT}) + + set(UP_PY ${CMAKE_CURRENT_SOURCE_DIR}/examples/configurations/update_slave_config_path.py) + set(UP_IN ${CFG_DST}/safety_estop.ros2_control.template.xacro) + set(UP_OUT ${CFG_DST}/safety_estop.ros2_control.xacro) + set(UP_DEPS ${UP_PY} ${UP_IN}) + + # message(WARNING "Running ${UP_PY} ${UP_IN} -p ${CFG_DST} -o ${UP_OUT}") + add_custom_command( + OUTPUT ${UP_OUT} + COMMAND ${CMAKE_COMMAND} -E echo "Running update_slave_config_path.py" + COMMAND ${CMAKE_COMMAND} -E env python3 ${UP_PY} ${UP_IN} -p ${CFG_DST} -o ${UP_OUT} + DEPENDS ${UP_DEPS} + COMMENT "Running update_slave_config_path.py" + ) + + add_custom_target(update_slave_config_path ALL DEPENDS ${UP_OUT}) + + ament_add_gmock(test_ethercat_safety_driver test/test_ethercat_safety_driver.cpp) + target_include_directories(test_ethercat_safety_driver PRIVATE include) + + target_compile_definitions(test_ethercat_safety_driver PRIVATE "TEST_RESOURCES_DIRECTORY=\"${CFG_DST}\"") + + target_link_libraries(test_ethercat_safety_driver + ${PROJECT_NAME} + ) + ament_target_dependencies(test_ethercat_safety_driver + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + ethercat_interface + yaml_cpp_vendor + ) endif() ## EXPORTS @@ -67,5 +149,6 @@ ament_export_dependencies( rclcpp rclcpp_lifecycle ethercat_interface + yaml_cpp_vendor ) ament_package() diff --git a/ethercat_driver/ethercat_driver_plugin.xml b/ethercat_driver/ethercat_driver_plugin.xml index f139569f..94b93521 100644 --- a/ethercat_driver/ethercat_driver_plugin.xml +++ b/ethercat_driver/ethercat_driver_plugin.xml @@ -1,9 +1,12 @@ - + EtherCAT Driver for ros2_control. + + + EtherCAT Safety Driver for ros2_control. + + diff --git a/ethercat_driver/examples/configurations/beckhoff_ek1914.yaml b/ethercat_driver/examples/configurations/beckhoff_ek1914.yaml new file mode 100644 index 00000000..fad8596b --- /dev/null +++ b/ethercat_driver/examples/configurations/beckhoff_ek1914.yaml @@ -0,0 +1,93 @@ +# Configuration file for Beckhoff EK1914 +# Description : Coupler, 4-channel digital input, +# 2-channel digital safety input, 4-channel digital output, +# 2-channel digital safety output, 24 V DC, 1 ms (TBC). +vendor_id: 0x00000002 +product_id: 0x077a2c52 +tpdo: # Slave-OUT & Master-IN + - index: 0x1a00 + channels: + - { index: 0x6000, sub_index: 0x01, type: int8 } + - { index: 0x6001, sub_index: 0x01, type: bool, mask: 1 } + - { index: 0x6001, sub_index: 0x02, type: bool, mask: 2 } + - { index: 0x0000, sub_index: 0x00, type: bit6 } + - { index: 0x6000, sub_index: 0x03, type: int16 } + - { index: 0x6000, sub_index: 0x02, type: int16 } + - index: 0x1a01 + channels: + - { + index: 0x6010, + sub_index: 0x01, + type: bool, + mask: 1, + state_interface: input_1, + } + - { + index: 0x6010, + sub_index: 0x02, + type: bool, + mask: 2, + state_interface: input_2, + } + - { + index: 0x6010, + sub_index: 0x03, + type: bool, + mask: 4, + state_interface: input_3, + } + - { + index: 0x6010, + sub_index: 0x04, + type: bool, + mask: 8, + state_interface: input_4, + } + - { index: 0x0000, sub_index: 0x00, type: bit12 } +rpdo: # Slave-IN & Master-OUT + - index: 0x1600 + channels: + - { index: 0x7000, sub_index: 0x01, type: int8 } + - { index: 0x7001, sub_index: 0x01, type: bool, mask: 1 } + - { index: 0x7001, sub_index: 0x02, type: bool, mask: 2 } + - { index: 0x0000, sub_index: 0x00, type: bit6 } + - { index: 0x7000, sub_index: 0x03, type: int16 } + - { index: 0x7000, sub_index: 0x02, type: int16 } + - index: 0x1601 + channels: + - { + index: 0x7010, + sub_index: 0x01, + type: bool, + mask: 1, + command_interface: output_1, + } + - { + index: 0x7010, + sub_index: 0x02, + type: bool, + mask: 2, + command_interface: output_2, + } + - { + index: 0x7010, + sub_index: 0x03, + type: bool, + mask: 4, + command_interface: output_3, + } + - { + index: 0x7010, + sub_index: 0x04, + type: bool, + mask: 8, + command_interface: output_4, + } + - { index: 0x7010, sub_index: 0x05, type: bool, mask: 16 } + - { index: 0x7010, sub_index: 0x06, type: bool, mask: 32 } + - { index: 0x0000, sub_index: 0x00, type: bit10 } +sm: + - { index: 0, type: output, watchdog: disable } + - { index: 1, type: input, watchdog: disable } + - { index: 2, type: output, pdo: rpdo, watchdog: disable } + - { index: 3, type: input, pdo: tpdo, watchdog: disable } diff --git a/ethercat_driver/examples/configurations/beckhoff_el1918.yaml b/ethercat_driver/examples/configurations/beckhoff_el1918.yaml new file mode 100644 index 00000000..26a6ae08 --- /dev/null +++ b/ethercat_driver/examples/configurations/beckhoff_el1918.yaml @@ -0,0 +1,41 @@ +# Configuration file for Beckhoff EL1918 +# Description : EtherCAT safety Terminal, master version, 8-channel digital safety input, 24 V DC, 10 us (TBC). +vendor_id: 0x00000002 +product_id: 0x077e3052 +tpdo: # TxPDO = transmit PDO Mapping, MASTER IN SLAVE OUT + - index: 0x1a00 + channels: + - { index: 0x6080, sub_index: 0x01, type: int8 } + - { index: 0x6081, sub_index: 0x01, type: int8 } + - { index: 0x6080, sub_index: 0x03, type: int16 } + - { index: 0x6080, sub_index: 0x02, type: int16 } + - index: 0x1bff + channels: + - { index: 0xf100, sub_index: 0x01, type: int8 } + - { index: 0xf100, sub_index: 0x02, type: int8 } +rpdo: # RxPDO = receive PDO Mapping, MASTER OUT SLAVE IN + - index: 0x1600 + channels: + - { index: 0x7080, sub_index: 0x01, type: int8 } + - { index: 0x7081, sub_index: 0x01, type: int8 } + - { index: 0x7080, sub_index: 0x03, type: int16 } + - { index: 0x7080, sub_index: 0x02, type: int16 } + - index: 0x17f0 + channels: + - { + index: 0xf788, + sub_index: 0x00, + type: int8, + command_interface: safety_states, # Bit 0 is safety error ACK and Bit 1 is safety RUN + } + - index: 0x17ff + channels: + - { index: 0x0000, sub_index: 0x00, type: bit16 } + +sm: # Sync Manager + - { index: 0, type: output, watchdog: disable } + - { index: 1, type: input, watchdog: disable } + - { index: 2, type: output, pdo: rpdo, watchdog: disable } + - { index: 3, type: input, pdo: tpdo, watchdog: disable } + - { index: 4, type: output, watchdog: disable } + - { index: 5, type: input, watchdog: disable } diff --git a/ethercat_driver/examples/configurations/estop_ethercat_safety.yaml b/ethercat_driver/examples/configurations/estop_ethercat_safety.yaml new file mode 100644 index 00000000..de8ae4b4 --- /dev/null +++ b/ethercat_driver/examples/configurations/estop_ethercat_safety.yaml @@ -0,0 +1,11 @@ +# Configuration file for the EtherCAT safety network of the estop test system +nets: + - name: safety_net + safety_master: el1918 + transfers: + - size: 6 + in: { ec_module: ek1914, index: 0x6000, subindex: 0x01 } + out: { ec_module: el1918, index: 0x7080, subindex: 0x01 } + - size: 6 + in: { ec_module: el1918, index: 0x6080, subindex: 0x01 } + out: { ec_module: ek1914, index: 0x7000, subindex: 0x01 } diff --git a/ethercat_driver/examples/configurations/safety.ros2_control.xacro b/ethercat_driver/examples/configurations/safety.ros2_control.xacro new file mode 100644 index 00000000..5fed3740 --- /dev/null +++ b/ethercat_driver/examples/configurations/safety.ros2_control.xacro @@ -0,0 +1,52 @@ + + + + + ethercat_driver/EthercatDriver + 0 + 100 + test_config_ethercat_safety.yaml + + + + + + + ethercat_generic_plugins/GenericEcSlave + 0 + 6 + beckhoff_el1018.yaml + + + + + + + + ethercat_generic_plugins/GenericEcSlave + 0 + 2 + beckhoff_el5101.yaml + + + + + + + + + + + + + ethercat_generic_plugins/EcCiA402Drive + 0 + 0 + + 9 + + maxon_epos3.yaml + + + + diff --git a/ethercat_driver/examples/configurations/safety_estop.ros2_control.template.xacro b/ethercat_driver/examples/configurations/safety_estop.ros2_control.template.xacro new file mode 100644 index 00000000..e2ff3462 --- /dev/null +++ b/ethercat_driver/examples/configurations/safety_estop.ros2_control.template.xacro @@ -0,0 +1,38 @@ + + + + + ethercat_driver/EthercatSafetyDriver + 0 + 100 + estop_ethercat_safety.yaml + + + + + 0 + 1 + beckhoff_el1918.yaml + + + + + + + + + + + + + + + + ethercat_generic_plugins/GenericEcSlave + 0 + 0 + beckhoff_ek1914.yaml + + + + diff --git a/ethercat_driver/examples/configurations/test_config_ethercat_safety.yaml b/ethercat_driver/examples/configurations/test_config_ethercat_safety.yaml new file mode 100644 index 00000000..b3a29c96 --- /dev/null +++ b/ethercat_driver/examples/configurations/test_config_ethercat_safety.yaml @@ -0,0 +1,27 @@ +# Configuration file for the EtherCAT safety network of the estop test system +safety_modules: + - name: s2 + plugin: ethercat_generic_plugins/GenericEcSlave + parameters: { alias: 0, position: 1, slave_config: safety_slave_s2.yaml } + - name: m1 + plugin: ethercat_generic_plugins/GenericEcSlave + parameters: { alias: 0, position: 4, slave_config: safety_master.yaml } + - name: m2 + plugin: ethercat_generic_plugins/GenericEcSlave + parameters: { alias: 0, position: 5, slave_config: safety_master.yaml } +nets: + - name: n1 + safety_master: m1 + transfers: + - size: 18 + in: { ec_module: s1, index: 0x607a, subindex: 0x02 } + out: { ec_module: s2, index: 0x60ff, subindex: 0x04 } + - size: 42 + in: { ec_module: s1, index: 0x707a, subindex: 0x01 } + out: { ec_module: s3, index: 0x10ff, subindex: 0x01 } + - name: n2 + safety_master: m2 + transfers: + - size: 18 + in: { ec_module: q1, index: 0x607a, subindex: 0x06 } + out: { ec_module: q2, index: 0x60ff, subindex: 0x03 } diff --git a/ethercat_driver/examples/configurations/update_slave_config_path.py b/ethercat_driver/examples/configurations/update_slave_config_path.py new file mode 100644 index 00000000..da065bd7 --- /dev/null +++ b/ethercat_driver/examples/configurations/update_slave_config_path.py @@ -0,0 +1,68 @@ +#!/usr/bin/env python3 +# Copyright 2024 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: Manuel YGUEL (yguel.robotics@gmail.com) + +# Executable python3 script to update the configuration of a slave device + +from lxml import etree +import os +import sys +import click + + +def prepend_slave_config_xml_tag(xml_file, prepend_path, output_file=None): + """ + Update the slave_config path in the xml file by prepending the build path. + + Open the xml file, search for the tags param in ec_module + that have name="slave_config" as attribute and update the slave_config + file path by appending the prepend_path to the existing path + """ + tree = etree.parse(xml_file) + find_all_param = tree.findall('.//param') + for param in find_all_param: + if param.attrib.get('name') == 'slave_config': + print(f"Found slave_config path: {param.text}") + param.text = os.path.join(prepend_path, param.text) + print(f"Updated slave_config path: {param.text}") + if param.attrib.get('name') == 'safety': + print(f"Found safety path: {param.text}") + param.text = os.path.join(prepend_path, param.text) + print(f"Updated safety path: {param.text}") + if output_file: + tree.write(output_file, pretty_print=True, + xml_declaration=True, encoding='UTF-8') + else: + tree.write(xml_file, pretty_print=True, + xml_declaration=True, encoding='UTF-8') + + +@click.command() +@click.argument('xml_file', type=click.Path(exists=True)) +@click.option('--prepend_path', '-p', type=click.Path(exists=True), + help='Prepend path', required=True) +@click.option('--output_file', '-o', type=click.Path(exists=False), + help='Output file', default=None) +def main(xml_file, prepend_path, output_file): + prepend_slave_config_xml_tag(xml_file, prepend_path, output_file) + msg = f"Updated the slave config path in {xml_file} with {prepend_path}" + if output_file: + msg += f" and saved to {output_file}" + else: + msg += f" and saved to {xml_file}" + print(msg) + return 0 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp b/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp index 8862c0a0..22f96e0f 100644 --- a/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp +++ b/ethercat_driver/include/ethercat_driver/ethercat_driver.hpp @@ -30,6 +30,7 @@ #include "ethercat_driver/visibility_control.h" #include "ethercat_interface/ec_slave.hpp" #include "ethercat_interface/ec_master.hpp" +#include "yaml-cpp/yaml.h" using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; @@ -65,10 +66,47 @@ class EthercatDriver : public hardware_interface::SystemInterface ETHERCAT_DRIVER_PUBLIC hardware_interface::return_type write(const rclcpp::Time &, const rclcpp::Duration &) override; -private: +protected: std::vector> getEcModuleParam( - std::string & urdf, std::string component_name, std::string component_type); - + const std::string & urdf, + const std::string & component_name, + const std::string & component_type); + + uint16_t getAliasOrDefaultAlias( + const std::unordered_map & slave_parameters); + + virtual CallbackReturn setupMaster(); + + CallbackReturn configNetwork(); + + /** @brief Load transfer config YAML file + * One use case is to load transfers for FailSafe Over EtherCAT Safety + * @param[out] node YAML node containing the transfer configuration root + * @param[in] path Path to the YAML file, if empty, the file is loaded from the *fsoe_config* + * or *transfer_config* of the YAML document + */ + void loadTransferConfigYamlFile(YAML::Node & node, const std::string & path = ""); + + /** @brief Get transfer module parameters from YAML file + * @param[in] config YAML node containing the transfer configuration root + * @return Vector of maps containing transfer module parameters, each map corresponds to a module + * involved in a transfer + */ + std::vector> getEcTransferModuleParam( + const YAML::Node & config); + + /** @brief Get transfer nets from YAML file + * @param[in] config YAML node containing the transfer configuration root + * @return Vector of transfer nets + */ + std::vector getEcTransferNets(const YAML::Node & config); + + /** @brief Configure the transfer networks + */ + void configTransferNetwork(); + +protected: std::vector> ec_modules_; std::vector> ec_module_parameters_; @@ -82,10 +120,22 @@ class EthercatDriver : public hardware_interface::SystemInterface pluginlib::ClassLoader ec_loader_{ "ethercat_interface", "ethercat_interface::EcSlave"}; - int control_frequency_; - ethercat_interface::EcMaster master_; + double control_frequency_; + + std::shared_ptr master_; std::mutex ec_mutex_; bool activated_; + + /** Transfer nets */ + std::vector ec_transfer_nets_; + + /** Indexes of modules inside ec_modules_ vector that are transfer masters */ + std::vector ec_transfer_masters_; + /** Indexes of modules inside ec_modules_ vector that are transfer slaves only */ + std::vector ec_transfer_slaves_; + + /** Empty interfaces */ + std::vector empty_interface_; }; } // namespace ethercat_driver diff --git a/ethercat_driver/package.xml b/ethercat_driver/package.xml index 15c0d486..29d6d4df 100644 --- a/ethercat_driver/package.xml +++ b/ethercat_driver/package.xml @@ -14,6 +14,8 @@ rclcpp rclcpp_lifecycle ethercat_interface + yaml_cpp_vendor + yaml-cpp ament_lint_auto ament_lint_common diff --git a/ethercat_driver/src/ethercat_driver.cpp b/ethercat_driver/src/ethercat_driver.cpp index deea06b5..89028b3b 100644 --- a/ethercat_driver/src/ethercat_driver.cpp +++ b/ethercat_driver/src/ethercat_driver.cpp @@ -23,6 +23,71 @@ namespace ethercat_driver { + +unsigned int uint_from_string(const std::string & str) +{ + // Strip leading and trailing whitespaces + std::string s = std::regex_replace(str, std::regex("^ +| +$|( ) +"), "$1"); + // Test if the number is in hexadecimal format + if (s.find("0x") == 0) { + return std::stoul(s, nullptr, 16); + } + return std::stoul(s); +} + +void getTransferMemoryInfo( + const YAML::Node & element, + ethercat_interface::EcMemoryEntry & entry, + const std::string & dir, + const std::string & transfer_net_name) +{ + if (!element["ec_module"]) { + std::string msg = "Transfer definition without ec_module entry, net: " + + transfer_net_name + " direction: " + dir; + throw std::runtime_error(msg); + } + if (!element["index"]) { + std::string msg = "Transfer definition without index entry, net: " + + transfer_net_name + " direction: " + dir; + throw std::runtime_error(msg); + } + if (!element["subindex"]) { + std::string msg = "Transfer definition without subindex entry, net: " + + transfer_net_name + " direction: " + dir; + throw std::runtime_error(msg); + } + + entry.module_name = element["ec_module"].as(); + entry.index = uint_from_string(element["index"].as()); + entry.subindex = uint_from_string(element["subindex"].as()); +} + +void throwErrorIfModuleParametersNotFound( + const ethercat_interface::EcTransferEntry & transfer, + const std::string & module_name, + const std::string & transfer_net_name, + const std::string & direction) +{ + std::string msg = "In transfer net: " + transfer_net_name + ", for transfer " + + transfer.to_simple_string() + ", the module name of the " + direction + "( " + module_name + + ") among all the recorded modules."; + RCLCPP_ERROR( + rclcpp::get_logger( + "EthercatDriver"), msg.c_str()); + throw std::runtime_error(msg); +} + +uint16_t EthercatDriver::getAliasOrDefaultAlias( + const std::unordered_map & slave_parameters) +{ + if (slave_parameters.find("alias") != slave_parameters.end()) { + return std::stoul(slave_parameters.at("alias")); + } else { + return 0; + } +} + CallbackReturn EthercatDriver::on_init( const hardware_interface::HardwareInfo & info) { @@ -33,6 +98,7 @@ CallbackReturn EthercatDriver::on_init( const std::lock_guard lock(ec_mutex_); activated_ = false; + // Set state vectors hw_joint_states_.resize(info_.joints.size()); for (uint j = 0; j < info_.joints.size(); j++) { hw_joint_states_[j].resize( @@ -51,6 +117,8 @@ CallbackReturn EthercatDriver::on_init( info_.gpios[g].state_interfaces.size(), std::numeric_limits::quiet_NaN()); } + + // Set command vectors hw_joint_commands_.resize(info_.joints.size()); for (uint j = 0; j < info_.joints.size(); j++) { hw_joint_commands_[j].resize( @@ -70,6 +138,7 @@ CallbackReturn EthercatDriver::on_init( std::numeric_limits::quiet_NaN()); } + // Setup slave modules defined per joints in the URDF 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_ @@ -95,6 +164,9 @@ CallbackReturn EthercatDriver::on_init( "Setup of Joint module %li FAILED.", i + 1); return CallbackReturn::ERROR; } + module->setAliasAndPosition( + getAliasOrDefaultAlias(module_params[i]), + std::stoul(module_params[i].at("position"))); ec_modules_.push_back(module); } catch (pluginlib::PluginlibException & ex) { RCLCPP_FATAL( @@ -104,6 +176,8 @@ CallbackReturn EthercatDriver::on_init( } } } + + // Setup slave modules defined per GPIOs in the URDF 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_ @@ -129,6 +203,9 @@ CallbackReturn EthercatDriver::on_init( "Setup of GPIO module %li FAILED.", i + 1); return CallbackReturn::ERROR; } + module->setAliasAndPosition( + getAliasOrDefaultAlias(module_params[i]), + std::stoul(module_params[i].at("position"))); ec_modules_.push_back(module); } catch (pluginlib::PluginlibException & ex) { RCLCPP_FATAL( @@ -138,6 +215,8 @@ CallbackReturn EthercatDriver::on_init( } } } + + // Setup slave modules defined per sensors in the URDF 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_ @@ -163,6 +242,9 @@ CallbackReturn EthercatDriver::on_init( "Setup of Sensor module %li FAILED.", i + 1); return CallbackReturn::ERROR; } + module->setAliasAndPosition( + getAliasOrDefaultAlias(module_params[i]), + std::stoul(module_params[i].at("position"))); ec_modules_.push_back(module); } catch (pluginlib::PluginlibException & ex) { RCLCPP_FATAL( @@ -175,6 +257,117 @@ CallbackReturn EthercatDriver::on_init( RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Got %li modules", ec_modules_.size()); + // Check if a transfer configuration is provided + if (info_.hardware_parameters.find("fsoe_config") != info_.hardware_parameters.end() || + info_.hardware_parameters.find("transfer_config") != info_.hardware_parameters.end()) + { + RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Transfer configuration detected, ..."); + + YAML::Node config; + // Load the transfer config file + loadTransferConfigYamlFile(config); + + // Parse transfer modules from the transfer yaml file + auto transfer_module_params = getEcTransferModuleParam(config); + + // Append the transfer modules parameters to the list of modules parameters + size_t idx_1st = ec_module_parameters_.size(); + ec_module_parameters_.insert( + ec_module_parameters_.end(), transfer_module_params.begin(), transfer_module_params.end()); + for (size_t i = 0; i < transfer_module_params.size(); i++) { + ec_transfer_slaves_.push_back(idx_1st + i); + } + + // Parse transfer nets from the transfer yaml file + ec_transfer_nets_ = getEcTransferNets(config); + + // Append the transfer modules to the list of modules and load them + for (const auto & transfer_module_param : transfer_module_params) { + try { + auto ec_module = ec_loader_.createSharedInstance(transfer_module_param.at("plugin")); + if (!ec_module->setupSlave( + transfer_module_param, &empty_interface_, &empty_interface_)) + { + const std::string & module_name = transfer_module_param.at("name"); + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), + "Setup of transfer only module %s FAILED.", module_name.c_str() ); + return CallbackReturn::ERROR; + } + + auto idx = ec_modules_.size(); + ec_module->setAliasAndPosition( + getAliasOrDefaultAlias(transfer_module_param), + std::stoul(transfer_module_param.at("position"))); + ec_modules_.push_back(ec_module); + ec_transfer_slaves_.push_back(idx); + } catch (const pluginlib::PluginlibException & ex) { + const std::string & module_name = transfer_module_param.at("name"); + RCLCPP_ERROR( + rclcpp::get_logger( + "EthercatDriver"), + "The plugin failed to load for transfer module %s. Error: %s\n", + module_name.c_str(), ex.what()); + } + } + + // Find all masters from the nets + { + std::vector master_names; + for (const auto & net : ec_transfer_nets_) { + master_names.push_back(net.master); + } + for (size_t i = 0; i < ec_module_parameters_.size(); i++) { + if (std::find( + master_names.begin(), master_names.end(), + ec_module_parameters_[i].at("name")) != + master_names.end()) + { + ec_transfer_masters_.push_back(i); + } + } + } + + // Identify (alias,position) all the modules participating in transfers + for (auto & net : ec_transfer_nets_) { + for (auto & transfer : net.transfers) { + // Update each EcMemoryEntry with the alias and position of the module + size_t in_idx = ec_module_parameters_.size(); + for (in_idx = 0; in_idx < ec_module_parameters_.size(); ++in_idx) { + if (ec_module_parameters_[in_idx].at("name") == transfer.input.module_name) { + break; + } + } + size_t out_idx = ec_module_parameters_.size(); + for (out_idx = 0; out_idx < ec_module_parameters_.size(); ++out_idx) { + if (ec_module_parameters_[out_idx].at("name") == transfer.output.module_name) { + break; + } + } + if (in_idx == ec_module_parameters_.size()) { + throwErrorIfModuleParametersNotFound( + transfer, transfer.input.module_name, net.name, "input"); + } + if (out_idx == ec_module_parameters_.size()) { + throwErrorIfModuleParametersNotFound( + transfer, transfer.output.module_name, net.name, "output"); + } + + const auto & input_module = ec_modules_[in_idx]; + const auto & output_module = ec_modules_[out_idx]; + + transfer.input.alias = input_module->alias_; + transfer.input.position = input_module->position_; + transfer.output.alias = output_module->alias_; + transfer.output.position = output_module->position_; + } + } + + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), + "Transfer configuration loaded successfully!"); + } + return CallbackReturn::SUCCESS; } @@ -259,64 +452,100 @@ EthercatDriver::export_command_interfaces() return command_interfaces; } -CallbackReturn EthercatDriver::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) +CallbackReturn EthercatDriver::setupMaster() { - const std::lock_guard 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; + unsigned int master_id = 666; + // Get master id + if (info_.hardware_parameters.find("master_id") == info_.hardware_parameters.end()) { + // Master id was not provided, default to 0 + master_id = 0; } else { - control_frequency_ = std::stod(info_.hardware_parameters["control_frequency"]); + try { + master_id = std::stoul(info_.hardware_parameters["master_id"]); + } catch (std::exception & e) { + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), "Invalid master id (%s)!", e.what()); + return CallbackReturn::ERROR; + } } + master_ = std::make_shared(master_id); - if (control_frequency_ < 0) { - RCLCPP_FATAL( - rclcpp::get_logger("EthercatDriver"), "Invalid control frequency!"); - return CallbackReturn::ERROR; + return CallbackReturn::SUCCESS; +} + +CallbackReturn EthercatDriver::configNetwork() +{ + // Get control frequency + if (info_.hardware_parameters.find("control_frequency") == info_.hardware_parameters.end()) { + // Control frequency was not provided, default to 100 Hz + control_frequency_ = 100.0; + } else { + try { + control_frequency_ = std::stod(info_.hardware_parameters["control_frequency"]); + } catch (std::exception & e) { + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), "Invalid control frequency (%s)!", e.what()); + return CallbackReturn::ERROR; + } } // start EC and wait until state operative - master_.setCtrlFrequency(control_frequency_); + 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()); + master_->addSlave(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( + int ret = master_->configSlaveSdo( std::stod(ec_module_parameters_[i]["position"]), sdo, - &abort_code - ); + &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 - ); + abort_code); } } } - if (!master_.activate()) { + return CallbackReturn::SUCCESS; +} + +CallbackReturn EthercatDriver::on_activate( + const rclcpp_lifecycle::State & /*previous_state*/) +{ + const std::lock_guard 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..."); + + // setup master + setupMaster(); + // configure network + configNetwork(); + + if (!master_->activate()) { RCLCPP_ERROR(rclcpp::get_logger("EthercatDriver"), "Activate EcMaster failed"); return CallbackReturn::ERROR; } RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Activated EcMaster!"); + // Configure transfer network if transfer nets are defined + if (!ec_transfer_nets_.empty()) { + RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Configuring transfer network..."); + master_->registerTransferInDomain(ec_transfer_nets_); + RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Transfer network configured!"); + } + // start after one second struct timespec t; clock_gettime(CLOCK_MONOTONIC, &t); @@ -328,7 +557,7 @@ CallbackReturn EthercatDriver::on_activate( clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &t, NULL); // update EtherCAT bus - master_.update(); + master_->update(); RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "updated!"); // check if operational @@ -340,7 +569,7 @@ CallbackReturn EthercatDriver::on_activate( running = false; } // calculate next shot. carry over nanoseconds into microseconds. - t.tv_nsec += master_.getInterval(); + t.tv_nsec += master_->getInterval(); while (t.tv_nsec >= 1000000000) { t.tv_nsec -= 1000000000; t.tv_sec++; @@ -364,7 +593,7 @@ CallbackReturn EthercatDriver::on_deactivate( RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Stopping ...please wait..."); // stop EC and disconnect - master_.stop(); + master_->stop(); RCLCPP_INFO( rclcpp::get_logger("EthercatDriver"), "System successfully stopped!"); @@ -379,7 +608,7 @@ hardware_interface::return_type EthercatDriver::read( // try to lock so we can avoid blocking the read/write loop on the lock. const std::unique_lock lock(ec_mutex_, std::try_to_lock); if (lock.owns_lock() && activated_) { - master_.readData(); + master_->readData(); } return hardware_interface::return_type::OK; } @@ -391,15 +620,15 @@ hardware_interface::return_type EthercatDriver::write( // try to lock so we can avoid blocking the read/write loop on the lock. const std::unique_lock lock(ec_mutex_, std::try_to_lock); if (lock.owns_lock() && activated_) { - master_.writeData(); + master_->writeData(); } return hardware_interface::return_type::OK; } std::vector> EthercatDriver::getEcModuleParam( - std::string & urdf, - std::string component_name, - std::string component_type) + const std::string & urdf, + const std::string & component_name, + const std::string & component_type) { // Check if everything OK with URDF string if (urdf.empty()) { @@ -455,6 +684,172 @@ std::vector> EthercatDriver::getEcM return module_params; } +void EthercatDriver::loadTransferConfigYamlFile(YAML::Node & node, const std::string & path) +{ + std::string file_path; + if (path.empty()) { + // Get the fsoe_config or transfer_config parameter of the ethercat_driver hardware plugin + if (info_.hardware_parameters.find("fsoe_config") == info_.hardware_parameters.end() || + info_.hardware_parameters.find("transfer_config") == info_.hardware_parameters.end() ) + { + std::string msg("transfer_config or fsoe_config parameter is missing!"); + // Transfer (or fsoe) config file was not provided + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), msg.c_str()); + throw std::runtime_error(msg); + } + if (info_.hardware_parameters.find("fsoe_config") == info_.hardware_parameters.end() ) { + file_path = info_.hardware_parameters.at("fsoe_config"); + } + if (info_.hardware_parameters.find("transfer_config") == info_.hardware_parameters.end()) { + file_path = info_.hardware_parameters.at("transfer_config"); + } + } else { + file_path = path; + } + + try { + node = YAML::LoadFile(file_path); + } catch (const YAML::ParserException & ex) { + std::string msg = + std::string( + "EthercatDriver : failed to load transfer configuration " + "(YAML file is incorrect): ") + std::string(ex.what()); + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), msg.c_str() ); + throw std::runtime_error(msg); + } catch (const YAML::BadFile & ex) { + std::string msg = + std::string( + "EthercatDriver : failed to load transfer configuration " + "(file path is incorrect or file is damaged): " + std::string(ex.what())); + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), msg.c_str() ); + throw std::runtime_error(msg); + } catch (std::exception & e) { + std::string msg = + std::string( + "EthercatDriver : error while loading transfer configuration: ") + std::string(e.what()); + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), msg.c_str() ); + throw std::runtime_error(msg); + } +} + +std::vector> EthercatDriver::getEcTransferModuleParam( + const YAML::Node & config) +{ + if (0 == config.size() ) { + std::string msg = "Empty transfer_config or fsoe_config parameter!"; + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), msg.c_str()); + throw std::runtime_error(msg); + } + std::vector> module_params; + std::unordered_map module_param; + + // It is possible that modules are only involved in transfers and hence + // not declared in the ros2_control xacro file. + // This is a common situation with modules only involved in safety + // operations. In this case, it is necessary to find the plugin to load, + // the position and the alias for those slaves. + if (config["transfer_modules"]) { + for (const auto & module : config["transfer_modules"]) { + module_param.clear(); + module_param["name"] = module["name"].as(); + module_param["plugin"] = module["plugin"].as(); + for (const auto & param : module["parameters"]) { + module_param[param.first.as()] = param.second.as(); + } + module_params.push_back(module_param); + } + } + + if (config["safety_modules"]) { + for (const auto & module : config["safety_modules"]) { + module_param.clear(); + module_param["name"] = module["name"].as(); + module_param["plugin"] = module["plugin"].as(); + for (const auto & param : module["parameters"]) { + module_param[param.first.as()] = param.second.as(); + } + module_params.push_back(module_param); + } + } + + return module_params; +} + +std::vector EthercatDriver::getEcTransferNets( + const YAML::Node & config) +{ + if (0 == config.size() ) { + std::string msg = "Empty transfer_config or fsoe_config parameter!"; + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), msg.c_str()); + throw std::runtime_error(msg); + } + + std::vector transfer_nets; + ethercat_interface::EcTransferNet transfer_net; + + if (config["nets"]) { + for (const auto & net : config["nets"]) { + transfer_net.reset(net["name"].as()); + if (net["safety_master"]) { + transfer_net.master = net["safety_master"].as(); + } + if (net["transfer_master"]) { + transfer_net.master = net["transfer_master"].as(); + } + for (const auto & transfer : net["transfers"]) { + ethercat_interface::EcTransferEntry transfer_entry; + if (!transfer["size"]) { + std::string msg = "ERROR: transfer n°" + std::to_string(transfer_nets.size()) + + " of net " + + transfer_net.name + " : definition without «size» parameter"; + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), msg.c_str()); + throw std::runtime_error(msg); + } + if (!transfer["in"]) { + std::string msg = "ERROR: transfer n°" + std::to_string(transfer_nets.size()) + + " of net " + + transfer_net.name + " : definition without «in» parameter"; + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), msg.c_str()); + throw std::runtime_error(msg); + } + if (!transfer["out"]) { + std::string msg = "ERROR: transfer n°" + std::to_string(transfer_nets.size()) + + " of net " + + transfer_net.name + " : definition without «out» parameter"; + RCLCPP_FATAL( + rclcpp::get_logger("EthercatDriver"), msg.c_str()); + throw std::runtime_error(msg); + } + transfer_entry.size = transfer["size"].as(); + getTransferMemoryInfo( + transfer["in"], transfer_entry.input, + "in", transfer_net.name); + getTransferMemoryInfo( + transfer["out"], transfer_entry.output, + "out", transfer_net.name); + transfer_net.transfers.push_back(transfer_entry); + } + transfer_nets.push_back(transfer_net); + } + } + + return transfer_nets; +} + +void EthercatDriver::configTransferNetwork() +{ + // This method can be used for additional transfer network configuration if needed + // Currently, transfer network configuration is handled in on_activate() +} + } // namespace ethercat_driver #include "pluginlib/class_list_macros.hpp" diff --git a/ethercat_driver/test/testHelper_ethercat_safety_driver.hpp b/ethercat_driver/test/testHelper_ethercat_safety_driver.hpp new file mode 100644 index 00000000..e2543e4e --- /dev/null +++ b/ethercat_driver/test/testHelper_ethercat_safety_driver.hpp @@ -0,0 +1,34 @@ +// Copyright 2024 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: Manuel YGUEL (yguel.robotics@gmail.com) + +#ifndef TESTHELPER_ETHERCAT_SAFETY_DRIVER_HPP_ +#define TESTHELPER_ETHERCAT_SAFETY_DRIVER_HPP_ + +#include "ethercat_driver/ethercat_driver.hpp" + +namespace ethercat_driver +{ + +class TestHelperEthercatSafetyDriver : public ethercat_driver::EthercatDriver +{ +public: + using EthercatDriver::getEcTransferModuleParam; + using EthercatDriver::getEcTransferNets; +}; + +} // namespace ethercat_driver + +#endif // TESTHELPER_ETHERCAT_SAFETY_DRIVER_HPP_ diff --git a/ethercat_driver/test/test_ethercat_safety_driver.cpp b/ethercat_driver/test/test_ethercat_safety_driver.cpp new file mode 100644 index 00000000..24019321 --- /dev/null +++ b/ethercat_driver/test/test_ethercat_safety_driver.cpp @@ -0,0 +1,227 @@ +// Copyright 2024 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: Manuel YGUEL (yguel.robotics@gmail.com) + +// Executes directly the gtest, call: +// ./build/ethercat_driver/test_ethercat_safety_driver +// from the root directory of the workspace + +#include +#include +#include +#include + +#include "testHelper_ethercat_safety_driver.hpp" + +TEST(TestEthercatSafetyDriver, getEcTransferModuleParam) +{ + ethercat_driver::TestHelperEthercatSafetyDriver driver; + std::filesystem::path dir = TEST_RESOURCES_DIRECTORY; + const std::string test_config_path = dir / "test_config_ethercat_safety.yaml"; + + std::cout << "Test config path: " << test_config_path << std::endl; + + YAML::Node config = YAML::LoadFile(test_config_path); + + auto modules = driver.getEcTransferModuleParam(config); + std::set names; + for (auto & module : modules) { + auto it = module.find("name"); + EXPECT_TRUE( + it != + nullptr) << "name is not a field of the ec safety module" << std::endl; + names.insert(it->second); + } + std::set expected_names = { + "s2", "m1", "m2" + }; + EXPECT_EQ(names, expected_names) << "Ec module names are not as expected" << std::endl; +} + +TEST(TestEthercatSafetyDriver, getEcTransferNet) +{ + ethercat_driver::TestHelperEthercatSafetyDriver driver; + std::string yaml; + { + std::filesystem::path dir = TEST_RESOURCES_DIRECTORY; + const std::string test_config_path = dir / "test_config_ethercat_safety.yaml"; + std::ifstream file(test_config_path); + std::stringstream buffer; + buffer << file.rdbuf(); + yaml = buffer.str(); + } + + YAML::Node config = YAML::Load(yaml); + + auto nets = driver.getEcTransferNets(config); + std::set net_names; + + EXPECT_EQ(nets.size(), 2) << "Number of safety nets is not as expected" << std::endl; + for (auto & net : nets) { + net_names.insert(net.name); + } + std::set expected_net_names = { + "n1", "n2" + }; + EXPECT_EQ(net_names, expected_net_names) << "Net names are not as expected" << std::endl; + + int net_1_index = nets[0].name == "n1" ? 0 : 1; + int net_2_index = (net_1_index + 1) % 2; + + auto & net1 = nets[net_1_index]; + auto & net2 = nets[net_2_index]; + + // Net 1 + EXPECT_EQ(net1.master, "m1") << "Master name is not as expected" << std::endl; + EXPECT_EQ( + net1.transfers.size(), + 2) << "Number of transfers is not as expected for net n1" << std::endl; + { + { + auto & tr = net1.transfers[0]; + EXPECT_EQ( + tr.input.module_name, + "s1") << "Input module name is not as expected for 1st transfer of net n1" << std::endl; + EXPECT_EQ( + tr.input.index, + 0x607a) << "Input index is not as expected for 1st transfer of net n1" << std::endl; + + EXPECT_EQ( + tr.output.module_name, + "s2") << "Output module name is not as expected for 1st transfer of net n1" << std::endl; + EXPECT_EQ( + tr.output.index, + 0x60ff) << "Output index is not as expected for 1st transfer of net n1" << std::endl; + + EXPECT_EQ( + tr.size, + 18) << "Size is not as expected for 1st transfer of net n1" << std::endl; + } + + { + auto & tr = net1.transfers[1]; + EXPECT_EQ( + tr.input.module_name, + "s1") << "Input module name is not as expected for 2nd transfer of net n1" << std::endl; + EXPECT_EQ( + tr.input.index, + 0x707a) << "Input index is not as expected for 2nd transfer of net n1" << std::endl; + + + EXPECT_EQ( + tr.output.module_name, + "s3") << "Output module name is not as expected for 2nd transfer of net n1" << std::endl; + EXPECT_EQ( + tr.output.index, + 0x10ff) << "Output index is not as expected for 2nd transfer of net n1" << std::endl; + + EXPECT_EQ( + tr.size, + 42) << "Size is not as expected for 2nd transfer of net n1" << std::endl; + } + } + + // Net 2 + EXPECT_EQ(net2.master, "m2") << "Master name is not as expected" << std::endl; + EXPECT_EQ( + net2.transfers.size(), + 1) << "Number of transfers is not as expected for net n2" << std::endl; + { + auto & tr = net2.transfers[0]; + EXPECT_EQ( + tr.input.module_name, + "q1") << "Input module name is not as expected for 1st transfer of net n2" << std::endl; + EXPECT_EQ( + tr.input.index, + 0x607a) << "Input index is not as expected for 1st transfer of net n2" << std::endl; + + EXPECT_EQ( + tr.output.module_name, + "q2") << "Output module name is not as expected for 1st transfer of net n2" << std::endl; + EXPECT_EQ( + tr.output.index, + 0x60ff) << "Output index is not as expected for 1st transfer of net n2" << std::endl; + + EXPECT_EQ( + tr.size, + 18) << "Size is not as expected for 1st transfer of net n2" << std::endl; + } +} + +TEST(TestEthercatSafetyDriver, estopParseConfigFile) +{ + ethercat_driver::TestHelperEthercatSafetyDriver driver; + std::string yaml; + { + std::filesystem::path dir = TEST_RESOURCES_DIRECTORY; + const std::string test_config_path = dir / "estop_ethercat_safety.yaml"; + std::ifstream file(test_config_path); + std::stringstream buffer; + buffer << file.rdbuf(); + yaml = buffer.str(); + } + + YAML::Node config = YAML::Load(yaml); + + auto nets = driver.getEcTransferNets(config); + EXPECT_EQ(1, nets.size()) << "Number of safety nets is not as expected" << std::endl; + + auto & net = nets[0]; + EXPECT_EQ(net.master, "el1918") << "Master name is not as expected" << std::endl; + + EXPECT_EQ( + net.transfers.size(), + 2) << "Number of transfers is not as expected for net n1" << std::endl; + { + auto & tr = net.transfers[0]; + EXPECT_EQ( + tr.input.module_name, + "ek1914") << "Input module name is not as expected for 1st transfer of net n1" << std::endl; + EXPECT_EQ( + tr.input.index, + 0x6000) << "Input index is not as expected for 1st transfer" << std::endl; + + EXPECT_EQ( + tr.output.module_name, + "el1918") << "Output module name is not as expected for 1st transfer" << std::endl; + EXPECT_EQ( + tr.output.index, + 0x7080) << "Output index is not as expected for 1st transfer" << std::endl; + + EXPECT_EQ( + tr.size, + 6) << "Size is not as expected for 1st transfer" << std::endl; + } + { + auto & tr = net.transfers[1]; + EXPECT_EQ( + tr.input.module_name, + "el1918") << "Input module name is not as expected for 2nd transfer" << std::endl; + EXPECT_EQ( + tr.input.index, + 0x6080) << "Input index is not as expected for 2nd transfer" << std::endl; + + EXPECT_EQ( + tr.output.module_name, + "ek1914") << "Output module name is not as expected for 2nd transfer" << std::endl; + EXPECT_EQ( + tr.output.index, + 0x7000) << "Output index is not as expected for 2nd transfer" << std::endl; + + EXPECT_EQ( + tr.size, + 6) << "Size is not as expected for 2nd transfer" << std::endl; + } +} diff --git a/ethercat_driver_ros2/sphinx/developer_guide/domain_construction.rst b/ethercat_driver_ros2/sphinx/developer_guide/domain_construction.rst new file mode 100644 index 00000000..cd5cfd43 --- /dev/null +++ b/ethercat_driver_ros2/sphinx/developer_guide/domain_construction.rst @@ -0,0 +1,98 @@ +How domain construction is done +=============================== + + +Interface between the master and the application +------------------------------------------------ + +The link between the data used by the application and the one that transit on the EtherCAT network as EtherCAT frames is explicitly provided via the definition of domains. +Domains are contiguous memory areas that transit on the EtherCAT network. + +This mapping is provided to the application as offsets inside the communication frames in the domains. +For instance, is a 32bit integer is used by the application, the application got a pointer :math:`\text{pd}` to the start of the domain and an offset :math:`o` that give the pointer :math:`p` to the first octet of the integer via pointer arithmetic: :math:`\text{p} = \text{pd} + o`. + +The application has access to objects of type :code:`ec_pdo_entry_reg_t` (IgH master data structures) that contains the information to access the data in the domain: + +.. code-block:: cpp + + typedef struct { + uint16_t alias; /**< Slave alias address. */ + uint16_t position; /**< Slave position. */ + uint32_t vendor_id; /**< Slave vendor ID. */ + uint32_t product_code; /**< Slave product code. */ + uint16_t index; /**< PDO entry index. */ + uint8_t subindex; /**< PDO entry subindex. */ + unsigned int *offset; /**< Pointer to a variable to store the PDO entry's + (byte-)offset in the process data. */ + unsigned int *bit_position; /**< Pointer to a variable to store a bit + position (0-7) within the \a offset. Can be + NULL, in which case an error is raised if the + PDO entry does not byte-align. */ + } ec_pdo_entry_reg_t; + +In this structure the :code:`offset` and :code:`bit_position` are pointers to the offset and bit position of the data in the domain because this structure is given to the master that fill in those values establishing the link for the application with its data. + +The application configures its link with the EtherCAT master via separate API calls. +It first creates domains with: +1. :code:`ec_domain_t * ecrt_master_create_domain(const ec_master_t *master)` +Then it provides the master with: +2. the list of slaves that are used in the domain + +.. code-block:: cpp + + ec_slave_config_t *ecrt_master_slave_config( + ec_master_t *master, /**< EtherCAT master */ + uint16_t alias, /**< Slave alias. */ + uint16_t position, /**< Slave position. */ + uint32_t vendor_id, /**< Expected vendor ID. */ + uint32_t product_code /**< Expected product code. */ + ); + +3. the list of PDO entries that are used in the domain, via an array of :code:`ec_pdo_entry_reg_t` objects, where offset and bit_position points to the data the application will use and have to be filled by the master: :code:`bool ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain, ec_pdo_entry_reg_t *entries)` +4. It asks for pointer to the start of the domain memory: :code:`uint8_t * ecrt_domain_data(ec_domain_t *domain)` +One domain can be used for read and one for writing data for instance, so the application may make several calls to the above functions to create the domains it needs. + +ethercat_driver_ros2 data structure organization +------------------------------------------------ + +The library keeps a representation of the masters used as :code:`EcMaster` objects. +Each :code:`EcMaster` object keeps a collection of domain configurations as :code:`DomainInfo` objects organised in a map with the domain index as key. +In a :code:`DomainInfo` a vector of :code:`ec_pdo_entry_reg_t` objects is kept that records the PDO entries used by the application with the links/offsets into that domain data. + +.. code-block:: cpp + + 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 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 entries; + }; + + struct EcMaster { + ... + ec_master_t *master; + ... + std::map domains; + ... + }; + +All the :code:`DomainInfo` data structures are created during the calls to the :code:`addSlave` method of the :code:`EcMaster` object. :code:`ec_pdo_entry_reg_t` objects are effectively created by :code:`addSlave` with a call of the :code:`registerPDOInDomain` method. + +The link is made at the :code:`on_activate` stage of the hardware interface life-cycle in ros2 control. diff --git a/ethercat_driver_ros2/sphinx/developer_guide/new_plugin.rst b/ethercat_driver_ros2/sphinx/developer_guide/new_plugin.rst index 145550cd..3de5dd66 100644 --- a/ethercat_driver_ros2/sphinx/developer_guide/new_plugin.rst +++ b/ethercat_driver_ros2/sphinx/developer_guide/new_plugin.rst @@ -59,13 +59,13 @@ To be loaded by the :code:`ethercat_driver_ros2`, the new module plugin needs to // configure the slave module with urdf parameters // and link ros2_control command and stat interface virtual bool setupSlave( - std::unordered_map slave_paramters, + std::unordered_map slave_parameters, std::vector * state_interface, std::vector * command_interface) { state_interface_ptr_ = state_interface; command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; + parameters_ = slave_parameters; // Your module setup logic goes here diff --git a/ethercat_driver_ros2/sphinx/developer_guide/safety.rst b/ethercat_driver_ros2/sphinx/developer_guide/safety.rst new file mode 100644 index 00000000..dce8158b --- /dev/null +++ b/ethercat_driver_ros2/sphinx/developer_guide/safety.rst @@ -0,0 +1,83 @@ +FailSafe over EtherCAT (FSoE) : Safety and the Ethercat Driver for ROS2 +======================================================================= + +In order to have safe robotic systems, it is important to have a safety layer that can monitor the system and take action in case of a failure. The FailSafe over EtherCAT (FSoE) protocol is a safety protocol that can be used to monitor the system and take action in case of a failure. +In order to use the FSoE protocol with ROS2, the Ethercat Driver for ROS2 has been extended to be compatible with EtherCAT communications using the FSoE protocol. + +This document describes the integration of FSoE communications in the Ethercat Driver for ROS2 and how to use it to monitor the system and take action in case of a failure. + +In addition to the standard EtherCAT slaves, safety is implemented by adding safety slaves and safety masters to the EtherCAT network, all being «viewed» by the EtherCAT master as standard slaves. +One safety master handles a set of safety slaves creating a safety sub-network within the EtherCAT network. +All the communications between the safety slaves and their respective safety masters are done inside the standard EtherCAT frames and must not be tampered with or an error will be raised. +The ethercat_driver_ros2 package has been extended: + +1. to be able to record safety slaves and safety masters in the ros2_control URDF configuration file +2. to extend communication frames to include safety data +3. to handle safety data transfer between safety slaves and safety masters. + +Safety module declaration +------------------------- + +There are 2 types of safety slaves or masters: + +1. the ones that expose safety data to ROS2 and will be either joints, sensors or gpios, we call them «ros2 safety» slaves or masters. +2. the ones that are not visible to ROS2, we call them «safety only» slaves or masters. + +The «ros2 safety» slaves or masters are declared in the URDF configuration file as usual inside the gpio, sensor or joint tags. + +The «safety only» slaves or masters are declared in the URDF configuration file in a new tag called «safety», inside standard «ec_module» tags. + +For each FSoE net, a «net» tag is added as a child of the «safety» tag. The «net» tag has a the following attributes: + +1. a «name» attribute, +2. a «safety_master» attribute providing the name of an «ec_module» that will act as the net safety master. + +The «net» tag has children «transfer» tags describing the safety data transfers. + +Safety data transfer specification +---------------------------------- + +A safety data transfer is a continuous memory array that has to be copied from one part of the communication frame to another part of the communication frame. The data to transfer is defined by: + + 1. a first offset in the communication frame where the data must be read + 2. a second offset in the communication frame where the data must be written to + 3. the length of the data to transfer. + +From a user perspective, the question is how to easily specify where the data is located in the communication frame for read and for write. +Our solution is to specify in the URDF configuration file the first data for read and for write by specifying: + +1. the name of the module, the data is coming from or going to +2. the channel index and sub-index of the data in the module + +The read location is defined in the the tag and the write location is defined in the tag of a tag. +The length of the data to transfer is defined the attribute «size» of the tag. + +The tag is a child of a tag inside the tag. + +Configuration of the field bus communication and ROS2 data exchanges +-------------------------------------------------------------------- + +From the user perspective modules are identified by their name, however in the EtherCAT network, modules are identified by their position (or alias) in the network. + + +on_init stage +~~~~~~~~~~~~~ + +At the «on_init» stage, the Ethercat Driver for ROS2 will: + +1. first, look for all the EtherCAT modules in the URDF configuration that are exposed to ROS2 and load the corresponding plugins and their configuration. It effectively called the :code:`addSlave` method of the :code:`EcMaster` class for each of these modules. +2. then, look for all the EtherCAT modules in the URDF that are «safety only» modules and load the corresponding plugins and their configuration. It effectively called the :code:`addSlave` method of the :code:`EcMaster` class for each of these modules. +3. Compute the map between transfers in terms of module, name, channel index and sub-index and the corresponding offsets in the communication frame. + +For each slave, in the add_slave method of the EcMaster class, the domain info are updated to prepare the ec_pdo_entry_reg_t data structures. +EcSafety is a derived class of EcMaster incorporating the safety data transfers. +So after each slave has been added, the safety data transfers are prepared such that all pointers to the offsets necessary for the safety data transfers are stored in the EcSafety class and will be ready when after the «on_activate» stage, the values pointed will contained the correct offset values. + + + +on_activate stage +~~~~~~~~~~~~~~~~~ + +At the «on_activate» stage, the Ethercat Driver for ROS2 will: + +1. Populate the ec_pdo_entry_reg_t data structures with the data with a call to the ecrt_domain_reg_pdo_entry_list function. diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp index 07ec2a04..3a79c9ee 100644 --- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp +++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/include/ethercat_generic_plugins/generic_ec_cia402_drive.hpp @@ -24,7 +24,7 @@ #include "yaml-cpp/yaml.h" #include "ethercat_interface/ec_slave.hpp" -#include "ethercat_interface/ec_pdo_channel_manager.hpp" +#include "ethercat_interface/ec_pdo_single_interface_channel_manager.hpp" #include "ethercat_generic_plugins/generic_ec_slave.hpp" #include "ethercat_generic_plugins/cia402_common_defs.hpp" @@ -40,16 +40,18 @@ class EcCiA402Drive : public GenericEcSlave * The transition through the state machine is handled automatically. */ bool initialized() const; - virtual void processData(size_t index, uint8_t * domain_address); + virtual void processData(size_t entry_idx, uint8_t * domain_address); virtual bool setupSlave( - std::unordered_map slave_paramters, + std::unordered_map slave_parameters, std::vector * state_interface, std::vector * command_interface); int8_t mode_of_operation_display_ = 0; int8_t mode_of_operation_ = -1; + void updateState(); + protected: uint32_t counter_ = 0; uint16_t last_status_word_ = -1; diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/package.xml b/ethercat_generic_plugins/ethercat_generic_cia402_drive/package.xml index 7e215d0c..d5f42352 100644 --- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/package.xml +++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/package.xml @@ -4,7 +4,7 @@ ethercat_generic_cia402_drive 1.2.0 Plugin implementations of generic EtherCAT modules for CiA402 drives. - Maciej Bednarczyk + Manuel YGUEL Apache-2.0 ament_cmake_ros @@ -20,4 +20,4 @@ ament_cmake - + \ No newline at end of file diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp index f06426db..34430fb5 100644 --- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp +++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/src/generic_ec_cia402_drive.cpp @@ -17,6 +17,7 @@ #include #include "ethercat_generic_plugins/generic_ec_cia402_drive.hpp" +#include "rclcpp/rclcpp.hpp" namespace ethercat_generic_plugins { @@ -27,10 +28,34 @@ EcCiA402Drive::~EcCiA402Drive() {} bool EcCiA402Drive::initialized() const {return initialized_;} -void EcCiA402Drive::processData(size_t index, uint8_t * domain_address) +void EcCiA402Drive::updateState() { + if (status_word_ != last_status_word_) { + state_ = deviceState(status_word_); + if (state_ != last_state_) { + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), + "STATE: %s with status word :%d", + DEVICE_STATE_STR.at(state_).c_str(), + status_word_ + ); + } + } + last_status_word_ = status_word_; + last_state_ = state_; + counter_++; +} + +void EcCiA402Drive::processData(size_t entry_idx, uint8_t * domain_address) +{ + auto index = domain_map_[entry_idx]; + ethercat_interface::EcPdoSingleInterfaceChannelManager * channel_ptr = + static_cast< + ethercat_interface::EcPdoSingleInterfaceChannelManager *>( + pdo_channels_info_[index]); + ethercat_interface::EcPdoSingleInterfaceChannelManager & channel(*channel_ptr); // Special case: ControlWord - if (pdo_channels_info_[index].index == CiA402D_RPDO_CONTROLWORD) { + if (channel.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) { @@ -46,77 +71,64 @@ void EcCiA402Drive::processData(size_t index, uint8_t * domain_address) } if (auto_state_transitions_) { - pdo_channels_info_[index].default_value = transition( + channel.default_value = transition( state_, - pdo_channels_info_[index].ec_read(domain_address)); + channel.ec_read(domain_address)); } } } // setup current position as default position - if (pdo_channels_info_[index].index == CiA402D_RPDO_POSITION) { + if (channel.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; + channel.default_value = + channel.factor * last_position_ + channel.offset; } - pdo_channels_info_[index].override_command = + channel.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 (channel.index == CiA402D_RPDO_MODE_OF_OPERATION) { if (mode_of_operation_ >= 0 && mode_of_operation_ <= 10) { - pdo_channels_info_[index].default_value = mode_of_operation_; + channel.default_value = mode_of_operation_; } } - pdo_channels_info_[index].ec_update(domain_address); + channel.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 (channel.index == CiA402D_TPDO_MODE_OF_OPERATION_DISPLAY) { + mode_of_operation_display_ = channel.last_value; } - if (pdo_channels_info_[index].index == CiA402D_TPDO_POSITION) { - last_position_ = pdo_channels_info_[index].last_value; + if (channel.index == CiA402D_TPDO_POSITION) { + last_position_ = channel.last_value; } // Special case: StatusWord - if (pdo_channels_info_[index].index == CiA402D_TPDO_STATUSWORD) { - status_word_ = pdo_channels_info_[index].last_value; + if (channel.index == CiA402D_TPDO_STATUSWORD) { + status_word_ = channel.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_++; + if (entry_idx == domain_map_.size() - 1) { // if last entry in domain + updateState(); } } bool EcCiA402Drive::setupSlave( - std::unordered_map slave_paramters, + std::unordered_map slave_parameters, std::vector * state_interface, std::vector * command_interface) { state_interface_ptr_ = state_interface; command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; + parameters_ = slave_parameters; - if (paramters_.find("slave_config") != paramters_.end()) { - if (!setup_from_config_file(paramters_["slave_config"])) { + if (parameters_.find("slave_config") != parameters_.end()) { + if (!setup_from_config_file(parameters_["slave_config"])) { return false; } } else { @@ -127,12 +139,12 @@ bool EcCiA402Drive::setupSlave( setup_interface_mapping(); setup_syncs(); - if (paramters_.find("mode_of_operation") != paramters_.end()) { - mode_of_operation_ = std::stod(paramters_["mode_of_operation"]); + if (parameters_.find("mode_of_operation") != parameters_.end()) { + mode_of_operation_ = std::stod(parameters_["mode_of_operation"]); } - if (paramters_.find("command_interface/reset_fault") != paramters_.end()) { - fault_reset_command_interface_index_ = std::stoi(paramters_["command_interface/reset_fault"]); + if (parameters_.find("command_interface/reset_fault") != parameters_.end()) { + fault_reset_command_interface_index_ = std::stoi(parameters_["command_interface/reset_fault"]); } return true; @@ -157,10 +169,16 @@ bool EcCiA402Drive::setup_from_config_file(std::string config_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; + RCLCPP_ERROR( + rclcpp::get_logger("EthercatDriver"), + "EcCiA402Drive: failed to load drive configuration: %s", + ex.what()); return false; } catch (const YAML::BadFile & ex) { - std::cerr << "EcCiA402Drive: failed to load drive configuration: " << ex.what() << std::endl; + RCLCPP_ERROR( + rclcpp::get_logger("EthercatDriver"), + "EcCiA402Drive: failed to load drive configuration: %s", + ex.what()); return false; } if (!setup_from_config(slave_config_)) { diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp b/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp index 8d2d959d..ab9990ce 100644 --- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp +++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/test/test_generic_ec_cia402_drive.cpp @@ -67,11 +67,11 @@ TEST_F(EcCiA402DriveTest, SlaveSetupNoDriveConfig) SetUp(); std::vector state_interface = {0}; std::vector command_interface = {0}; - std::unordered_map slave_paramters; + std::unordered_map slave_parameters; // setup failed, 'drive_config' parameter not set ASSERT_EQ( plugin_->setupSlave( - slave_paramters, + slave_parameters, &state_interface, &command_interface ), @@ -84,12 +84,12 @@ TEST_F(EcCiA402DriveTest, SlaveSetupMissingFileDriveConfig) SetUp(); std::vector state_interface = {0}; std::vector command_interface = {0}; - std::unordered_map slave_paramters; - slave_paramters["drive_config"] = "drive_config.yaml"; + std::unordered_map slave_parameters; + slave_parameters["drive_config"] = "drive_config.yaml"; // setup failed, 'drive_config.yaml' file not set ASSERT_EQ( plugin_->setupSlave( - slave_paramters, + slave_parameters, &state_interface, &command_interface ), @@ -116,12 +116,16 @@ TEST_F(EcCiA402DriveTest, SlaveSetupDriveFromConfig) 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"); + + auto channels = plugin_->pdo_channels_info_; + ASSERT_EQ(channels[1]->interface_name(), "velocity") << "Interface name is not 'velocity'"; + ASSERT_EQ(channels[3]->data().default_value, 1000) << "Default value is not 1000"; + ASSERT_TRUE(std::isnan(channels[0]->data().default_value)) << "Default value is not NaN"; + ASSERT_EQ(channels[4]->interface_name(), "null") << "Interface name is not 'null'"; + ASSERT_EQ( + channels[12]->interface_name(), + "analog_input2") << "Interface name is not 'analog_input2'"; + ASSERT_EQ(channels[4]->data_type(), "uint16") << "Data type is not 'uint16'"; } TEST_F(EcCiA402DriveTest, SlaveSetupPdoChannels) @@ -177,14 +181,14 @@ TEST_F(EcCiA402DriveTest, SlaveSetupDomains) TEST_F(EcCiA402DriveTest, EcReadTPDOToStateInterface) { SetUp(); - std::unordered_map slave_paramters; + std::unordered_map slave_parameters; std::vector state_interface = {0, 0}; plugin_->state_interface_ptr_ = &state_interface; - slave_paramters["state_interface/effort"] = "1"; - plugin_->paramters_ = slave_paramters; + slave_parameters["state_interface/effort"] = "1"; + plugin_->parameters_ = slave_parameters; plugin_->setup_from_config(YAML::Load(test_drive_config)); plugin_->setup_interface_mapping(); - ASSERT_EQ(plugin_->pdo_channels_info_[8].interface_index, 1); + ASSERT_EQ(plugin_->pdo_channels_info_[8]->state_interface_index(), 1); uint8_t domain_address[2]; EC_WRITE_S16(domain_address, 42); plugin_->processData(8, domain_address); @@ -194,18 +198,19 @@ TEST_F(EcCiA402DriveTest, EcReadTPDOToStateInterface) TEST_F(EcCiA402DriveTest, EcWriteRPDOFromCommandInterface) { SetUp(); - std::unordered_map slave_paramters; + std::unordered_map slave_parameters; std::vector command_interface = {0, 42}; plugin_->command_interface_ptr_ = &command_interface; - slave_paramters["command_interface/effort"] = "1"; - plugin_->paramters_ = slave_paramters; + slave_parameters["command_interface/effort"] = "1"; + plugin_->parameters_ = slave_parameters; plugin_->setup_from_config(YAML::Load(test_drive_config)); plugin_->setup_interface_mapping(); - ASSERT_EQ(plugin_->pdo_channels_info_[2].interface_index, 1); + auto channels = plugin_->pdo_channels_info_; + ASSERT_EQ(channels[2]->command_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(channels[2]->data().last_value, 42); ASSERT_EQ(EC_READ_S16(domain_address), 42); } @@ -217,13 +222,14 @@ TEST_F(EcCiA402DriveTest, EcWriteRPDODefaultValue) 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); + auto channels = plugin_->pdo_channels_info_; + ASSERT_EQ(channels[2]->data().last_value, -5); ASSERT_EQ(EC_READ_S16(domain_address), -5); } // TEST_F(EcCiA402DriveTest, FaultReset) // { -// std::unordered_map slave_paramters; +// std::unordered_map slave_parameters; // std::vector command_interface = {0, 1}; // plugin_->command_interface_ptr_ = &command_interface; // plugin_->setup_from_config(YAML::Load(test_drive_config)); @@ -251,12 +257,12 @@ TEST_F(EcCiA402DriveTest, EcWriteRPDODefaultValue) TEST_F(EcCiA402DriveTest, SwitchModeOfOperation) { - std::unordered_map slave_paramters; + std::unordered_map slave_parameters; std::vector command_interface = { std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}; - slave_paramters["command_interface/mode_of_operation"] = "1"; - plugin_->paramters_ = slave_paramters; + slave_parameters["command_interface/mode_of_operation"] = "1"; + plugin_->parameters_ = slave_parameters; plugin_->command_interface_ptr_ = &command_interface; plugin_->setup_from_config(YAML::Load(test_drive_config)); plugin_->setup_interface_mapping(); @@ -273,12 +279,12 @@ TEST_F(EcCiA402DriveTest, SwitchModeOfOperation) TEST_F(EcCiA402DriveTest, EcWriteDefaultTargetPosition) { - std::unordered_map slave_paramters; + std::unordered_map slave_parameters; std::vector command_interface = { std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}; - slave_paramters["command_interface/mode_of_operation"] = "1"; - plugin_->paramters_ = slave_paramters; + slave_parameters["command_interface/mode_of_operation"] = "1"; + plugin_->parameters_ = slave_parameters; plugin_->command_interface_ptr_ = &command_interface; plugin_->setup_from_config(YAML::Load(test_drive_config)); plugin_->setup_interface_mapping(); @@ -287,8 +293,8 @@ TEST_F(EcCiA402DriveTest, EcWriteDefaultTargetPosition) uint8_t domain_address[4]; uint8_t domain_address_moo[2]; - plugin_->processData(5, domain_address_moo); - plugin_->processData(10, domain_address_moo); + plugin_->processData(5, domain_address_moo); // mode_of_operation + plugin_->processData(10, domain_address_moo); // mode_of_operation_display ASSERT_EQ(plugin_->mode_of_operation_display_, 8); EC_WRITE_S32(domain_address, 123456); diff --git a/ethercat_generic_plugins/ethercat_generic_slave/CMakeLists.txt b/ethercat_generic_plugins/ethercat_generic_slave/CMakeLists.txt index b2050f3d..3a6b787e 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/CMakeLists.txt +++ b/ethercat_generic_plugins/ethercat_generic_slave/CMakeLists.txt @@ -13,13 +13,16 @@ 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 + +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 $ - $) -ament_target_dependencies( - ethercat_generic_slave + $ +) +ament_target_dependencies( ${PROJECT_NAME} ethercat_interface pluginlib yaml_cpp_vendor @@ -33,7 +36,7 @@ install( ) install( - TARGETS ethercat_generic_slave + TARGETS ${PROJECT_NAME} EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -62,21 +65,21 @@ if(BUILD_TESTING) 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 ) + target_include_directories(test_generic_ec_slave PRIVATE include) + target_link_libraries(test_generic_ec_slave + ${PROJECT_NAME} + ) endif() ament_export_include_directories( include ) ament_export_libraries( - ethercat_generic_slave + ${PROJECT_NAME} ) ament_export_targets( export_${PROJECT_NAME} diff --git a/ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp b/ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp index 9d733ee9..c349cc5b 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp +++ b/ethercat_generic_plugins/ethercat_generic_slave/include/ethercat_generic_plugins/generic_ec_slave.hpp @@ -36,7 +36,7 @@ class GenericEcSlave : public ethercat_interface::EcSlave virtual ~GenericEcSlave(); virtual int assign_activate_dc_sync(); - virtual void processData(size_t index, uint8_t * domain_address); + virtual void processData(size_t entry_idx, uint8_t * domain_address); virtual const ec_sync_info_t * syncs(); virtual size_t syncSize(); @@ -44,7 +44,7 @@ class GenericEcSlave : public ethercat_interface::EcSlave virtual void domains(DomainMap & domains) const; virtual bool setupSlave( - std::unordered_map slave_paramters, + std::unordered_map slave_parameters, std::vector * state_interface, std::vector * command_interface); @@ -52,8 +52,9 @@ class GenericEcSlave : public ethercat_interface::EcSlave uint32_t counter_ = 0; std::vector rpdos_; std::vector tpdos_; + std::vector all_channels_skip_list_; std::vector all_channels_; - std::vector pdo_channels_info_; + std::vector pdo_channels_info_; std::vector sm_configs_; std::vector syncs_; std::vector domain_map_; diff --git a/ethercat_generic_plugins/ethercat_generic_slave/package.xml b/ethercat_generic_plugins/ethercat_generic_slave/package.xml index 8655060a..ab4bf929 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/package.xml +++ b/ethercat_generic_plugins/ethercat_generic_slave/package.xml @@ -19,4 +19,4 @@ ament_cmake - + \ No newline at end of file diff --git a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp index 7514cf01..8ff956cf 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp +++ b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp @@ -17,32 +17,25 @@ #include #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; - } -} +#include "ethercat_interface/ec_pdo_single_interface_channel_manager.hpp" +#include "ethercat_interface/ec_pdo_group_interface_channel_manager.hpp" namespace ethercat_generic_plugins { GenericEcSlave::GenericEcSlave() : EcSlave(0, 0) {} -GenericEcSlave::~GenericEcSlave() {} +GenericEcSlave::~GenericEcSlave() +{ + for (size_t c = 0; c < pdo_channels_info_.size(); c++) { + delete pdo_channels_info_[c]; + } +} int GenericEcSlave::assign_activate_dc_sync() {return assign_activate_;} -void GenericEcSlave::processData(size_t index, uint8_t * domain_address) +void GenericEcSlave::processData(size_t entry_idx, uint8_t * domain_address) { - pdo_channels_info_[domain_map_[index]].ec_update(domain_address); + pdo_channels_info_[domain_map_[entry_idx]]->ec_update(domain_address); } const ec_sync_info_t * GenericEcSlave::syncs() @@ -67,33 +60,41 @@ 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}); + syncs_.push_back( + {2, EC_DIR_OUTPUT, (unsigned int)(rpdos_.size()), rpdos_.data(), + EC_WD_ENABLE}); + syncs_.push_back( + {3, EC_DIR_INPUT, (unsigned int)(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}); + syncs_.push_back( + {sm.index, sm.type, (unsigned int)(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( + {sm.index, sm.type, (unsigned int)(tpdos_.size()), + tpdos_.data(), sm.watchdog}); } } } - syncs_.push_back({0xff}); + syncs_.push_back({0xff, EC_DIR_INVALID, 0, nullptr, EC_WD_DISABLE}); } bool GenericEcSlave::setupSlave( - std::unordered_map slave_paramters, + std::unordered_map slave_parameters, std::vector * state_interface, std::vector * command_interface) { state_interface_ptr_ = state_interface; command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; + parameters_ = slave_parameters; - if (paramters_.find("slave_config") != paramters_.end()) { - if (!setup_from_config_file(paramters_["slave_config"])) { + if (parameters_.find("slave_config") != parameters_.end()) { + if (!setup_from_config_file(parameters_["slave_config"])) { return false; } } else { @@ -158,22 +159,31 @@ bool GenericEcSlave::setup_from_config(YAML::Node slave_config) } all_channels_.reserve(channels_nbr); + all_channels_skip_list_.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]); + ethercat_interface::EcPdoChannelManager * channel_info = nullptr; + // Check if the channel is a special data area holding several in memory data + if (slave_config["rpdo"][i]["channels"][c]["data_mapping"]) { + channel_info = new ethercat_interface::EcPdoGroupInterfaceChannelManager; + } else { + channel_info = new ethercat_interface::EcPdoSingleInterfaceChannelManager; + } + + 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()); + all_channels_.push_back(channel_info->get_pdo_entry_info()); + all_channels_skip_list_.push_back(channel_info->skip); } rpdos_.push_back( { slave_config["rpdo"][i]["index"].as(), - rpdo_channels_size, + (unsigned int)(rpdo_channels_size), all_channels_.data() + channels_nbr } ); @@ -186,16 +196,23 @@ bool GenericEcSlave::setup_from_config(YAML::Node slave_config) 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]); + ethercat_interface::EcPdoChannelManager * channel_info = nullptr; + // Check if the channel is a special data area holding several in memory data + if (slave_config["tpdo"][i]["channels"][c]["data_mapping"]) { + channel_info = new ethercat_interface::EcPdoGroupInterfaceChannelManager; + } else { + channel_info = new ethercat_interface::EcPdoSingleInterfaceChannelManager; + } + 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()); + all_channels_.push_back(channel_info->get_pdo_entry_info()); + all_channels_skip_list_.push_back(channel_info->skip); } tpdos_.push_back( { slave_config["tpdo"][i]["index"].as(), - tpdo_channels_size, + (unsigned int)(tpdo_channels_size), all_channels_.data() + channels_nbr } ); @@ -205,7 +222,7 @@ bool GenericEcSlave::setup_from_config(YAML::Node slave_config) // Remove gaps from domain mapping for (auto i = 0ul; i < all_channels_.size(); i++) { - if (all_channels_[i].index != 0x0000) { + if (all_channels_[i].index != 0x0000 && all_channels_skip_list_[i] != true) { domain_map_.push_back(i); } } @@ -224,10 +241,15 @@ bool GenericEcSlave::setup_from_config_file(std::string config_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; + std::cerr << + "GenericEcSlave: failed to load EtherCAT module configuration " + "(YAML file is incorrect): " << ex.what() << std::endl; return false; } catch (const YAML::BadFile & ex) { - std::cerr << "GenericEcSlave: failed to load drive configuration: " << ex.what() << std::endl; + std::cerr << + "GenericEcSlave: failed to load EtherCAT module configuration " + "(file path is incorrect or file is damaged): " << ex.what() + << std::endl; return false; } if (!setup_from_config(slave_config_)) { @@ -238,17 +260,31 @@ bool GenericEcSlave::setup_from_config_file(std::string config_file) 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]); + for (auto & channel_ptr : pdo_channels_info_) { + auto & channel = *channel_ptr; + for (size_t i = 0; i < channel.number_of_interfaces(); ++i) { + if (channel.has_state_interface_name(i) ) { + std::string interface = "state_interface/" + channel.interface_name(i); + if (parameters_.find(interface) != parameters_.end()) { + const size_t idx = std::stoi(parameters_[interface]); + channel.set_state_interface_index(channel.interface_name(i), idx); + } + } else if (channel.has_command_interface_name(i) ) { + std::string interface = "command_interface/" + channel.interface_name(i); + if (channel.pdo_type == ethercat_interface::RPDO) { + std::string interface = "command_interface/" + channel.interface_name(i); + if (parameters_.find(interface) != parameters_.end()) { + const size_t idx = std::stoi(parameters_[interface]); + channel.set_command_interface_index(channel.interface_name(i), idx); + } + } else { + throw std::runtime_error( + std::string("GenericEcSlave: command interface (") + + "index: " + channel.index_hex_str() + ", " + + "sub_index: " + channel.sub_index_hex_str() + ", " + + "name: " + interface + + ") is not allowed for TPDO channels"); + } } } diff --git a/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.cpp b/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.cpp index d16c3527..0508de5d 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.cpp +++ b/ethercat_generic_plugins/ethercat_generic_slave/test/test_generic_ec_slave.cpp @@ -71,11 +71,11 @@ TEST_F(GenericEcSlaveTest, SlaveSetupNoSlaveConfig) SetUp(); std::vector state_interface = {0}; std::vector command_interface = {0}; - std::unordered_map slave_paramters; + std::unordered_map slave_parameters; // setup failed, 'slave_config' parameter not set ASSERT_EQ( plugin_->setupSlave( - slave_paramters, + slave_parameters, &state_interface, &command_interface ), @@ -88,12 +88,12 @@ TEST_F(GenericEcSlaveTest, SlaveSetupMissingFileSlaveConfig) SetUp(); std::vector state_interface = {0}; std::vector command_interface = {0}; - std::unordered_map slave_paramters; - slave_paramters["slave_config"] = "slave_config.yaml"; + std::unordered_map slave_parameters; + slave_parameters["slave_config"] = "slave_config.yaml"; // setup failed, 'slave_config.yaml' file not set ASSERT_EQ( plugin_->setupSlave( - slave_paramters, + slave_parameters, &state_interface, &command_interface ), @@ -119,14 +119,15 @@ TEST_F(GenericEcSlaveTest, SlaveSetupSlaveFromConfig) 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"); + auto channels = plugin_->pdo_channels_info_; + ASSERT_EQ(channels[1]->interface_name(), "velocity"); + ASSERT_EQ(channels[2]->data().factor, 2); + ASSERT_EQ(channels[2]->data().offset, 10); + ASSERT_EQ(channels[3]->data().default_value, 1000); + ASSERT_TRUE(std::isnan(channels[0]->data().default_value)); + ASSERT_EQ(channels[4]->interface_name(), "null"); + ASSERT_EQ(channels[12]->interface_name(), "analog_input2"); + ASSERT_EQ(channels[4]->data_type(), "uint16"); } TEST_F(GenericEcSlaveTest, SlaveSetupPdoChannels) @@ -182,14 +183,14 @@ TEST_F(GenericEcSlaveTest, SlaveSetupDomains) TEST_F(GenericEcSlaveTest, EcReadTPDOToStateInterface) { SetUp(); - std::unordered_map slave_paramters; + std::unordered_map slave_parameters; std::vector state_interface = {0, 0}; plugin_->state_interface_ptr_ = &state_interface; - slave_paramters["state_interface/effort"] = "1"; - plugin_->paramters_ = slave_paramters; + slave_parameters["state_interface/effort"] = "1"; + plugin_->parameters_ = slave_parameters; plugin_->setup_from_config(YAML::Load(test_slave_config)); plugin_->setup_interface_mapping(); - ASSERT_EQ(plugin_->pdo_channels_info_[8].interface_index, 1); + ASSERT_EQ(plugin_->pdo_channels_info_[8]->state_interface_index(), 1); uint8_t domain_address[2]; EC_WRITE_S16(domain_address, 42); plugin_->processData(8, domain_address); @@ -199,17 +200,18 @@ TEST_F(GenericEcSlaveTest, EcReadTPDOToStateInterface) TEST_F(GenericEcSlaveTest, EcWriteRPDOFromCommandInterface) { SetUp(); - std::unordered_map slave_paramters; + std::unordered_map slave_parameters; std::vector command_interface = {0, 42}; plugin_->command_interface_ptr_ = &command_interface; - slave_paramters["command_interface/effort"] = "1"; - plugin_->paramters_ = slave_paramters; + slave_parameters["command_interface/effort"] = "1"; + plugin_->parameters_ = slave_parameters; plugin_->setup_from_config(YAML::Load(test_slave_config)); plugin_->setup_interface_mapping(); - ASSERT_EQ(plugin_->pdo_channels_info_[2].interface_index, 1); + auto channels = plugin_->pdo_channels_info_; + ASSERT_EQ(channels[2]->command_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(channels[2]->data().last_value, 2 * 42 + 10); ASSERT_EQ(EC_READ_S16(domain_address), 2 * 42 + 10); } @@ -220,7 +222,7 @@ TEST_F(GenericEcSlaveTest, EcWriteRPDODefaultValue) 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(plugin_->pdo_channels_info_[2]->data().last_value, -5); ASSERT_EQ(EC_READ_S16(domain_address), -5); } diff --git a/ethercat_interface/CMakeLists.txt b/ethercat_interface/CMakeLists.txt index 3843fe6e..bab7a7d4 100644 --- a/ethercat_interface/CMakeLists.txt +++ b/ethercat_interface/CMakeLists.txt @@ -24,7 +24,11 @@ ament_export_include_directories( add_library( ${PROJECT_NAME} SHARED - src/ec_master.cpp) + src/ec_master.cpp + src/ec_pdo_channel_manager.cpp + src/ec_pdo_single_interface_channel_manager.cpp + src/ec_pdo_group_interface_channel_manager.cpp +) target_include_directories( ${PROJECT_NAME} @@ -65,6 +69,10 @@ if(BUILD_TESTING) ament_target_dependencies(test_ec_pdo_channel_manager yaml_cpp_vendor ) + + target_link_libraries(test_ec_pdo_channel_manager + ${PROJECT_NAME} + ) endif() ## EXPORTS diff --git a/ethercat_interface/include/ethercat_interface/ec_master.hpp b/ethercat_interface/include/ethercat_interface/ec_master.hpp index 26ae64f0..7fcd1c1a 100644 --- a/ethercat_interface/include/ethercat_interface/ec_master.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_master.hpp @@ -22,16 +22,105 @@ #include #include #include +#include #include "ethercat_interface/ec_slave.hpp" +#include "ethercat_interface/ec_transfer.hpp" +#include "rclcpp/rclcpp.hpp" namespace ethercat_interface { +inline uint64_t EC_NEWTIMEVAL2NANO(struct timespec & TV) +{ + return (TV.tv_sec - 946684800ULL) * 1000000000ULL + TV.tv_nsec; +} + +class EcMemoryEntry +{ +public: + std::string module_name; //< Module. + uint16_t alias; //< Slave alias. + uint16_t position; //< Slave position. + uint16_t index; //< Channel index. + uint16_t subindex; //< Channel subindex. + +public: + inline + std::string to_simple_string() const + { + return "( name= " + module_name + ", index= " + + std::to_string(index) + ", subindex= " + std::to_string(subindex) + " )"; + } +}; + +class EcTransferEntry +{ +public: + EcMemoryEntry input; + EcMemoryEntry output; + size_t size; //< Size of the exchange data. + +public: + inline + std::string to_simple_string() const + { + return input.to_simple_string() + " -> " + output.to_simple_string() + " / ( size= " + + std::to_string(size) + " )"; + } +}; + +class EcTransferNet +{ +public: + std::string name; //< transfer net name (e.g. a safety net) + std::string master; //< transfer master (e.g. the safety master of the net) + std::vector transfers; //< Data transfers (e.g. safety data transfers) + +public: + void reset(const std::string & new_name) + { + name = new_name; + master = ""; + transfers.clear(); + } +}; + +// Forward declarations +class EcSlave; + +/** 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; //< pointer to process domain data + + /** domain pdo registration array. + * do not modify after active(), or may invalidate */ + std::vector domain_regs; + + /** slave's pdo and in memory data entries in the domain */ + struct Entry + { + EcSlave * slave = NULL; + int num_pdos = 0; + uint32_t * offset = NULL; + uint32_t * bit_position = NULL; + int num_in_memory_data = 0; + uint32_t * offset_in_memory = NULL; + }; + + std::vector entries; +}; + class EcMaster { public: - explicit EcMaster(const int master = 0); + explicit EcMaster(const unsigned int master = 0); virtual ~EcMaster(); /** \brief add a slave device to the master @@ -41,6 +130,12 @@ class EcMaster */ void addSlave(uint16_t alias, uint16_t position, EcSlave * slave); + /** \brief add a slave device to the master + * alias and position should have been set + * before calling this function. + */ + void addSlave(EcSlave * slave); + /** \brief configure slave using SDO */ int configSlaveSdo(uint16_t slave_position, SdoConfigEntry sdo_config, uint32_t * abort_code); @@ -87,10 +182,55 @@ class EcMaster uint32_t getInterval() {return interval_;} - void readData(uint32_t domain = 0); - void writeData(uint32_t domain = 0); - -private: + virtual void readData(uint32_t domain = 0); + virtual void writeData(uint32_t domain = 0); + + /** @brief Fill in the EcTransferInfo structures + * + * @param transfer_nets transfer nets + * + * \pre DomainInfo and domain_regs vectors must have been initialized and + * activated. A call to EcMaster::activate() is required before calling + * this function, to fill in the domain_regs vector offsets. Specifically + * with IgH EtherCAT Master, the offset must have been initialized with the + * ecrt_domain_reg_pdo_entry_list function. + * + * @throw std::runtime_error if some domain_info or some pdo_entry_reg are + * not valid + */ + void registerTransferInDomain(const std::vector & transfer_nets); + + /** @brief Proceed to the transfer of all the data declared in transfers_. + */ + void transferAll(); + +protected: + /** @brief Output the memory content of the all the domains + * (available for pedagogic and debug purposes) + * + * @param[out] os Output stream + */ + void printMemoryFrames(std::ostream & os); + + /** @brief Get pointer on memory frame for a certain point + * in the frame defined by a slave position, an index and a subindex + */ + uint8_t * getMemoryStart( + const uint16_t position, + const uint16_t index, + const uint16_t subindex); + + /** @brief Output memory n bytes of memory from a certain point in + * the frame defined by a slave position, an index, subindex */ + void printMemoryFrame( + const uint16_t position, + const uint16_t index, + const uint16_t subindex, + const size_t n, + bool binary = false, + std::ostream & os = std::cout); + +protected: /** true if running */ volatile bool running_ = false; @@ -100,9 +240,7 @@ class EcMaster // EtherCAT Control /** register a domain of the slave */ - struct DomainInfo; void registerPDOInDomain( - uint16_t alias, uint16_t position, std::vector & channel_indices, DomainInfo * domain_info, EcSlave * slave); @@ -117,38 +255,29 @@ class EcMaster void checkSlaveStates(); /** print warning message to terminal */ - static void printWarning(const std::string & message); + inline + static void printWarning(const std::string & message) + { + RCLCPP_WARN(rclcpp::get_logger("EthercatDriver"), "WARNING. Master. %s", message.c_str()); + } + + + /** @brief Check the validity of the domain info and the ec_pdo_entry_reg_t + * and throw an exception if not valid. + * + * @param domain_info Domain info + * @param pdo_entry_reg PDO entry registration + * + * @throw std::runtime_error if domain_info or pdo_entry_reg is not valid + */ + void checkDomainInfoValidity( + const DomainInfo & domain_info, + const ec_pdo_entry_reg_t & pdo_entry_reg); /** 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 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 entries; - }; - /** map from domain index to domain info */ std::map domain_info_; @@ -157,7 +286,7 @@ class EcMaster { EcSlave * slave = NULL; ec_slave_config_t * config = NULL; - ec_slave_config_state_t config_state = {0}; + ec_slave_config_state_t config_state = {0, 0, 0}; }; std::vector slave_info_; @@ -170,6 +299,13 @@ class EcMaster uint32_t check_state_frequency_ = 10; uint32_t interval_; + + /** Data transfers (necessary for transfer communication) */ + std::vector transfers_; + +protected: + friend struct DomainInfo; + friend struct EcTransferInfo; }; } // namespace ethercat_interface diff --git a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp index f6981d93..977016cf 100644 --- a/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_pdo_channel_manager.hpp @@ -1,4 +1,4 @@ -// Copyright 2023 ICUBE Laboratory, University of Strasbourg +// Copyright 2023-2024 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. @@ -13,222 +13,237 @@ // limitations under the License. // // Author: Maciej Bednarczyk (macbednarczyk@gmail.com) +// Author: Manuel YGUEL (yguel.robotics@gmail.com) + #ifndef ETHERCAT_INTERFACE__EC_PDO_CHANNEL_MANAGER_HPP_ #define ETHERCAT_INTERFACE__EC_PDO_CHANNEL_MANAGER_HPP_ +#include #include + #include #include #include - -#include "yaml-cpp/yaml.h" +#include +#include +#include namespace ethercat_interface { + enum PdoType { - RPDO = 0, - TPDO = 1 + RPDO = 0, // < Receive PDO, Master out to Slave in (MoSi) + TPDO = 1 // < Transmit PDO, Master in from Slave out (MiSo) }; +/** @brief Global table that stores all the names of the known types */ +extern const std::vector ec_pdo_channel_data_types; + +/** @brief Global table that stores all the number of bits of the known types */ +extern const std::vector ec_pdo_channel_data_bits; + +/** @brief Returns the index of a data type in the table of types */ +size_t typeIdx(const std::string & type); + +/** @brief Returns the number of bits associated with a data type */ +uint8_t type2bits(const std::string & type); + +/** @brief Returns the type name */ +std::string id_and_bits_to_type(size_t i, uint8_t bits); + +/** @brief Check if the definition of a type is correct + * @param[in] type the name of the type to check + * @param[in] mask the mask associated with the type to check the compatibility + * @return true if the type is correct, false otherwise + */ +bool check_type(const std::string & type, uint8_t mask); + +/** @brief Global table that stores all the names of the recorded state interfaces */ +extern std::vector all_state_interface_names; + +/** @brief Global table that stores all the names of the recorded command interfaces */ +extern std::vector all_command_interface_names; + +/** @brief The type of the functions used to read from EtherCAT frame and return a double + * to be compatible with a ROS2 control state interface */ +typedef double (* SingleReadFunctionType)(uint8_t * domain_address, uint8_t data_mask); + +/** @brief Global table that stores each read function associated with a data type */ +extern const SingleReadFunctionType ec_pdo_single_read_functions[]; + +/** @brief The type of the functions used to write to EtherCAT frame from a double + * to be compatible with a ROS2 control command interface */ +typedef void (* SingleWriteFunctionType)(uint8_t * domain_address, double value, uint8_t data_mask); + +/** @brief Global table that stores each write function associated with a data type */ +extern const SingleWriteFunctionType ec_pdo_single_write_functions[]; + +struct InterfaceData +{ + bool override_command = false; + uint8_t mask = 255; + double default_value = std::numeric_limits::quiet_NaN(); + /** last_value stores either: + * - the last read value modified by mask, factor and offset + * - the last written value modified by mask, factor and offset + * */ + double last_value = std::numeric_limits::quiet_NaN(); + double factor = 1; + double offset = 0; +}; + + +/** + * @brief Virtual class for managing a single PDO channel + * @details The EcPdoChannelManager class is used to manage a single PDO + * channel. It is used to read and write data to the channel, and to update + * the ROS2 control interfaces (state and command). + * The PDO can correspond to a single interface, or to a group of interfaces. + * Hence the specializations of this class to manage these two cases. + * The most common case is the single interface case, where the PDO corresponds + * to a single interface. It is less common to have a group of interfaces, + * but it is still possible and useful (see the example below). + * @example Single interface example: a joint position PDO. + * The channel manager will read the joint position from the PDO and write the + * joint position to the PDO. The joint position will be updated in the state + * interface, and the joint position will be read from the command interface. + * The data in the channel may be an integer corresponding to coder ticks. + * The coder ticks will be converted to radians and stored in the state + * interface. The joint position in radians will be read from the command + * interface and converted to coder ticks and written to the PDO. + * @example Group of interfaces example: a set of states can be encoded in an + * octet, each bit corresponding to a state. It is particularly useful from a + * coding perspective to have one interface per state. The channel manager will + * read the octet from the PDO and update a group of state interfaces + * corresponding to each bit in the octet related to a state. The channel + * manager will also read the group of command interfaces and write the single + * octet to the PDO that encodes all the states. + * @note More generally the EcGroupInterfacePdoChannelManager class is used + * when some specific data encoding is used to represent a group of + * interfaces in the EtherCAT memory frame but associated to a single PDO. + */ class EcPdoChannelManager { public: - EcPdoChannelManager() {} - ~EcPdoChannelManager() {} + EcPdoChannelManager(); + EcPdoChannelManager(const EcPdoChannelManager &) = delete; + virtual ~EcPdoChannelManager() = 0; + +public: +//======================= +/** @name Setup methods + * @brief Methods to setup the PDO channel manager + * @{ + */ + + /** @brief Record the pointers to the state and command interfaces */ void setup_interface_ptrs( std::vector * state_interface, - std::vector * command_interface) - { - command_interface_ptr_ = command_interface; - state_interface_ptr_ = state_interface; - } + std::vector * command_interface); - ec_pdo_entry_info_t get_pdo_entry_info() {return {index, sub_index, type2bits(data_type)};} + /** @brief Load the channel configuration from a YAML node */ + virtual bool load_from_config(YAML::Node channel_config) = 0; - double ec_read(uint8_t * domain_address) - { - if (data_type == "uint8") { - last_value = static_cast(EC_READ_U8(domain_address)); - } else if (data_type == "int8") { - last_value = static_cast(EC_READ_S8(domain_address)); - } else if (data_type == "uint16") { - last_value = static_cast(EC_READ_U16(domain_address)); - } else if (data_type == "int16") { - last_value = static_cast(EC_READ_S16(domain_address)); - } else if (data_type == "uint32") { - last_value = static_cast(EC_READ_U32(domain_address)); - } else if (data_type == "int32") { - last_value = static_cast(EC_READ_S32(domain_address)); - } else if (data_type == "uint64") { - last_value = static_cast(EC_READ_U64(domain_address)); - } else if (data_type == "int64") { - last_value = static_cast(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(EC_READ_U8(domain_address) & data_mask); - } - last_value = factor * last_value + offset; - return last_value; - } +/** @} */ // < end of Setup methods +//======================= - void ec_write(uint8_t * domain_address, double value) - { - if (data_type == "uint8") { - EC_WRITE_U8(domain_address, static_cast(value)); - } else if (data_type == "int8") { - EC_WRITE_S8(domain_address, static_cast(value)); - } else if (data_type == "uint16") { - EC_WRITE_U16(domain_address, static_cast(value)); - } else if (data_type == "int16") { - EC_WRITE_S16(domain_address, static_cast(value)); - } else if (data_type == "uint32") { - EC_WRITE_U32(domain_address, static_cast(value)); - } else if (data_type == "int32") { - EC_WRITE_S32(domain_address, static_cast(value)); - } else if (data_type == "uint64") { - EC_WRITE_U64(domain_address, static_cast(value)); - } else if (data_type == "int64") { - EC_WRITE_S64(domain_address, static_cast(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(value) & data_mask); - } - EC_WRITE_U8(domain_address, buffer_); - } - last_value = value; - } +public: +//======================= +/** @name Data exchange methods + * @brief Methods to read and write data from/to the PDO + * @{ + */ + virtual double ec_read(uint8_t * domain_address, size_t i = 0) = 0; + + /// @brief Perform an ec_read and update the state interface + virtual void ec_read_to_interface(uint8_t * domain_address) = 0; + + virtual void ec_write(uint8_t * domain_address, double value, size_t i = 0) = 0; + + /// @brief Perform an ec_write and update the command interface + virtual void ec_write_from_interface(uint8_t * domain_address) = 0; + + /// @brief Update the state and command interfaces + virtual void ec_update(uint8_t * domain_address); + +/** @} */ // < end of Data exchange methods +//======================= + +public: + /** @brief Get the PDO entry info as it should be recorded by the master*/ + ec_pdo_entry_info_t get_pdo_entry_info(); + +public: + PdoType pdo_type; + uint16_t index; + uint8_t sub_index; - void ec_update(uint8_t * domain_address) + inline + std::string index_hex_str() const { - // 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); - } - } - } + std::stringstream ss; + ss << "0x" << std::hex << index; + return ss.str(); } - bool load_from_config(YAML::Node channel_config) + inline + std::string sub_index_hex_str() const { - // index - if (channel_config["index"]) { - index = channel_config["index"].as(); - } else { - std::cerr << "missing channel index info" << std::endl; - } - // sub_index - if (channel_config["sub_index"]) { - sub_index = channel_config["sub_index"].as(); - } else { - std::cerr << "channel " << index << ": missing channel info" << std::endl; - } - // data type - if (channel_config["type"]) { - data_type = channel_config["type"].as(); - } 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(); - } - // default value - if (channel_config["default"]) { - default_value = channel_config["default"].as(); - } - - } else if (pdo_type == TPDO) { - // interface name - if (channel_config["state_interface"]) { - interface_name = channel_config["state_interface"].as(); - } - } - - // factor - if (channel_config["factor"]) { - factor = channel_config["factor"].as(); - } - // offset - if (channel_config["offset"]) { - offset = channel_config["offset"].as(); - } - // mask - if (channel_config["mask"]) { - data_mask = channel_config["mask"].as(); - } - - return true; + std::stringstream ss; + ss << "0x" << std::hex << sub_index; + return ss.str(); } - uint8_t type2bits(std::string type) + /** @brief Get the number of bits in the PDO */ + inline uint8_t pdo_bits() const {return bits_;} + + /** @brief Get the string describing the data type of the PDO */ + inline + std::string pdo_data_type() const { - 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(std::stoi(n_bits)); - } - return -1; + return id_and_bits_to_type(data_type_idx_, bits_); } - 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::quiet_NaN(); - int interface_index = -1; - double last_value = std::numeric_limits::quiet_NaN(); - bool allow_ec_write = true; - bool override_command = false; - double factor = 1; - double offset = 0; + /** @brief Get the string describing the type of the data associated with interface i + * @param i The index of the interface in the group of interfaces managed by the PDO channel. In case of a single interface, i must be 0. + */ + virtual std::string data_type(size_t i = 0) const = 0; + + /** @brief Get the data */ + virtual InterfaceData & data(size_t i = 0) = 0; -private: + /** @brief Get the data */ + virtual const InterfaceData & data(size_t i = 0) const = 0; + + bool allow_ec_write = true; // < Is the PDO channel writable ? + +public: + bool skip = false; // < Skip the PDO channel in ? TODO(@yguel@unistra.fr) + +public: + virtual size_t number_of_interfaces() const = 0; + virtual void setup_managed_interfaces() = 0; + virtual size_t number_of_managed_interfaces() const = 0; + virtual std::string interface_name(size_t i = 0) const = 0; + virtual std::pair is_interface_managed(std::string interface_name) const = 0; + virtual void set_state_interface_index(const std::string & interface_name, size_t index) = 0; + virtual void set_command_interface_index(const std::string & interface_name, size_t index) = 0; + virtual size_t state_interface_index(size_t i = 0) const = 0; + virtual size_t command_interface_index(size_t i = 0) const = 0; + virtual bool has_interface_name(size_t i = 0) const = 0; + virtual bool has_state_interface_name(size_t i = 0) const = 0; + virtual bool has_command_interface_name(size_t i = 0) const = 0; + +protected: + uint8_t bits_; // < Number of bits declared in the PDO + uint8_t data_type_idx_; // < Index to the table of types to infer the data type + +protected: std::vector * command_interface_ptr_; std::vector * 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 +} // < namespace ethercat_interface #endif // ETHERCAT_INTERFACE__EC_PDO_CHANNEL_MANAGER_HPP_ diff --git a/ethercat_interface/include/ethercat_interface/ec_pdo_group_interface_channel_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_pdo_group_interface_channel_manager.hpp new file mode 100644 index 00000000..f34ee114 --- /dev/null +++ b/ethercat_interface/include/ethercat_interface/ec_pdo_group_interface_channel_manager.hpp @@ -0,0 +1,278 @@ +// Copyright 2024 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: Manuel YGUEL (yguel.robotics@gmail.com) + +#ifndef ETHERCAT_INTERFACE__EC_PDO_GROUP_INTERFACE_CHANNEL_MANAGER_HPP_ +#define ETHERCAT_INTERFACE__EC_PDO_GROUP_INTERFACE_CHANNEL_MANAGER_HPP_ + +#include // for std::pair +#include +#include +#include +#include "ethercat_interface/ec_pdo_channel_manager.hpp" + +namespace ethercat_interface +{ + +struct InterfaceDataWithAddrOffset : public InterfaceData +{ + InterfaceDataWithAddrOffset() + : InterfaceData(), + addr_offset(0) {} + + explicit InterfaceDataWithAddrOffset(const InterfaceData & data) + : InterfaceData(data), + addr_offset(0) {} + + InterfaceDataWithAddrOffset(const InterfaceData & data, size_t addr_offset) + : InterfaceData(data), + addr_offset(addr_offset) {} + + size_t addr_offset = 0; + uint8_t bits = 0; + uint8_t data_type_idx = 0; +}; + +/** + * @brief Class to manage a PDO channel corresponding to a group of interfaces + */ +class EcPdoGroupInterfaceChannelManager : public EcPdoChannelManager +{ +public: + EcPdoGroupInterfaceChannelManager(); + EcPdoGroupInterfaceChannelManager(const EcPdoGroupInterfaceChannelManager &) = delete; + ~EcPdoGroupInterfaceChannelManager(); + +public: +//======================= +/** @name Setup methods + * @brief Methods to setup the PDO channel manager + * @{ + */ + +/// @brief Load the channel configuration from a YAML node + bool load_from_config(YAML::Node channel_config); + +/** @} */ // end of Setup methods +//======================= + +public: +//======================= +/** @name Data exchange methods + * @brief Methods to read and write data from/to the PDO + * @{ + */ + + /** @brief Read the data from the PDO applying data mask, factor and offset + * @param i The index of the interface to read + * @param domain_address The pdo start address in the domain to read from, the read address will be domain_address + v_data[i].addr_offset + * @returns The value read from the domain + */ + double ec_read(uint8_t * domain_address, size_t i = 0); + + /// @brief Perform an ec_read and update the state interface + void ec_read_to_interface(uint8_t * domain_address); + + /** @brief Write the value to the PDO applying data mask, factor and offset + * @param i The index of the interface to write + * @param domain_address The pdo start address in the domain to write into, the write address will be domain_address + v_data[i].addr_offset + * @param value The value to write + */ + void ec_write(uint8_t * domain_address, double value, size_t i = 0); + + /// @brief Perform an ec_write and update the command interface + void ec_write_from_interface(uint8_t * domain_address); + +/** @} */ // < end of Data exchange methods +//======================= + +public: + std::vector v_data; + +public: + inline + size_t number_of_interfaces() const + { + return v_data.size(); + } + + inline + size_t number_of_managed_interfaces() const + { + return managed_.size(); + } + + void setup_managed_interfaces(); + + std::string interface_name(size_t i = 0) const; + + std::pair is_interface_managed(std::string name) const; + + size_t channel_state_interface_index(const std::string & name) const; + + size_t channel_command_interface_index(const std::string & name) const; + + std::string data_type(size_t i = 0) const; + + size_t state_interface_index(size_t i = 0) const; + size_t command_interface_index(size_t i = 0) const; + +public: + inline + void set_state_interface_index(const std::string & interface_name, size_t index) + { + size_t i = channel_state_interface_index(interface_name); + interface_ids_.at(i) = index; + } + + inline + void set_command_interface_index(const std::string & interface_name, size_t index) + { + size_t i = channel_command_interface_index(interface_name); + interface_ids_.at(i) = index; + } + + inline + bool is_interface_defined(size_t i) const + { + return std::numeric_limits::max() != interface_ids_.at(i); + } + + inline + bool is_state_interface_defined(size_t i) const + { + return is_interface_defined(i) && !is_command_interface_.at(i); + } + + inline + bool is_command_interface_defined(size_t i) const + { + return is_interface_defined(i) && is_command_interface_.at(i); + } + + inline + bool has_interface_name(size_t i = 0) const + { + return 0 != interface_name_ids_.at(i); + } + + inline bool has_state_interface_name(size_t i = 0) const + { + return has_interface_name(i) && !is_command_interface_.at(i); + } + + inline bool has_command_interface_name(size_t i = 0) const + { + return has_interface_name(i) && is_command_interface_.at(i); + } + +public: + inline + InterfaceData & data(size_t i = 0) + { + return v_data.at(i); + } + + inline + const InterfaceData & data(size_t i = 0) const + { + return v_data.at(i); + } + +protected: +/** @brief Create the necessary allocations to add a new interface */ + void allocate_for_new_interface(); + +/** @brief Add a state interface named name + * @details If the interface is already present, the function does nothing and + * returns the index of the interface, otherwise it adds the interface and + * returns its index + * + * @param[in] name the name of the interface to add + * @returns the index for the added interface in all the vectors + * (interface_ids_, interface_name_ids_, read_functions_, data, + * write_functions_) + * + */ + size_t add_state_interface(const std::string & name); + +/** @brief Add a data without interface + * @details The function adds a data without interface and returns the index + * of the data in all the vectors (interface_ids_, interface_name_ids_, + * read_functions_, data, write_functions_) + * @returns the index for the added data in all the vectors + */ + size_t add_data_without_interface(); + +/** @brief Add a command interface named name + * @details If the interface is already present, the function does nothing and + * returns the index of the interface, otherwise it adds the interface and + * returns its index + * + * @param[in] name the name of the interface to add + * @returns the index for the added interface in all the vectors + * (command_interface_ids_, command_interface_name_ids_, write_functions_, data + * but also state_interface_ids_, state_interface_name_ids_, + * read_functions_) + * + */ + size_t add_command_interface(const std::string & name); + +protected: + /** @brief Indices of the state/or command interfaces in the ros2 control + * state interface or command interface vector + * @details If the index is not set, the value is std::numeric_limits::max() + * To know which type of interface vector the index refers to, it is necessary to + * look into the is_command_interface_ vector. + */ + std::vector interface_ids_; + + /** @brief Store a boolean indicating if the interface is a command interface + * or a state interface + */ + std::vector is_command_interface_; + + /** @brief Stores the function used to read the data from the EtherCAT + * frame for each interface */ + std::vector read_functions_; + + /** @brief Stores the function used to write the data to the EtherCAT + * frame for each interface */ + std::vector write_functions_; + + /** @brief Stores the index to the name of the interfaces + * @details The index of the name of the interfaces is stored in the same + * order as the interfaces in the data vector. To know if the interface is + * a command or a state interface, it is necessary to look into the + * is_command_interface_ vector. + * If the interface is a command interface then the index refers to a name in + * the all_command_interface_names vector, otherwise it refers to a name in the + * all_state_interface_names vector. + * If the interface is not defined, the value is 0 + */ + std::vector interface_name_ids_; + + /** @brief Indices of the interfaces that are either state or command interfaces + * @details The indices are stored in the same order as the interfaces in the + * data vector. It stores only the indices of the interfaces index idx where + * interface_name_ids_[idx] is not 0. + */ + std::vector managed_; +}; + +} // < namespace ethercat_interface + + +#endif // ETHERCAT_INTERFACE__EC_PDO_GROUP_INTERFACE_CHANNEL_MANAGER_HPP_ diff --git a/ethercat_interface/include/ethercat_interface/ec_pdo_single_interface_channel_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_pdo_single_interface_channel_manager.hpp new file mode 100644 index 00000000..d1842a22 --- /dev/null +++ b/ethercat_interface/include/ethercat_interface/ec_pdo_single_interface_channel_manager.hpp @@ -0,0 +1,224 @@ +// Copyright 2024 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: Manuel YGUEL (yguel.robotics@gmail.com) + +#ifndef ETHERCAT_INTERFACE__EC_PDO_SINGLE_INTERFACE_CHANNEL_MANAGER_HPP_ +#define ETHERCAT_INTERFACE__EC_PDO_SINGLE_INTERFACE_CHANNEL_MANAGER_HPP_ + +#include // for std::pair +#include +#include +#include +#include "ethercat_interface/ec_pdo_channel_manager.hpp" + +namespace ethercat_interface +{ + +/** + * @brief Class to manage a PDO channel corresponding to a single interface + */ +class EcPdoSingleInterfaceChannelManager : public EcPdoChannelManager, public InterfaceData +{ +public: + EcPdoSingleInterfaceChannelManager(); + EcPdoSingleInterfaceChannelManager(const EcPdoSingleInterfaceChannelManager &) = delete; + ~EcPdoSingleInterfaceChannelManager(); + +public: +//======================= +/** @name Setup methods + * @brief Methods to setup the PDO channel manager + * @{ + */ + + /// @brief Load the channel configuration from a YAML node + bool load_from_config(YAML::Node channel_config); + +/** @} */ // end of Setup methods +//======================= + +public: +//======================= +/** @name Data exchange methods + * @brief Methods to read and write data from/to the PDO + * @{ + */ + + /// @brief Read the data from the PDO applying data mask, factor and offset + double ec_read(uint8_t * domain_address, size_t i = 0); + + /// @brief Perform an ec_read and update the state interface + void ec_read_to_interface(uint8_t * domain_address); + + /// @brief Write the value to the PDO applying data mask, factor and offset + void ec_write(uint8_t * domain_address, double value, size_t i = 0); + + /// @brief Perform an ec_write and update the command interface + void ec_write_from_interface(uint8_t * domain_address); + +/** @} */ // < end of Data exchange methods +//======================= + +public: + inline + size_t number_of_interfaces() const + { + return 1; + } + + inline + size_t number_of_managed_interfaces() const + { + return (state_interface_name_idx_ + command_interface_name_idx_) > 0; + } + + inline + void setup_managed_interfaces() {} + + inline std::string interface_name(size_t i = 0) const + { + if (0 == i) { + if (has_command_interface_name()) { + return all_command_interface_names[command_interface_name_idx_]; + } else { + if (has_state_interface_name()) { + return all_state_interface_names[state_interface_name_idx_]; + } else { + return "null"; + } + } + } + throw std::out_of_range( + "EcPdoSingleInterfaceChannelManager::interface_name " + "unknown interface index : must be 0 (instead of " + + std::to_string(i) + ")"); + } + + inline + std::string data_type(size_t i = 0) const + { + if (0 == i) { + return id_and_bits_to_type(data_type_idx_, bits_); + } else { + throw std::out_of_range( + "EcPdoSingleInterfaceChannelManager::data_type unknown " + "interface index : must be 0 (instead of " + + std::to_string(i) + ")"); + } + } + + /** @brief Test if an interface named «name» is managed and returns its index among the + * interfaces managed by the PDO channel. + * @param name the name of the interface to test + * @return a pair with a boolean indicating if the interface is managed and the index + * of the interface among the interfaces managed by the PDO channel + */ + std::pair is_interface_managed(std::string name) const; + +public: + inline + void set_state_interface_index(const std::string & /*interface_name*/, size_t index) + { + state_interface_index_ = index; + } + + inline + void set_command_interface_index(const std::string & /*interface_name*/, size_t index) + { + command_interface_index_ = index; + } + + inline + bool has_state_interface_name(size_t /*i*/ = 0) const + { + return 0 != state_interface_name_idx_; + } + + inline + bool has_command_interface_name(size_t /*i*/ = 0) const + { + return 0 != command_interface_name_idx_; + } + + inline + bool has_interface_name(size_t /*i*/) const + { + return has_state_interface_name() || has_command_interface_name(); + } + + inline + bool is_state_interface_defined() const + { + return std::numeric_limits::max() != state_interface_index_; + } + + inline + bool is_command_interface_defined() const + { + return std::numeric_limits::max() != command_interface_index_; + } + + inline size_t state_interface_index(size_t /*i*/) const + { + return state_interface_index_; + } + + inline size_t command_interface_index(size_t /*i*/) const + { + return command_interface_index_; + } + +public: + inline InterfaceData & data(size_t i = 0) + { + if (0 == i) { + return *this; + } + throw std::out_of_range( + "EcPdoSingleInterfaceChannelManager::data unknown " + "interface index : must be 0 (instead of " + + std::to_string(i) + ")"); + } + + inline const InterfaceData & data(size_t i = 0) const + { + if (0 == i) { + return *this; + } + throw std::out_of_range( + "EcPdoSingleInterfaceChannelManager::data unknown " + "interface index : must be 0 (instead of " + + std::to_string(i) + ")"); + } + +protected: + /** @brief Index of the state interface in the ros2 control state interface vector + * @details If the index is not set, the value is std::numeric_limits::max() + */ + size_t state_interface_index_ = std::numeric_limits::max(); + /** @brief Index of the command interface in the ros2 control state interface vector + * @details If the index is not set, the value is std::numeric_limits::max() + */ + size_t command_interface_index_ = std::numeric_limits::max(); + SingleReadFunctionType read_function_ = nullptr; + SingleWriteFunctionType write_function_ = nullptr; + size_t state_interface_name_idx_ = 0; + size_t command_interface_name_idx_ = 0; +}; + +} // < namespace ethercat_interface + + +#endif // ETHERCAT_INTERFACE__EC_PDO_SINGLE_INTERFACE_CHANNEL_MANAGER_HPP_ diff --git a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp index 9f141595..125aee6a 100644 --- a/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_sdo_manager.hpp @@ -110,6 +110,7 @@ class SdoConfigEntry } else if (type == "int64" || type == "uint64") { return 8; } + return 0; } }; diff --git a/ethercat_interface/include/ethercat_interface/ec_slave.hpp b/ethercat_interface/include/ethercat_interface/ec_slave.hpp index da88a826..3e1c5361 100644 --- a/ethercat_interface/include/ethercat_interface/ec_slave.hpp +++ b/ethercat_interface/include/ethercat_interface/ec_slave.hpp @@ -34,9 +34,17 @@ class EcSlave EcSlave(uint32_t vendor_id, uint32_t product_id) : vendor_id_(vendor_id), product_id_(product_id) {} + EcSlave(uint32_t vendor_id, uint32_t product_id, uint16_t alias, uint16_t position) + : vendor_id_(vendor_id), + product_id_(product_id) + { + setAliasAndPosition(alias, position); + } virtual ~EcSlave() {} - /** read or write data to the domain */ - virtual void processData(size_t /*index*/, uint8_t * /*domain_address*/) {} + +public: + /** read or write data to the domain from the index of the entry in the recorded pdos */ + virtual void processData(size_t /*entry_idx*/, 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;} @@ -52,25 +60,45 @@ class EcSlave typedef std::map> DomainMap; virtual void domains(DomainMap & /*domains*/) const {} virtual bool setupSlave( - std::unordered_map slave_paramters, + std::unordered_map slave_parameters, std::vector * state_interface, std::vector * command_interface) { state_interface_ptr_ = state_interface; command_interface_ptr_ = command_interface; - paramters_ = slave_paramters; + parameters_ = slave_parameters; return true; } - uint32_t vendor_id_; - uint32_t product_id_; + +public: + inline + void setAliasAndPosition(uint16_t alias, uint16_t position) + { + alias_ = alias; + position_ = position; + is_alias_and_position_set_ = true; + } + + inline + bool isAliasAndPositionSet() + { + return is_alias_and_position_set_; + } + +public: + uint16_t alias_; //< Slave alias. + uint16_t position_; //< Index after alias. If alias is zero, stores the ring position. + uint32_t vendor_id_; //< Slave vendor ID. + uint32_t product_id_; //< Slave product code. std::vector sdo_config; protected: std::vector * state_interface_ptr_; std::vector * command_interface_ptr_; - std::unordered_map paramters_; + std::unordered_map parameters_; bool is_operational_ = false; + bool is_alias_and_position_set_ = false; }; } // namespace ethercat_interface #endif // ETHERCAT_INTERFACE__EC_SLAVE_HPP_ diff --git a/ethercat_interface/include/ethercat_interface/ec_transfer.hpp b/ethercat_interface/include/ethercat_interface/ec_transfer.hpp new file mode 100644 index 00000000..a03eba4a --- /dev/null +++ b/ethercat_interface/include/ethercat_interface/ec_transfer.hpp @@ -0,0 +1,54 @@ +// Copyright 2024 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: Manuel YGUEL (yguel.robotics@gmail.com) + +#ifndef ETHERCAT_INTERFACE__EC_TRANSFER_HPP_ +#define ETHERCAT_INTERFACE__EC_TRANSFER_HPP_ + +#include +#include + +namespace ethercat_interface +{ + +// Forward declaration to avoid circular dependency +struct DomainInfo; + +struct EcTransferInfo +{ + const DomainInfo * input_domain; + const DomainInfo * output_domain; + + /** Pointer into the input process domain, equal to + * domain process data pointer + the offset + * defined in the ec_pdo_entry_reg_t data structure + * of a domain_regs array in a DomainInfo data structure. + */ + uint8_t * in_ptr; + + /** Pointer into the output process domain, equal to + * domain process data pointer + the offset + * defined in the ec_pdo_entry_reg_t data structure + * of a domain_regs array in a DomainInfo data structure. + */ + uint8_t * out_ptr; + + /** Number of octets of the exchange data. */ + size_t size; +}; + +} // namespace ethercat_interface + +#endif // ETHERCAT_INTERFACE__EC_TRANSFER_HPP_ diff --git a/ethercat_interface/src/ec_master.cpp b/ethercat_interface/src/ec_master.cpp index db31e680..ff3b2730 100644 --- a/ethercat_interface/src/ec_master.cpp +++ b/ethercat_interface/src/ec_master.cpp @@ -25,27 +25,26 @@ #include #include #include - -#define EC_NEWTIMEVAL2NANO(TV) \ - (((TV).tv_sec - 946684800ULL) * 1000000000ULL + (TV).tv_nsec) +#include +#include namespace ethercat_interface { -EcMaster::DomainInfo::DomainInfo(ec_master_t * master) +DomainInfo::DomainInfo(ec_master_t * master) { domain = ecrt_master_create_domain(master); if (domain == NULL) { - printWarning("Failed to create domain"); + EcMaster::printWarning("Failed to create domain"); return; } - const ec_pdo_entry_reg_t empty = {0}; + const ec_pdo_entry_reg_t empty = {0, 0, 0, 0, 0, 0, nullptr, nullptr}; domain_regs.push_back(empty); } -EcMaster::DomainInfo::~DomainInfo() +DomainInfo::~DomainInfo() { for (Entry & entry : entries) { delete[] entry.offset; @@ -54,7 +53,7 @@ EcMaster::DomainInfo::~DomainInfo() } -EcMaster::EcMaster(const int master) +EcMaster::EcMaster(const unsigned int master) { master_ = ecrt_request_master(master); if (master_ == NULL) { @@ -66,9 +65,11 @@ EcMaster::EcMaster(const int master) EcMaster::~EcMaster() { + /* for (SlaveInfo & slave : slave_info_) { - // + //TODO verify what this piece of code was here for } + */ for (auto & domain : domain_info_) { if (domain.second != NULL) { delete domain.second; @@ -78,14 +79,25 @@ EcMaster::~EcMaster() void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave * slave) { - // configure slave in master + slave->setAliasAndPosition(alias, position); + addSlave(slave); +} +void EcMaster::addSlave(EcSlave * slave) +{ + if (false == slave->isAliasAndPositionSet()) { + std::string error_message = "Alias and position not set for slave (vendor id=" + std::to_string( + slave->vendor_id_) + ",product_code=" + std::to_string(slave->product_id_) + ")."; + throw std::runtime_error(error_message); + } + + // 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_); + master_, + slave->alias_, slave->position_, + slave->vendor_id_, slave->product_id_); if (slave_info.config == NULL) { printWarning("Add slave. Failed to get slave configuration."); return; @@ -108,7 +120,10 @@ void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave * slave) slave_info_.push_back(slave_info); - // check if slave has pdos + // Setup PDOs registered by the slave. + // For each slave, PDOs are grouped by sync manager. + // For each active sync manager of the slave, + // register the associated set of PDOs. size_t num_syncs = slave->syncSize(); const ec_sync_info_t * syncs = slave->syncs(); if (num_syncs > 0) { @@ -121,27 +136,26 @@ void EcMaster::addSlave(uint16_t alias, uint16_t position, EcSlave * slave) } else { printWarning( "Add slave. Sync size is zero for " + - std::to_string(alias) + ":" + std::to_string(position)); + std::to_string(slave->alias_) + ":" + std::to_string(slave->position_)); } - // check if slave registered any pdos for the domain + // Get all domains and associated pdos that the slave registers 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; + DomainInfo * domain = NULL; if (domain_info_.count(domain_index)) { - domain_info = domain_info_.at(domain_index); + domain = domain_info_.at(domain_index); } - if (domain_info == NULL) { - domain_info = new DomainInfo(master_); - domain_info_[domain_index] = domain_info; + if (domain == NULL) { + domain = new DomainInfo(master_); + domain_info_[domain_index] = domain; } registerPDOInDomain( - alias, position, - iter.second, domain_info, + iter.second, domain, slave); } } @@ -165,7 +179,6 @@ int EcMaster::configSlaveSdo( } void EcMaster::registerPDOInDomain( - uint16_t alias, uint16_t position, std::vector & channel_indices, DomainInfo * domain_info, EcSlave * slave) @@ -191,8 +204,8 @@ void EcMaster::registerPDOInDomain( 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.alias = slave->alias_; + pdo_reg.position = slave->position_; pdo_reg.vendor_id = slave->vendor_id_; pdo_reg.product_code = slave->product_id_; pdo_reg.index = pdo_regs[channel_indices[i]].index; @@ -202,16 +215,20 @@ void EcMaster::registerPDOInDomain( // 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(pdo_reg.subindex); - std::cout << "}" << std::dec << std::endl; + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), + "{ %d, %d, 0x%x, 0x%x, 0x%x, 0x%x }", + pdo_reg.alias, + pdo_reg.position, + pdo_reg.vendor_id, + pdo_reg.product_code, + pdo_reg.index, + static_cast(pdo_reg.subindex) + ); } // set the last element to null - ec_pdo_entry_reg_t empty = {0}; + ec_pdo_entry_reg_t empty = {0, 0, 0, 0, 0, 0, nullptr, nullptr}; domain_info->domain_regs.back() = empty; } @@ -270,6 +287,10 @@ void EcMaster::update(uint32_t domain) ecrt_domain_process(domain_info->domain); + // Transfer data if configured + // TODO(@yguel) make transfer per domain ? Quid of transfers across domains ? + transferAll(); + // check process data state (optional) checkDomainState(domain); @@ -312,6 +333,10 @@ void EcMaster::readData(uint32_t domain) ecrt_domain_process(domain_info->domain); + // Transfer data if configured + // TODO(@yguel) make transfer per domain ? Quid of transfers across domains ? + transferAll(); + // check process data state (optional) checkDomainState(domain); @@ -441,7 +466,7 @@ void EcMaster::setThreadRealTime() /* 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; + constexpr unsigned int MAX_SAFE_STACK = 8 * 1024; unsigned char dummy[MAX_SAFE_STACK]; memset(dummy, 0, MAX_SAFE_STACK); } @@ -457,10 +482,18 @@ void EcMaster::checkDomainState(uint32_t domain) 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); + RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Domain: WC %d.", ds.working_counter); } if (ds.wc_state != domain_info->domain_state.wc_state) { - printf("Domain: State %u.\n", ds.wc_state); + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), + "Domain: State %s.", + ds.wc_state == EC_WC_ZERO ? "ZERO" : + ( + (ds.wc_state == EC_WC_INCOMPLETE) ? "INCOMPLETE" : + (ds.wc_state == EC_WC_COMPLETE) ? "COMPLETE" : "UNKNOWN" + ) + ); } domain_info->domain_state = ds; } @@ -472,13 +505,13 @@ void EcMaster::checkMasterState() ecrt_master_state(master_, &ms); if (ms.slaves_responding != master_state_.slaves_responding) { - printf("%u slave(s).\n", ms.slaves_responding); + RCLCPP_WARN(rclcpp::get_logger("EthercatDriver"), "%d slave(s).", ms.slaves_responding); } if (ms.al_states != master_state_.al_states) { - printf("Master AL states: 0x%02X.\n", ms.al_states); + RCLCPP_WARN(rclcpp::get_logger("EthercatDriver"), "Master AL states: 0x%02X.", ms.al_states); } if (ms.link_up != master_state_.link_up) { - printf("Link is %s.\n", ms.link_up ? "up" : "down"); + RCLCPP_WARN(rclcpp::get_logger("EthercatDriver"), "Link is %s.", ms.link_up ? "up" : "down"); } master_state_ = ms; } @@ -492,23 +525,162 @@ void EcMaster::checkSlaveStates() if (s.al_state != slave.config_state.al_state) { // this spams the terminal at initialization. - printf("Slave: State 0x%02X.\n", s.al_state); + RCLCPP_WARN(rclcpp::get_logger("EthercatDriver"), "Slave: State 0x%02X.", s.al_state); } if (s.online != slave.config_state.online) { - printf("Slave: %s.\n", s.online ? "online" : "offline"); + RCLCPP_WARN( + rclcpp::get_logger( + "EthercatDriver"), "Slave: %s.", s.online ? "online" : "offline"); } if (s.operational != slave.config_state.operational) { - printf("Slave: %soperational.\n", s.operational ? "" : "Not "); + RCLCPP_WARN( + rclcpp::get_logger("EthercatDriver"), + "Slave: (alias: %d, pos: %d, vendor_id: %d, prod_id: %d) --> %soperational.", + slave.slave->alias_, + slave.slave->position_, + slave.slave->vendor_id_, + slave.slave->product_id_, + s.operational ? "" : "NOT "); slave.slave->set_state_is_operational(s.operational ? true : false); } slave.config_state = s; } } +void EcMaster::checkDomainInfoValidity( + const DomainInfo & domain_info, + const ec_pdo_entry_reg_t & pdo_entry_reg) +{ + if (nullptr == domain_info.domain_pd) { + throw std::runtime_error("Domain process data pointer not set."); + } + if (nullptr == pdo_entry_reg.offset) { + throw std::runtime_error("Offset not set in pdo_entry_reg."); + } +} + +void EcMaster::registerTransferInDomain(const std::vector & transfer_nets) +{ + // Fill in the EcTransferInfo structures + + // For each transfer of each net, + for (auto & net : transfer_nets) { + for (auto & transfer : net.transfers) { + EcTransferInfo transfer_info; + transfer_info.size = transfer.size; + RCLCPP_INFO(rclcpp::get_logger("EthercatDriver"), "Transfer size: %ld", transfer.size); + /** + * For the input and the output of the transfer find + * 1. the process domain data pointer + * 2. the offset in the process domain data + * By iterating over the existing DomainInfo and domain_regs vector + * to find the ec_pdo_entry_reg_t whose alias, position, index and subindex + * match the transfer input and output memory entries + */ + for (const auto & key_val : domain_info_) { + const DomainInfo & domain = *(key_val.second); + for (auto & domain_reg : domain.domain_regs) { + // Find match for input + if (domain_reg.alias == transfer.input.alias && + domain_reg.position == transfer.input.position && + domain_reg.index == transfer.input.index && + domain_reg.subindex == transfer.input.subindex) + { + transfer_info.input_domain = &domain; + // 3. Compute the pointer arithmetic and store the result in the EcTransferInfo object + transfer_info.in_ptr = domain.domain_pd + *(domain_reg.offset); + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), + "Transfer input: esclave position: %d / index: 0x%x / in offset: %d", + domain_reg.position, + domain_reg.index, + *(domain_reg.offset) + ); + } + // Find match for output + if (domain_reg.alias == transfer.output.alias && + domain_reg.position == transfer.output.position && + domain_reg.index == transfer.output.index && + domain_reg.subindex == transfer.output.subindex) + { + transfer_info.output_domain = &domain; + // 3. Compute the pointer arithmetic and store the result in the EcTransferInfo object + transfer_info.out_ptr = domain.domain_pd + *(domain_reg.offset); + RCLCPP_INFO( + rclcpp::get_logger("EthercatDriver"), + "Transfer output: slave position: %d / index: 0x%x / out offset: %d", + domain_reg.position, + domain_reg.index, + *(domain_reg.offset) + ); + } + } + } + + // Record the transfer + transfers_.push_back(transfer_info); + } + } +} + +void EcMaster::transferAll() +{ + // Proceed to the transfer of all the data declared in transfers_. + for (auto & transfer : transfers_) { + // Copy the data from the input to the output + memcpy(transfer.out_ptr, transfer.in_ptr, transfer.size); + } +} + +void EcMaster::printMemoryFrames(std::ostream & os) +{ + for (auto & kv : domain_info_) { + os << "Domain: " << kv.first << std::endl; + auto & d = kv.second; + size_t size = ecrt_domain_size(d->domain); + // Display the memory + for (size_t i = 0; i < size; i++) { + os << std::hex << static_cast(d->domain_pd[i]) << " "; + } + os << std::endl; + } +} + +uint8_t * EcMaster::getMemoryStart( + const uint16_t position, + const uint16_t index, + const uint16_t subindex) +{ + for (auto & kv : domain_info_) { + auto & d = kv.second; + for (auto & reg : d->domain_regs) { + if (reg.position == position && reg.index == index && reg.subindex == subindex) { + return d->domain_pd + *(reg.offset); + } + } + } + return nullptr; +} -void EcMaster::printWarning(const std::string & message) +void EcMaster::printMemoryFrame( + const uint16_t position, + const uint16_t index, + const uint16_t subindex, + const size_t n, + bool binary, + std::ostream & os) { - std::cout << "WARNING. Master. " << message << std::endl; + uint8_t * pointer = getMemoryStart(position, index, subindex); + if (pointer != nullptr) { + for (size_t i = 0; i < n; i++) { + if (binary) { + os << std::bitset<8>(pointer[i]) << " "; + } else { + os << std::hex << static_cast(pointer[i]) << " "; + } + } + os << std::endl; + } } } // namespace ethercat_interface diff --git a/ethercat_interface/src/ec_pdo_channel_manager.cpp b/ethercat_interface/src/ec_pdo_channel_manager.cpp new file mode 100644 index 00000000..506b9fb0 --- /dev/null +++ b/ethercat_interface/src/ec_pdo_channel_manager.cpp @@ -0,0 +1,309 @@ +// Copyright 2024 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: Manuel YGUEL (yguel.robotics@gmail.com) + +#include +#include +#include +// #include // For debugging purpose +#include "ethercat_interface/ec_pdo_channel_manager.hpp" + +namespace ethercat_interface +{ + +const std::vector ec_pdo_channel_data_types = { + "unknown", + "bit", + "bool", + "int8", "uint8", + "int16", "uint16", + "int32", "uint32", + "int64", "uint64" +}; + +const std::vector ec_pdo_channel_data_bits = { + 0, + 0, + 1, + 8, 8, + 16, 16, + 32, 32, + 64, 64 +}; + +std::vector all_state_interface_names = { + "unknown" +}; + +std::vector all_command_interface_names = { + "unknown" +}; + +size_t typeIdx(const std::string & type) +{ + // Handle the types of the form bitXXX + if (type.find("bit") != std::string::npos) { + return 1; + } + // Handle the standard types + auto it = + std::find(ec_pdo_channel_data_types.begin(), ec_pdo_channel_data_types.end(), type); + if (it != ec_pdo_channel_data_types.end()) { + return std::distance(ec_pdo_channel_data_types.begin(), it); + } + // Handle an unknown type + return 0; +} + +uint8_t type2bits(const std::string & type) +{ + size_t type_idx = typeIdx(type); + // Handle the types of the form bitXXX + if (1 == type_idx) { + try { + std::string n_bits = type.substr(type.find("bit") + 3); + return static_cast(std::stoi(n_bits)); + } catch (std::invalid_argument & e) { + return 0; + } + } + // Handle the other types + return ec_pdo_channel_data_bits[type_idx]; +} + +std::string id_and_bits_to_type(size_t type_idx, uint8_t bits) +{ + if (type_idx < ec_pdo_channel_data_types.size()) { + if (1 == type_idx) { + return "bit" + std::to_string(bits); + } + return ec_pdo_channel_data_types[type_idx]; + } else { + throw std::out_of_range( + "id_and_bits_to_type: unknown index type (type_idx must be < " + + std::to_string( + ec_pdo_channel_data_types.size()) + ", the size of known types, instead of " + + std::to_string(type_idx) + " )"); + } +} + +#ifndef CLASSM +#define CLASSM EcPdoChannelManager +#else +#error alias CLASSM all ready defined! +#endif // < CLASSM + +CLASSM::EcPdoChannelManager() { +} + +CLASSM::~EcPdoChannelManager() { +} + +ec_pdo_entry_info_t CLASSM::get_pdo_entry_info() +{ + RCLCPP_INFO( + rclcpp::get_logger("EcPdoChannelManager"), + "{0x%x, 0x%x, %d}", + index, + static_cast(sub_index), + static_cast(pdo_bits()) + ); + + return {index, sub_index, pdo_bits()}; +} + +void CLASSM::setup_interface_ptrs( + std::vector * state_interface, + std::vector * command_interface) +{ + command_interface_ptr_ = command_interface; + state_interface_ptr_ = state_interface; +} + +void CLASSM::ec_update(uint8_t * domain_address) +{ + ec_read_to_interface(domain_address); + ec_write_from_interface(domain_address); +} + + +#undef CLASSM + +double uint8_read(uint8_t * domain_address, uint8_t /*data_mask*/) +{ + return static_cast(EC_READ_U8(domain_address)); +} + +double int8_read(uint8_t * domain_address, uint8_t /*data_mask*/) +{ + return static_cast(EC_READ_S8(domain_address)); +} + +double uint16_read(uint8_t * domain_address, uint8_t /*data_mask*/) +{ + return static_cast(EC_READ_U16(domain_address)); +} + +double int16_read(uint8_t * domain_address, uint8_t /*data_mask*/) +{ + return static_cast(EC_READ_S16(domain_address)); +} + +double uint32_read(uint8_t * domain_address, uint8_t /*data_mask*/) +{ + return static_cast(EC_READ_U32(domain_address)); +} + +double int32_read(uint8_t * domain_address, uint8_t /*data_mask*/) +{ + return static_cast(EC_READ_S32(domain_address)); +} + +double uint64_read(uint8_t * domain_address, uint8_t /*data_mask*/) +{ + return static_cast(EC_READ_U64(domain_address)); +} + +double int64_read(uint8_t * domain_address, uint8_t /*data_mask*/) +{ + return static_cast(EC_READ_S64(domain_address)); +} + +double bool_read(uint8_t * domain_address, uint8_t data_mask) +{ + return (EC_READ_U8(domain_address) & data_mask) ? 1. : 0.; +} + +double octet_read(uint8_t * domain_address, uint8_t data_mask) +{ + return static_cast(EC_READ_U8(domain_address) & data_mask); +} + +const SingleReadFunctionType ec_pdo_single_read_functions[] = { + NULL, + octet_read, + bool_read, + int8_read, uint8_read, + int16_read, uint16_read, + int32_read, uint32_read, + int64_read, uint64_read, + octet_read +}; + +/*############################*/ +/* Single Write functions */ +/*############################*/ + +void uint8_write(uint8_t * domain_address, double value, uint8_t /*data_mask*/) +{ + EC_WRITE_U8(domain_address, static_cast(value)); +} + +void int8_write(uint8_t * domain_address, double value, uint8_t /*data_mask*/) +{ + EC_WRITE_S8(domain_address, static_cast(value)); +} + +void uint16_write(uint8_t * domain_address, double value, uint8_t /*data_mask*/) +{ + EC_WRITE_U16(domain_address, static_cast(value)); +} + +void int16_write(uint8_t * domain_address, double value, uint8_t /*data_mask*/) +{ + EC_WRITE_S16(domain_address, static_cast(value)); +} + +void uint32_write(uint8_t * domain_address, double value, uint8_t /*data_mask*/) +{ + EC_WRITE_U32(domain_address, static_cast(value)); +} + +void int32_write(uint8_t * domain_address, double value, uint8_t /*data_mask*/) +{ + EC_WRITE_S32(domain_address, static_cast(value)); +} + +void uint64_write(uint8_t * domain_address, double value, uint8_t /*data_mask*/) +{ + EC_WRITE_U64(domain_address, static_cast(value)); +} + +void int64_write(uint8_t * domain_address, double value, uint8_t /*data_mask*/) +{ + EC_WRITE_S64(domain_address, static_cast(value)); +} + +/** @brief Helper function that counts the number of bits in an octet */ +inline +uint8_t count_bits(uint8_t octet) +{ + return __builtin_popcount(octet); +} + +bool check_type(const std::string & type, uint8_t mask) +{ + if ("bool" == type) { + return 1 == count_bits(mask); + } + return true; +} + +/** @brief Modify one bit defined by the mask + * \pre The mask must contain only one bit set to one + */ +void bool_compose(uint8_t * domain_address, double value, uint8_t data_mask) +{ + uint8_t buffer = EC_READ_U8(domain_address); + // Clear the bit + buffer &= ~(data_mask); + if (value) { // Set the bit + buffer |= data_mask; + } + EC_WRITE_U8(domain_address, buffer); +} + +/** Modify only the bits set to one in the mask */ +void octet_compose(uint8_t * domain_address, double value, uint8_t data_mask) +{ + uint8_t buffer = EC_READ_U8(domain_address); + // Clear the bits + buffer &= ~(data_mask); + uint8_t compose_buffer = static_cast(value) & data_mask; + EC_WRITE_U8(domain_address, buffer | compose_buffer); +} + +/** Modify the whole octet with the result of the applied mask */ +void octet_override(uint8_t * domain_address, double value, uint8_t data_mask) +{ + EC_WRITE_U8(domain_address, static_cast(value) & data_mask); +} + +const SingleWriteFunctionType ec_pdo_single_write_functions[] = { + NULL, + octet_compose, + bool_compose, + int8_write, uint8_write, + int16_write, uint16_write, + int32_write, uint32_write, + int64_write, uint64_write, + octet_override +}; + +/*###############################*/ +/* End of Single Write functions */ +/*###############################*/ + +} // < namespace ethercat_interface diff --git a/ethercat_interface/src/ec_pdo_group_interface_channel_manager.cpp b/ethercat_interface/src/ec_pdo_group_interface_channel_manager.cpp new file mode 100644 index 00000000..89e9a074 --- /dev/null +++ b/ethercat_interface/src/ec_pdo_group_interface_channel_manager.cpp @@ -0,0 +1,438 @@ +// Copyright 2024 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: Manuel YGUEL (yguel.robotics@gmail.com) + +#include +#include +#include "ethercat_interface/ec_pdo_group_interface_channel_manager.hpp" + +namespace ethercat_interface +{ + +#ifndef CLASSM +#define CLASSM EcPdoGroupInterfaceChannelManager +#else +#error alias CLASSM all ready defined! +#endif // < CLASSM + +CLASSM::EcPdoGroupInterfaceChannelManager() +{ +} + +CLASSM::~EcPdoGroupInterfaceChannelManager() +{ +} + +std::string CLASSM::interface_name(size_t i) const +{ + if (v_data.size() > i) { + if (has_interface_name(i)) { + if (is_command_interface_[i]) { + return all_command_interface_names[interface_name_ids_[i]]; + } else { + return all_state_interface_names[interface_name_ids_[i]]; + } + } else { + return "null"; + } + } + throw std::out_of_range( + "EcPdoGroupInterfaceChannelManager::interface_name " + "unknown interface index : must be < " + + std::to_string(v_data.size()) + "(instead of " + + std::to_string(i) + ")"); +} + +std::string CLASSM::data_type(size_t i) const +{ + if (v_data.size() > i) { + const InterfaceDataWithAddrOffset & d = v_data[i]; + return id_and_bits_to_type(d.data_type_idx, d.bits); + } else { + throw std::out_of_range( + "EcPdoGroupInterfaceChannelManager::data_type unknown " + "interface index : must be < " + + std::to_string(v_data.size()) + " (instead of " + + std::to_string(i) + ") "); + } +} + +std::pair CLASSM::is_interface_managed(std::string name) const +{ + for (size_t i = 0; i < v_data.size(); ++i) { + if (name == interface_name(i)) { + return std::make_pair(true, i); + } + } + return std::make_pair(false, std::numeric_limits::max()); // not found +} + +size_t CLASSM::channel_state_interface_index(const std::string & name) const +{ + for (size_t i = 0; i < v_data.size(); ++i) { + if (has_interface_name(i) && !is_command_interface_.at(i)) { + if (name == all_state_interface_names.at(interface_name_ids_.at(i))) { + return i; + } + } + } + throw std::out_of_range( + "EcPdoGroupInterfaceChannelManager::channel_state_interface_index '" + + name + "' unknown index for state interface"); +} + +size_t CLASSM::channel_command_interface_index(const std::string & name) const +{ + for (size_t i = 0; i < v_data.size(); ++i) { + if (has_interface_name(i) && is_command_interface_.at(i)) { + if (name == all_command_interface_names.at(interface_name_ids_.at(i))) { + return i; + } + } + } + throw std::out_of_range( + "EcPdoGroupInterfaceChannelManager::channel_command_interface_index '" + + name + + "' unknown index for command interface"); +} + +size_t CLASSM::state_interface_index(size_t i) const +{ + if (v_data.size() > i) { + if (!is_command_interface_[i]) { + return interface_ids_[i]; + } else { + return std::numeric_limits::max(); + } + } + throw std::out_of_range( + "EcPdoGroupInterfaceChannelManager::state_interface_index " + "unknown interface index : must be < " + + std::to_string(v_data.size()) + + "(instead of " + std::to_string(i) + ")"); +} + +size_t CLASSM::command_interface_index(size_t i) const +{ + if (v_data.size() > i) { + if (is_command_interface_[i]) { + return interface_ids_[i]; + } else { + return std::numeric_limits::max(); + } + } + throw std::out_of_range( + "EcPdoGroupInterfaceChannelManager::command_interface_index " + "unknown interface index : must be < " + + std::to_string(v_data.size()) + + "(instead of " + std::to_string(i) + ")"); +} + +void CLASSM::allocate_for_new_interface() +{ + v_data.push_back(InterfaceDataWithAddrOffset()); + interface_ids_.push_back(std::numeric_limits::max()); + is_command_interface_.push_back(false); + read_functions_.push_back(nullptr); + write_functions_.push_back(nullptr); +} + +size_t CLASSM::add_command_interface(const std::string & name) +{ + // Check if the interface is already present + auto managed = is_interface_managed(name); + if (managed.first) { + return managed.second; + } + + // Add the interface + // Stores the index of the interface in all the vectors + size_t id = v_data.size(); + // Resize all the vectors + allocate_for_new_interface(); + is_command_interface_[id] = true; + // Add the name of the interface to the command interface names + size_t name_idx = all_command_interface_names.size(); + all_command_interface_names.push_back(name); + interface_name_ids_.push_back(name_idx); + + // Add the interface to the managed interfaces + managed_.push_back(id); + + return id; +} + +size_t CLASSM::add_data_without_interface() +{ + size_t id = v_data.size(); + allocate_for_new_interface(); + is_command_interface_[id] = false; + interface_name_ids_.push_back(0); + return id; +} + +size_t CLASSM::add_state_interface(const std::string & name) +{ + // Check if the interface is already present + auto managed = is_interface_managed(name); + if (managed.first) { + return managed.second; + } + + // Add the interface + // Stores the index of the interface in all the vectors + size_t id = v_data.size(); + // Resize all the vectors + allocate_for_new_interface(); + is_command_interface_[id] = false; + // Add the name of the interface to the command interface names + size_t name_idx = all_state_interface_names.size(); + all_state_interface_names.push_back(name); + interface_name_ids_.push_back(name_idx); + + // Add the interface to the managed interfaces + managed_.push_back(id); + + return id; +} + +bool CLASSM::load_from_config(YAML::Node channel_config) +{ + // TODO(@yguel@unistra): Use ROS2 logging + // index + if (channel_config["index"]) { + index = channel_config["index"].as(); + } else { + std::cerr << "missing channel index info" << std::endl; + } + + // sub_index + if (channel_config["sub_index"]) { + sub_index = channel_config["sub_index"].as(); + } else { + std::cerr << "channel " << index << " : missing channel info" << std::endl; + } + + // data type + std::string data_type; + if (channel_config["type"]) { + data_type = channel_config["type"].as(); + data_type_idx_ = typeIdx(data_type); + if (0 == data_type_idx_) { + std::cerr << "channel" << index << " : unknown data type " << data_type << std::endl; + return false; + } + bits_ = type2bits(data_type); + } else { + std::cerr << "channel" << index << " : missing channel data type info" << std::endl; + } + + size_t id = std::numeric_limits::max(); + if (channel_config["command_interface"]) { + throw std::runtime_error( + "Global command_interface is not allowed in a grouped " + "interface pdo channel, it must be defined per interface " + "in the data_mapping"); + } + + if (channel_config["state_interface"]) { + auto state_interface_name = channel_config["state_interface"].as(); + id = add_state_interface(state_interface_name); + } else { + id = add_data_without_interface(); + } + + v_data[id].data_type_idx = data_type_idx_; + v_data[id].bits = bits_; + read_functions_[id] = ec_pdo_single_read_functions[v_data[id].data_type_idx]; + + // factor + if (channel_config["factor"]) { + if (id != std::numeric_limits::max()) { + v_data[id].factor = channel_config["factor"].as(); + } else { + // Error + // TODO(yguel@unistra.fr: log error) + } + } + // offset + if (channel_config["offset"]) { + if (id != std::numeric_limits::max()) { + v_data[id].offset = channel_config["offset"].as(); + } else { + // Error + // TODO(yguel@unistra.fr: log error) + } + } + // mask + if (channel_config["mask"]) { + if (id != std::numeric_limits::max()) { + v_data[id].mask = channel_config["mask"].as(); + if (!check_type(data_type, v_data[id].mask) ) { + std::cerr << "channel: " << index << " : mask " << std::bitset<8>(v_data[id].mask) << + " is not compatible with data type " << + data_type << std::endl; + return false; + } + } else { + // Error + // TODO(yguel@unistra: log error) + } + } + + // skip + if (channel_config["skip"]) { + skip = channel_config["skip"].as(); + // TODO(yguel@unistra: check if skip is relevant in a grouped interface pdo channel) + } + + // Handle data mapping + if (channel_config["data_mapping"]) { + auto data_mapping = channel_config["data_mapping"]; + for (auto map : data_mapping) { + // Reset the id to skip adding data if no interface is defined + id = std::numeric_limits::max(); + if (map["command_interface"]) { + auto command_interface_name = map["command_interface"].as(); + id = add_command_interface(command_interface_name); + if (map["default_value"]) { + v_data[id].default_value = map["default_value"].as(); + } + } + + if (map["state_interface"]) { + auto state_interface_name = map["state_interface"].as(); + id = add_state_interface(state_interface_name); + } + + if (std::numeric_limits::max() == id) { + id = add_data_without_interface(); + } + + if (map["addr_offset"]) { + v_data[id].addr_offset = map["addr_offset"].as(); + } + if (map["type"]) { + data_type = map["type"].as(); + auto type_idx = typeIdx(data_type); + v_data[id].data_type_idx = type_idx; + if (0 == v_data[id].data_type_idx) { + std::cerr << "channel: " << index << " : unknown data type " << data_type << std::endl; + return false; + } + v_data[id].bits = type2bits(data_type); + read_functions_[id] = ec_pdo_single_read_functions[type_idx]; + write_functions_[id] = ec_pdo_single_write_functions[type_idx]; + } + + // factor + if (map["factor"]) { + v_data[id].factor = map["factor"].as(); + } + + // offset + if (map["offset"]) { + v_data[id].offset = map["offset"].as(); + } + + // mask + if (map["mask"]) { + v_data[id].mask = map["mask"].as(); + if (!check_type(data_type, v_data[id].mask)) { + std::cerr << "channel" << index << " : mask " << std::bitset<8>(v_data[id].mask) << + " is not compatible with data type " << data_type << std::endl; + return false; + } + } + } + } + + return true; +} + +void CLASSM::setup_managed_interfaces() +{ + managed_.clear(); + for (size_t i = 0; i < interface_name_ids_.size(); ++i) { + if (0 != interface_ids_[i]) { + managed_.push_back(i); + } + } +} + +double CLASSM::ec_read(uint8_t * domain_address, size_t i) +{ + InterfaceDataWithAddrOffset & d = v_data[i]; + double last_value = read_functions_[i](domain_address + d.addr_offset, d.mask); + last_value = d.factor * last_value + d.offset; + if (is_state_interface_defined(i) ) { + state_interface_ptr_->at(interface_ids_[i]) = last_value; + } + d.last_value = last_value; + return last_value; +} + +void CLASSM::ec_read_to_interface(uint8_t * domain_address) +{ + for (size_t i = 0; i < managed_.size(); ++i) { + const size_t idx = managed_[i]; + ec_read(domain_address, idx); + if (is_state_interface_defined(idx) ) { + state_interface_ptr_->at(interface_ids_[idx]) = v_data[idx].last_value; + } + } +} + +void CLASSM::ec_write(uint8_t * domain_address, double value, size_t i) +{ + if (RPDO != pdo_type || !allow_ec_write) { + return; + } + + InterfaceDataWithAddrOffset & d = v_data[i]; + + if (!std::isnan(value) && !d.override_command) { + d.last_value = d.factor * value + d.offset; + write_functions_[i](domain_address + d.addr_offset, d.last_value, d.mask); + } else { + if (!std::isnan(d.default_value)) { + d.last_value = d.default_value; + write_functions_[i](domain_address + d.addr_offset, d.last_value, d.mask); + } else { // Do nothing + return; + } + } +} + +void CLASSM::ec_write_from_interface(uint8_t * domain_address) +{ + for (size_t i = 0; i < managed_.size(); ++i) { + const size_t idx = managed_[i]; + if (is_command_interface_defined(idx) ) { + const auto value = command_interface_ptr_->at(interface_ids_[idx]); + ec_write(domain_address, value, idx); + } else { + auto & d = v_data[idx]; + if ( (RPDO == pdo_type) && allow_ec_write && !std::isnan(d.default_value)) { + d.last_value = d.default_value; + write_functions_[idx](domain_address + d.addr_offset, d.last_value, d.mask); + } + } + } +} + +#undef CLASSM + +} // < namespace ethercat_interface diff --git a/ethercat_interface/src/ec_pdo_single_interface_channel_manager.cpp b/ethercat_interface/src/ec_pdo_single_interface_channel_manager.cpp new file mode 100644 index 00000000..be3a7b9f --- /dev/null +++ b/ethercat_interface/src/ec_pdo_single_interface_channel_manager.cpp @@ -0,0 +1,190 @@ +// Copyright 2024 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: Manuel YGUEL (yguel.robotics@gmail.com) + +#include +#include +#include "ethercat_interface/ec_pdo_single_interface_channel_manager.hpp" + +namespace ethercat_interface +{ + +#ifndef CLASSM +#define CLASSM EcPdoSingleInterfaceChannelManager +#else +#error alias CLASSM all ready defined! +#endif // < CLASSM + + +CLASSM::EcPdoSingleInterfaceChannelManager() + : state_interface_index_(std::numeric_limits::max()), + command_interface_index_(std::numeric_limits::max()), + read_function_(nullptr), + write_function_(nullptr), + state_interface_name_idx_(0), + command_interface_name_idx_(0) +{ +} + +CLASSM::~EcPdoSingleInterfaceChannelManager() +{ +} + +std::pair CLASSM::is_interface_managed(std::string name) const +{ + if (has_command_interface_name()) { + bool managed = (name == all_command_interface_names[command_interface_name_idx_]); + if (managed) { + return std::make_pair(true, 0); + } + } + if (has_state_interface_name()) { + bool managed = (name == all_state_interface_names[state_interface_name_idx_]); + if (managed) { + return std::make_pair(true, 0); + } else { + return std::make_pair(false, std::numeric_limits::max()); + } + } + return std::make_pair(false, std::numeric_limits::max()); +} + +bool CLASSM::load_from_config(YAML::Node channel_config) +{ + // TODO(@yguel@unistra): Use ROS2 logging + // index + if (channel_config["index"]) { + index = channel_config["index"].as(); + } else { + std::cerr << "missing channel index info" << std::endl; + } + + // sub_index + if (channel_config["sub_index"]) { + sub_index = channel_config["sub_index"].as(); + } else { + std::cerr << "channel: " << index << " : missing channel info" << std::endl; + } + + // data type + std::string data_type; + if (channel_config["type"]) { + data_type = channel_config["type"].as(); + data_type_idx_ = typeIdx(data_type); + if (0 == data_type_idx_) { + std::cerr << "channel: " << index << " : unknown data type " << data_type << std::endl; + return false; + } + bits_ = type2bits(data_type); + read_function_ = ec_pdo_single_read_functions[data_type_idx_]; + write_function_ = ec_pdo_single_write_functions[data_type_idx_]; + } else { + std::cerr << "channel: " << index << " : missing channel data type info" << std::endl; + } + + if (channel_config["command_interface"]) { + auto command_interface_name = channel_config["command_interface"].as(); + command_interface_name_idx_ = all_command_interface_names.size(); + all_command_interface_names.push_back(command_interface_name); + // default value + if (channel_config["default"]) { + default_value = channel_config["default"].as(); + } + } + + if (channel_config["state_interface"]) { + auto state_interface_name = channel_config["state_interface"].as(); + state_interface_name_idx_ = all_state_interface_names.size(); + all_state_interface_names.push_back(state_interface_name); + } + + // factor + if (channel_config["factor"]) { + factor = channel_config["factor"].as(); + } + // offset + if (channel_config["offset"]) { + offset = channel_config["offset"].as(); + } + // mask + if (channel_config["mask"]) { + mask = channel_config["mask"].as(); + if (!check_type(data_type, mask)) { + std::cerr << "channel: " << index << " : mask " << std::bitset<8>(mask) << + " is not compatible with data type " << data_type << std::endl; + return false; + } + } + + // skip + if (channel_config["skip"]) { + skip = channel_config["skip"].as(); + } + + return true; +} + +double CLASSM::ec_read(uint8_t * domain_address, size_t /*i*/) +{ + last_value = read_function_(domain_address, mask); + last_value = factor * last_value + offset; + if (is_state_interface_defined() ) { + state_interface_ptr_->at(state_interface_index_) = last_value; + } + return last_value; +} + +void CLASSM::ec_read_to_interface(uint8_t * domain_address) +{ + ec_read(domain_address); + if (is_state_interface_defined() ) { + state_interface_ptr_->at(state_interface_index_) = last_value; + } +} + +void CLASSM::ec_write(uint8_t * domain_address, double value, size_t /*i*/) +{ + if (RPDO != pdo_type || !allow_ec_write) { + return; + } + if (!std::isnan(value) && !override_command) { + last_value = factor * value + offset; + write_function_(domain_address, last_value, mask); + } else { + if (!std::isnan(default_value)) { + last_value = default_value; + write_function_(domain_address, last_value, mask); + } else { // Do nothing + return; + } + } +} + +void CLASSM::ec_write_from_interface(uint8_t * domain_address) +{ + if (is_command_interface_defined() ) { + const auto value = command_interface_ptr_->at(command_interface_index_); + ec_write(domain_address, value); + } else { + if ( (RPDO == pdo_type) && allow_ec_write && !std::isnan(default_value)) { + last_value = default_value; + write_function_(domain_address, last_value, mask); + } + } +} + +#undef CLASSM + +} // < namespace ethercat_interface diff --git a/ethercat_interface/test/test_ec_pdo_channel_manager.cpp b/ethercat_interface/test/test_ec_pdo_channel_manager.cpp index a43dd51c..2384f4df 100644 --- a/ethercat_interface/test/test_ec_pdo_channel_manager.cpp +++ b/ethercat_interface/test/test_ec_pdo_channel_manager.cpp @@ -14,38 +14,43 @@ #include #include +#include +#include +#include +#include -#include "ethercat_interface/ec_pdo_channel_manager.hpp" +#include "ethercat_interface/ec_pdo_single_interface_channel_manager.hpp" +#include "ethercat_interface/ec_pdo_group_interface_channel_manager.hpp" #include "yaml-cpp/yaml.h" -TEST(TestEcPdoChannelManager, LoadFromConfig) +TEST(TestEcPdoSingleInterfaceChannelManager, 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; + ethercat_interface::EcPdoSingleInterfaceChannelManager 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.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) +TEST(TestEcPdoSingleInterfaceChannelManager, 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; + ethercat_interface::EcPdoSingleInterfaceChannelManager pdo_manager; pdo_manager.pdo_type = ethercat_interface::PdoType::RPDO; pdo_manager.load_from_config(config); @@ -54,20 +59,20 @@ TEST(TestEcPdoChannelManager, EcReadS16) ASSERT_EQ(pdo_manager.ec_read(buffer), 2 * 42 + 10); } -TEST(TestEcPdoChannelManager, EcReadWriteBit2) +TEST(TestEcPdoSingleInterfaceChannelManager, 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; + ethercat_interface::EcPdoSingleInterfaceChannelManager 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); + ASSERT_EQ(pdo_manager.data_type(), "bit2"); + ASSERT_EQ(pdo_manager.mask, 3); + ASSERT_EQ(ethercat_interface::type2bits(pdo_manager.data_type()), 2); uint8_t buffer[1]; EC_WRITE_U8(buffer, 0); @@ -78,27 +83,28 @@ TEST(TestEcPdoChannelManager, EcReadWriteBit2) ASSERT_EQ(pdo_manager.ec_read(buffer), 1); pdo_manager.ec_write(buffer, 0); - ASSERT_EQ(EC_READ_U8(buffer), 0); + ASSERT_EQ(EC_READ_U8(buffer), 4); pdo_manager.ec_write(buffer, 2); - ASSERT_EQ(EC_READ_U8(buffer), 2); + ASSERT_EQ(EC_READ_U8(buffer), 6); + EC_WRITE_U8(buffer, 0); pdo_manager.ec_write(buffer, 5); ASSERT_EQ(EC_READ_U8(buffer), 1); } -TEST(TestEcPdoChannelManager, EcReadWriteBoolMask1) +TEST(TestEcPdoSingleInterfaceChannelManager, 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; + ethercat_interface::EcPdoSingleInterfaceChannelManager 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); + ASSERT_EQ(pdo_manager.data_type(), "bool"); + ASSERT_EQ(pdo_manager.mask, 1); + ASSERT_EQ(ethercat_interface::type2bits(pdo_manager.data_type()), 1); uint8_t buffer[1]; EC_WRITE_U8(buffer, 3); @@ -112,33 +118,270 @@ TEST(TestEcPdoChannelManager, EcReadWriteBoolMask1) ASSERT_EQ(EC_READ_U8(buffer), 1); } -TEST(TestEcPdoChannelManager, EcReadWriteBoolMask5) +TEST(TestEcPdoSingleInterfaceChannelManager, EcReadWriteBit8Mask5) { const char channel_config[] = R"( - {index: 0x6071, sub_index: 0, type: bool, mask: 5} + {index: 0x6071, sub_index: 0, type: bit8, mask: 5} )"; YAML::Node config = YAML::Load(channel_config); - ethercat_interface::EcPdoChannelManager pdo_manager; + ethercat_interface::EcPdoSingleInterfaceChannelManager 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); + ASSERT_EQ(pdo_manager.data_type(), "bit8"); + ASSERT_EQ(pdo_manager.mask, 5); // < Set mask 0b00000101 + ASSERT_EQ(ethercat_interface::type2bits(pdo_manager.data_type()), 8); uint8_t buffer[1]; - EC_WRITE_U8(buffer, 7); - ASSERT_EQ(pdo_manager.ec_read(buffer), 1); + // Should only soft read the bit 5 and 1 that is both in the mask and in the buffer + EC_WRITE_U8(buffer, 7); // < Hard write 0b00000111 + ASSERT_EQ(pdo_manager.ec_read(buffer), 5); + + // Hard write 0, should soft read 0 EC_WRITE_U8(buffer, 0); ASSERT_EQ(pdo_manager.ec_read(buffer), 0); + // Soft write 0, should hard read 0 pdo_manager.ec_write(buffer, 0); ASSERT_EQ(EC_READ_U8(buffer), 0); + + // Soft write 3 (with mask applied is 1) should hard read 0b00000001 pdo_manager.ec_write(buffer, 3); ASSERT_EQ(EC_READ_U8(buffer), 1); + + // Soft write 7 (with mask applied is 5) should hard read 0b00000101 pdo_manager.ec_write(buffer, 7); ASSERT_EQ(EC_READ_U8(buffer), 5); + + // Soft write 5 (with mask applied is 5) should hard read 0b00000101 pdo_manager.ec_write(buffer, 5); ASSERT_EQ(EC_READ_U8(buffer), 5); } + + +// This test is very weird, the expected behaviour is somehow confusing +// since it writes the entire octet, but read only the bits set to 1 in the mask +// and converts the result to 1 or 0 (1 if at least one bit is set to 1) +TEST(TestEcPdoSingleInterfaceChannelManager, EcReadWriteBoolMask5) +{ + const char channel_config[] = + R"( + {index: 0x6071, sub_index: 0, type: bool, mask: 5} + )"; + YAML::Node config = YAML::Load(channel_config); + ethercat_interface::EcPdoSingleInterfaceChannelManager pdo_manager; + pdo_manager.pdo_type = ethercat_interface::PdoType::RPDO; + ASSERT_EQ(pdo_manager.load_from_config(config), false); + + return; + // ASSERT_EQ(pdo_manager.data_type(), "bool"); + // ASSERT_EQ(pdo_manager.mask, 5); // < Set mask 0b00000101 + // ASSERT_EQ(ethercat_interface::type2bits(pdo_manager.data_type()), 1); + + // uint8_t buffer[1]; + // // Should only soft read the bit 1 that is both in the mask and in the buffer + // EC_WRITE_U8(buffer, 7); // < Hard write 0b00000111 + // ASSERT_EQ(pdo_manager.ec_read(buffer), 1); + + // // Hard write 0, should soft read 0 + // EC_WRITE_U8(buffer, 0); + // ASSERT_EQ(pdo_manager.ec_read(buffer), 0); + + // // Soft write 0, should hard read 0 + // pdo_manager.ec_write(buffer, 0); + // ASSERT_EQ(EC_READ_U8(buffer), 0); + + // // Soft write 3 (with mask applied is 1) should hard read 0b00000001 + // pdo_manager.ec_write(buffer, 3); + // ASSERT_EQ(EC_READ_U8(buffer), 1); + + // // Soft write 7 (with mask applied is 5) should hard read 0b00000101 + // pdo_manager.ec_write(buffer, 7); + // ASSERT_EQ(EC_READ_U8(buffer), 5); + + // // Soft write 5 (with mask applied is 5) should hard read 0b00000101 + // pdo_manager.ec_write(buffer, 5); + // ASSERT_EQ(EC_READ_U8(buffer), 5); +} + + +TEST(TestEcPdoGroupInterfaceChannelManager, LoadConfigTest) +{ + const char channel_config[] = + R"( + { + index: 0xf788, + sub_index: 0x00, + type: bit240, + data_mapping: [ + { + addr_offset: 60, + type: int32, + factor: 3.14, + offset: 2.71, + command_interface: effort + }, + { + addr_offset: 64, + type: int16, + factor: 1.1, + offset: 0.1, + state_interface: position + }, + { + addr_offset: 66, + type: uint8, + mask: 7, + }, + { + addr_offset: 67, + type: bool, + mask: 8, + } + ] + } + )"; + YAML::Node config = YAML::Load(channel_config); + ethercat_interface::EcPdoGroupInterfaceChannelManager pdo_manager; + pdo_manager.pdo_type = ethercat_interface::PdoType::RPDO; + ASSERT_EQ(pdo_manager.load_from_config(config), true); + + ASSERT_EQ(pdo_manager.number_of_interfaces(), 5); + ASSERT_EQ(pdo_manager.number_of_managed_interfaces(), 2); + + ASSERT_EQ(pdo_manager.data_type(0), "bit240"); + ASSERT_EQ(pdo_manager.interface_name(0), "null"); + ASSERT_EQ(pdo_manager.index, 0xf788); + ASSERT_EQ(pdo_manager.sub_index, 0); + + ASSERT_EQ(pdo_manager.data_type(1), "int32"); + ASSERT_EQ(pdo_manager.interface_name(1), "effort"); + ASSERT_EQ(pdo_manager.data(1).factor, 3.14); + ASSERT_EQ(pdo_manager.data(1).offset, 2.71); + ASSERT_EQ(pdo_manager.v_data[1].addr_offset, 60); + + ASSERT_EQ(pdo_manager.data_type(2), "int16"); + ASSERT_EQ(pdo_manager.interface_name(2), "position"); + ASSERT_EQ(pdo_manager.data(2).factor, 1.1); + ASSERT_EQ(pdo_manager.data(2).offset, 0.1); + ASSERT_EQ(pdo_manager.v_data[2].addr_offset, 64); + + ASSERT_EQ(pdo_manager.data_type(3), "uint8"); + ASSERT_EQ(pdo_manager.interface_name(3), "null"); + ASSERT_EQ(pdo_manager.data(3).mask, 7); + ASSERT_EQ(pdo_manager.v_data[3].addr_offset, 66); + + ASSERT_EQ(pdo_manager.data_type(4), "bool"); + ASSERT_EQ(pdo_manager.interface_name(4), "null"); + ASSERT_EQ(pdo_manager.data(4).mask, 8); +} + + +TEST(TestEcPdoGroupInterfaceChannelManager, ReadWriteBits) +{ + const char channel_config[] = + R"( + { + index: 0x6071, + sub_index: 0x00, + type: bit8, + data_mapping: [ + { + type: bool, + mask: 1, + command_interface: input1 + }, + { + type: bool, + mask: 2, + state_interface: output1 + }, + { + type: bool, + mask: 4, + command_interface: input2 + }, + { + type: bool, + mask: 8, + state_interface: output2 + }, + { + type: bool, + mask: 16, + command_interface: input3 + }, + { + type: bool, + mask: 32, + state_interface: output3 + } + ] + } + )"; + YAML::Node config = YAML::Load(channel_config); + ethercat_interface::EcPdoGroupInterfaceChannelManager pdo_manager; + pdo_manager.pdo_type = ethercat_interface::PdoType::RPDO; + ASSERT_EQ(pdo_manager.load_from_config(config), true); + + ASSERT_EQ(pdo_manager.number_of_interfaces(), 7); + ASSERT_EQ(pdo_manager.number_of_managed_interfaces(), 6); + + ASSERT_EQ(pdo_manager.data_type(0), "bit8"); + ASSERT_EQ(pdo_manager.interface_name(0), "null"); + ASSERT_EQ(pdo_manager.index, 0x6071); + ASSERT_EQ(pdo_manager.sub_index, 0); + + ASSERT_EQ(pdo_manager.data_type(1), "bool"); + ASSERT_EQ(pdo_manager.interface_name(1), "input1"); + ASSERT_EQ(pdo_manager.data(1).mask, 1); + ASSERT_EQ(pdo_manager.v_data[1].addr_offset, 0); + + ASSERT_EQ(pdo_manager.data_type(2), "bool"); + ASSERT_EQ(pdo_manager.interface_name(2), "output1"); + ASSERT_EQ(pdo_manager.data(2).mask, 2); + ASSERT_EQ(pdo_manager.v_data[2].addr_offset, 0); + + ASSERT_EQ(pdo_manager.data_type(3), "bool"); + ASSERT_EQ(pdo_manager.interface_name(3), "input2"); + ASSERT_EQ(pdo_manager.data(3).mask, 4); + ASSERT_EQ(pdo_manager.v_data[3].addr_offset, 0); + + ASSERT_EQ(pdo_manager.data_type(4), "bool"); + ASSERT_EQ(pdo_manager.interface_name(4), "output2"); + ASSERT_EQ(pdo_manager.data(4).mask, 8); + ASSERT_EQ(pdo_manager.v_data[4].addr_offset, 0); + + ASSERT_EQ(pdo_manager.data_type(5), "bool"); + ASSERT_EQ(pdo_manager.interface_name(5), "input3"); + ASSERT_EQ(pdo_manager.data(5).mask, 16); + ASSERT_EQ(pdo_manager.v_data[5].addr_offset, 0); + + ASSERT_EQ(pdo_manager.data_type(6), "bool"); + ASSERT_EQ(pdo_manager.interface_name(6), "output3"); + ASSERT_EQ(pdo_manager.data(6).mask, 32); + ASSERT_EQ(pdo_manager.v_data[6].addr_offset, 0); + + uint8_t buffer[1]; + std::vector write_tests0 = + { + 0, 1, 2, 4, 8, 16, 32, + 0b00111111, + 0b11000000, + 0b11111111, + 0b00101010, + 0b11010101, + 0b00001111 + }; + + for (size_t n = 0; n < write_tests0.size(); ++n) { + EC_WRITE_U8(buffer, write_tests0[n]); + ASSERT_EQ(pdo_manager.ec_read(buffer), write_tests0[n]); + + std::bitset<8> bits(write_tests0[n]); + for (size_t i = 1; i < 7; i++) { + ASSERT_EQ(pdo_manager.ec_read(buffer, i), bits.test(i - 1)); + } + } +} diff --git a/ethercat_manager/src/ethercat_sdo_srv_server.cpp b/ethercat_manager/src/ethercat_sdo_srv_server.cpp index 24a70132..4ff3d75b 100644 --- a/ethercat_manager/src/ethercat_sdo_srv_server.cpp +++ b/ethercat_manager/src/ethercat_sdo_srv_server.cpp @@ -13,6 +13,7 @@ // limitations under the License. // // Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) +// Author: Manuel YGUEL (yguel.robotics@gmail.com) #include #include @@ -24,7 +25,6 @@ #include "ethercat_manager/ec_master_async.hpp" #include "ethercat_manager/data_convertion_tools.hpp" - namespace ethercat_manager { void upload( @@ -60,6 +60,9 @@ void upload( response->success = false; RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); response->sdo_return_message = return_stream.str(); + if (nullptr != data.target) { + delete[] data.target; + } return; } @@ -70,6 +73,9 @@ void upload( response->success = false; RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); response->sdo_return_message = return_stream.str(); + if (nullptr != data.target) { + delete[] data.target; + } return; } @@ -78,11 +84,13 @@ void upload( 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(); + if (nullptr != data.target) { + delete[] data.target; + } return; } return_stream << "SDO upload done successfully"; @@ -91,7 +99,9 @@ void upload( response->sdo_return_value = data_value; response->sdo_return_message = return_stream.str(); - delete[] data.target; + if (nullptr != data.target) { + delete[] data.target; + } RCLCPP_INFO(rclcpp::get_logger("ethercat_sdo_srv_server"), return_stream.str().c_str()); } @@ -122,18 +132,22 @@ void download( 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()); + if (nullptr != data.data) { + delete[] data.data; + } 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()); + if (nullptr != data.data) { + delete[] data.data; + } return; } @@ -145,6 +159,9 @@ void download( response->success = false; RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); response->sdo_return_message = return_stream.str(); + if (nullptr != data.data) { + delete[] data.data; + } return; } @@ -155,6 +172,9 @@ void download( response->success = false; RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); response->sdo_return_message = return_stream.str(); + if (nullptr != data.data) { + delete[] data.data; + } return; } @@ -164,12 +184,13 @@ void download( response->success = true; response->sdo_return_message = return_stream.str(); - delete[] data.data; + if (nullptr != data.data) { + 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); From 6365a110b592abd56b9c1eb3c3f86554b3012d1f Mon Sep 17 00:00:00 2001 From: Manuel YGUEL Date: Fri, 7 Jun 2024 11:37:33 +0200 Subject: [PATCH 02/11] Fix memory leak in some catch clauses of exception handling. --- .../src/generic_ec_slave.cpp | 12 ++++--- .../src/ethercat_sdo_srv_server.cpp | 35 +++++++++++++++---- 2 files changed, 36 insertions(+), 11 deletions(-) diff --git a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp index 3663d726..5bc68cc3 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp +++ b/ethercat_generic_plugins/ethercat_generic_slave/src/generic_ec_slave.cpp @@ -68,19 +68,23 @@ 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, (unsigned int)(rpdos_.size()), rpdos_.data(), + syncs_.push_back( + {2, EC_DIR_OUTPUT, (unsigned int)(rpdos_.size()), rpdos_.data(), EC_WD_ENABLE}); - syncs_.push_back({3, EC_DIR_INPUT, (unsigned int)(tpdos_.size()), tpdos_.data(), + syncs_.push_back( + {3, EC_DIR_INPUT, (unsigned int)(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, (unsigned int)(rpdos_.size()), rpdos_.data(), + syncs_.push_back( + {sm.index, sm.type, (unsigned int)(rpdos_.size()), rpdos_.data(), sm.watchdog}); } else if (sm.pdo_name == "tpdo") { - syncs_.push_back({sm.index, sm.type, (unsigned int)(tpdos_.size()), tpdos_.data(), + syncs_.push_back( + {sm.index, sm.type, (unsigned int)(tpdos_.size()), tpdos_.data(), sm.watchdog}); } } diff --git a/ethercat_manager/src/ethercat_sdo_srv_server.cpp b/ethercat_manager/src/ethercat_sdo_srv_server.cpp index 24a70132..4ff3d75b 100644 --- a/ethercat_manager/src/ethercat_sdo_srv_server.cpp +++ b/ethercat_manager/src/ethercat_sdo_srv_server.cpp @@ -13,6 +13,7 @@ // limitations under the License. // // Author: Maciej Bednarczyk (mcbed.robotics@gmail.com) +// Author: Manuel YGUEL (yguel.robotics@gmail.com) #include #include @@ -24,7 +25,6 @@ #include "ethercat_manager/ec_master_async.hpp" #include "ethercat_manager/data_convertion_tools.hpp" - namespace ethercat_manager { void upload( @@ -60,6 +60,9 @@ void upload( response->success = false; RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); response->sdo_return_message = return_stream.str(); + if (nullptr != data.target) { + delete[] data.target; + } return; } @@ -70,6 +73,9 @@ void upload( response->success = false; RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); response->sdo_return_message = return_stream.str(); + if (nullptr != data.target) { + delete[] data.target; + } return; } @@ -78,11 +84,13 @@ void upload( 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(); + if (nullptr != data.target) { + delete[] data.target; + } return; } return_stream << "SDO upload done successfully"; @@ -91,7 +99,9 @@ void upload( response->sdo_return_value = data_value; response->sdo_return_message = return_stream.str(); - delete[] data.target; + if (nullptr != data.target) { + delete[] data.target; + } RCLCPP_INFO(rclcpp::get_logger("ethercat_sdo_srv_server"), return_stream.str().c_str()); } @@ -122,18 +132,22 @@ void download( 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()); + if (nullptr != data.data) { + delete[] data.data; + } 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()); + if (nullptr != data.data) { + delete[] data.data; + } return; } @@ -145,6 +159,9 @@ void download( response->success = false; RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); response->sdo_return_message = return_stream.str(); + if (nullptr != data.data) { + delete[] data.data; + } return; } @@ -155,6 +172,9 @@ void download( response->success = false; RCLCPP_ERROR(rclcpp::get_logger("ethercat_manager"), return_stream.str().c_str()); response->sdo_return_message = return_stream.str(); + if (nullptr != data.data) { + delete[] data.data; + } return; } @@ -164,12 +184,13 @@ void download( response->success = true; response->sdo_return_message = return_stream.str(); - delete[] data.data; + if (nullptr != data.data) { + 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); From 1d054e1cf426670b421a8fcb645c66be4fbc9e42 Mon Sep 17 00:00:00 2001 From: Manuel YGUEL Date: Thu, 25 Sep 2025 12:48:00 +0200 Subject: [PATCH 03/11] Solve formatting problems. --- .pre-commit-config.yaml | 18 +++++++++--------- ethercat_driver/CMakeLists.txt | 19 +++++++++++++++++++ .../developer_guide/domain_construction.rst | 7 ++++--- .../sphinx/developer_guide/safety.rst | 4 ++-- .../ethercat_generic_cia402_drive/package.xml | 2 +- .../ethercat_generic_slave/package.xml | 2 +- 6 files changed, 36 insertions(+), 16 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 2abcec7c..a6e5a575 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -15,7 +15,7 @@ repos: # Standard hooks - repo: https://github.com/pre-commit/pre-commit-hooks - rev: v4.4.0 + rev: v6.0.0 hooks: - id: check-added-large-files - id: check-ast @@ -35,7 +35,7 @@ repos: # Python hooks - repo: https://github.com/asottile/pyupgrade - rev: v3.3.1 + rev: v3.20.0 hooks: - id: pyupgrade args: [--py36-plus] @@ -48,7 +48,7 @@ repos: args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D404"] - repo: https://github.com/pycqa/flake8 - rev: 6.0.0 + rev: 7.3.0 hooks: - id: flake8 args: ["--extend-ignore=E501"] @@ -59,7 +59,7 @@ repos: - id: ament_uncrustify name: ament_uncrustify description: Uncrustify. - stages: [commit] + stages: [pre-commit] entry: ament_uncrustify language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ @@ -92,7 +92,7 @@ repos: - id: ament_cpplint name: ament_cpplint description: Static code analysis of C/C++ files. - stages: [commit] + stages: [pre-commit] entry: ament_cpplint language: system files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ @@ -104,7 +104,7 @@ repos: - id: ament_lint_cmake name: ament_lint_cmake description: Check format of CMakeLists.txt files. - stages: [commit] + stages: [pre-commit] entry: ament_lint_cmake language: system files: CMakeLists\.txt$ @@ -115,13 +115,13 @@ repos: - id: ament_copyright name: ament_copyright description: Check if copyright notice is available in all files. - stages: [commit] + stages: [pre-commit] entry: ament_copyright language: system # Docs - RestructuredText hooks - repo: https://github.com/PyCQA/doc8 - rev: v1.1.1 + rev: v2.0.0 hooks: - id: doc8 args: ['--max-line-length=100', '--ignore=D001'] @@ -138,7 +138,7 @@ repos: # Spellcheck in comments and docs # skipping of *.svg files is not working... - repo: https://github.com/codespell-project/codespell - rev: v2.2.2 + rev: v2.4.1 hooks: - id: codespell args: ['--write-changes'] diff --git a/ethercat_driver/CMakeLists.txt b/ethercat_driver/CMakeLists.txt index 65134a45..fba10ec6 100644 --- a/ethercat_driver/CMakeLists.txt +++ b/ethercat_driver/CMakeLists.txt @@ -102,6 +102,25 @@ if(BUILD_TESTING) # Create a custom target to group the copy commands add_custom_target(copy_safety_config ALL DEPENDS ${CFG_OUT}) + # Install click if not available + execute_process( + COMMAND python3 -c "import click" + RESULT_VARIABLE CLICK_CHECK_RESULT + OUTPUT_QUIET + ERROR_QUIET + ) + + if(NOT CLICK_CHECK_RESULT EQUAL 0) + message(STATUS "Installing click Python package...") + execute_process( + COMMAND python3 -m pip install click + RESULT_VARIABLE CLICK_INSTALL_RESULT + ) + if(NOT CLICK_INSTALL_RESULT EQUAL 0) + message(FATAL_ERROR "Failed to install click package") + endif() + endif() + set(UP_PY ${CMAKE_CURRENT_SOURCE_DIR}/examples/configurations/update_slave_config_path.py) set(UP_IN ${CFG_DST}/safety_estop.ros2_control.template.xacro) set(UP_OUT ${CFG_DST}/safety_estop.ros2_control.xacro) diff --git a/ethercat_driver_ros2/sphinx/developer_guide/domain_construction.rst b/ethercat_driver_ros2/sphinx/developer_guide/domain_construction.rst index cd5cfd43..3a483060 100644 --- a/ethercat_driver_ros2/sphinx/developer_guide/domain_construction.rst +++ b/ethercat_driver_ros2/sphinx/developer_guide/domain_construction.rst @@ -32,14 +32,14 @@ The application has access to objects of type :code:`ec_pdo_entry_reg_t` (IgH ma In this structure the :code:`offset` and :code:`bit_position` are pointers to the offset and bit position of the data in the domain because this structure is given to the master that fill in those values establishing the link for the application with its data. -The application configures its link with the EtherCAT master via separate API calls. +The application configures its link with the EtherCAT master via separate API calls. It first creates domains with: 1. :code:`ec_domain_t * ecrt_master_create_domain(const ec_master_t *master)` Then it provides the master with: -2. the list of slaves that are used in the domain +2. the list of slaves that are used in the domain .. code-block:: cpp - + ec_slave_config_t *ecrt_master_slave_config( ec_master_t *master, /**< EtherCAT master */ uint16_t alias, /**< Slave alias. */ @@ -50,6 +50,7 @@ Then it provides the master with: 3. the list of PDO entries that are used in the domain, via an array of :code:`ec_pdo_entry_reg_t` objects, where offset and bit_position points to the data the application will use and have to be filled by the master: :code:`bool ecrt_domain_reg_pdo_entry_list(ec_domain_t *domain, ec_pdo_entry_reg_t *entries)` 4. It asks for pointer to the start of the domain memory: :code:`uint8_t * ecrt_domain_data(ec_domain_t *domain)` + One domain can be used for read and one for writing data for instance, so the application may make several calls to the above functions to create the domains it needs. ethercat_driver_ros2 data structure organization diff --git a/ethercat_driver_ros2/sphinx/developer_guide/safety.rst b/ethercat_driver_ros2/sphinx/developer_guide/safety.rst index dce8158b..b12e555b 100644 --- a/ethercat_driver_ros2/sphinx/developer_guide/safety.rst +++ b/ethercat_driver_ros2/sphinx/developer_guide/safety.rst @@ -1,13 +1,13 @@ FailSafe over EtherCAT (FSoE) : Safety and the Ethercat Driver for ROS2 ======================================================================= -In order to have safe robotic systems, it is important to have a safety layer that can monitor the system and take action in case of a failure. The FailSafe over EtherCAT (FSoE) protocol is a safety protocol that can be used to monitor the system and take action in case of a failure. +In order to have safe robotic systems, it is important to have a safety layer that can monitor the system and take action in case of a failure. The FailSafe over EtherCAT (FSoE) protocol is a safety protocol that can be used to monitor the system and take action in case of a failure. In order to use the FSoE protocol with ROS2, the Ethercat Driver for ROS2 has been extended to be compatible with EtherCAT communications using the FSoE protocol. This document describes the integration of FSoE communications in the Ethercat Driver for ROS2 and how to use it to monitor the system and take action in case of a failure. In addition to the standard EtherCAT slaves, safety is implemented by adding safety slaves and safety masters to the EtherCAT network, all being «viewed» by the EtherCAT master as standard slaves. -One safety master handles a set of safety slaves creating a safety sub-network within the EtherCAT network. +One safety master handles a set of safety slaves creating a safety sub-network within the EtherCAT network. All the communications between the safety slaves and their respective safety masters are done inside the standard EtherCAT frames and must not be tampered with or an error will be raised. The ethercat_driver_ros2 package has been extended: diff --git a/ethercat_generic_plugins/ethercat_generic_cia402_drive/package.xml b/ethercat_generic_plugins/ethercat_generic_cia402_drive/package.xml index d5f42352..d096eb23 100644 --- a/ethercat_generic_plugins/ethercat_generic_cia402_drive/package.xml +++ b/ethercat_generic_plugins/ethercat_generic_cia402_drive/package.xml @@ -20,4 +20,4 @@ ament_cmake - \ No newline at end of file + diff --git a/ethercat_generic_plugins/ethercat_generic_slave/package.xml b/ethercat_generic_plugins/ethercat_generic_slave/package.xml index ab4bf929..8655060a 100644 --- a/ethercat_generic_plugins/ethercat_generic_slave/package.xml +++ b/ethercat_generic_plugins/ethercat_generic_slave/package.xml @@ -19,4 +19,4 @@ ament_cmake - \ No newline at end of file + From 9ba622a741b966ea01208eefb8174cd6e641041f Mon Sep 17 00:00:00 2001 From: Manuel YGUEL Date: Thu, 25 Sep 2025 12:58:13 +0200 Subject: [PATCH 04/11] Try to solve click dependency for tests. --- .github/workflows/ci.yml | 6 ++++++ ethercat_driver/CMakeLists.txt | 19 ------------------- 2 files changed, 6 insertions(+), 19 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 9b743d03..857c485d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -40,6 +40,12 @@ jobs: rosdep install --ignore-src --from-paths . -y -r && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release + - name: Install Python dependencies + run: | + sudo apt-get update + sudo apt-get install -y python3-pip + python3 -m pip install click + - name: Tests uses: addnab/docker-run-action@v3 with: diff --git a/ethercat_driver/CMakeLists.txt b/ethercat_driver/CMakeLists.txt index fba10ec6..65134a45 100644 --- a/ethercat_driver/CMakeLists.txt +++ b/ethercat_driver/CMakeLists.txt @@ -102,25 +102,6 @@ if(BUILD_TESTING) # Create a custom target to group the copy commands add_custom_target(copy_safety_config ALL DEPENDS ${CFG_OUT}) - # Install click if not available - execute_process( - COMMAND python3 -c "import click" - RESULT_VARIABLE CLICK_CHECK_RESULT - OUTPUT_QUIET - ERROR_QUIET - ) - - if(NOT CLICK_CHECK_RESULT EQUAL 0) - message(STATUS "Installing click Python package...") - execute_process( - COMMAND python3 -m pip install click - RESULT_VARIABLE CLICK_INSTALL_RESULT - ) - if(NOT CLICK_INSTALL_RESULT EQUAL 0) - message(FATAL_ERROR "Failed to install click package") - endif() - endif() - set(UP_PY ${CMAKE_CURRENT_SOURCE_DIR}/examples/configurations/update_slave_config_path.py) set(UP_IN ${CFG_DST}/safety_estop.ros2_control.template.xacro) set(UP_OUT ${CFG_DST}/safety_estop.ros2_control.xacro) From c901501e73c1b70f5880b014b85f5e47aa1b16aa Mon Sep 17 00:00:00 2001 From: Manuel YGUEL Date: Thu, 25 Sep 2025 15:24:12 +0200 Subject: [PATCH 05/11] Fix python import. --- .github/workflows/ci.yml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 857c485d..7e2bbc17 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -29,6 +29,12 @@ jobs: file: .docker/Dockerfile push: false + - name: Install Python dependencies + run: | + sudo apt-get update + sudo apt-get install -y python3-pip + python3 -m pip install click + - name: Build uses: addnab/docker-run-action@v3 with: @@ -40,12 +46,6 @@ jobs: rosdep install --ignore-src --from-paths . -y -r && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release - - name: Install Python dependencies - run: | - sudo apt-get update - sudo apt-get install -y python3-pip - python3 -m pip install click - - name: Tests uses: addnab/docker-run-action@v3 with: From 7945d1f37a029e026b089be7b7f214cfe5273e88 Mon Sep 17 00:00:00 2001 From: Manuel YGUEL Date: Thu, 25 Sep 2025 15:33:55 +0200 Subject: [PATCH 06/11] Fix python import. --- .github/workflows/ci.yml | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 7e2bbc17..917f7c40 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -29,12 +29,6 @@ jobs: file: .docker/Dockerfile push: false - - name: Install Python dependencies - run: | - sudo apt-get update - sudo apt-get install -y python3-pip - python3 -m pip install click - - name: Build uses: addnab/docker-run-action@v3 with: @@ -42,6 +36,9 @@ jobs: options: -v ${{github.workspace}}/:/ros/ run: | cd /ros + # Install click inside the container + apt-get update && apt-get install -y python3-pip + python3 -m pip install click . /opt/ros/jazzy/setup.sh rosdep install --ignore-src --from-paths . -y -r && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release From d48ed114d29d0d47cc78516085a81e45e59239a4 Mon Sep 17 00:00:00 2001 From: Manuel YGUEL Date: Thu, 25 Sep 2025 15:40:18 +0200 Subject: [PATCH 07/11] Fix python import. --- .github/workflows/ci.yml | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 917f7c40..0587647d 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -36,9 +36,9 @@ jobs: options: -v ${{github.workspace}}/:/ros/ run: | cd /ros - # Install click inside the container - apt-get update && apt-get install -y python3-pip - python3 -m pip install click + # Install click, try via apt first, fallback to pip with --break-system-packages + apt-get update + apt-get install -y python3-click || (apt-get install -y python3-pip && python3 -m pip install --break-system-packages click) . /opt/ros/jazzy/setup.sh rosdep install --ignore-src --from-paths . -y -r && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release @@ -50,6 +50,9 @@ jobs: options: -v ${{github.workspace}}/:/ros/ run: | cd /ros + # Install click, try via apt first, fallback to pip with --break-system-packages + apt-get update + apt-get install -y python3-click || (apt-get install -y python3-pip && python3 -m pip install --break-system-packages click) . /opt/ros/jazzy/setup.sh rosdep install --ignore-src --from-paths . -y -r && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release From f710d0de72a4a73cab3c967682a449135c36eb19 Mon Sep 17 00:00:00 2001 From: Manuel YGUEL Date: Thu, 25 Sep 2025 15:47:50 +0200 Subject: [PATCH 08/11] Fix yaml-cpp build --- .github/workflows/ci.yml | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 0587647d..5ad64970 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -39,6 +39,8 @@ jobs: # Install click, try via apt first, fallback to pip with --break-system-packages apt-get update apt-get install -y python3-click || (apt-get install -y python3-pip && python3 -m pip install --break-system-packages click) + # Install yaml-cpp + apt-get install -y libyaml-cpp-dev . /opt/ros/jazzy/setup.sh rosdep install --ignore-src --from-paths . -y -r && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release @@ -53,6 +55,8 @@ jobs: # Install click, try via apt first, fallback to pip with --break-system-packages apt-get update apt-get install -y python3-click || (apt-get install -y python3-pip && python3 -m pip install --break-system-packages click) + # Install yaml-cpp + apt-get install -y libyaml-cpp-dev . /opt/ros/jazzy/setup.sh rosdep install --ignore-src --from-paths . -y -r && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release From 5fc2ab679c973e071f0f33cd0bf34095b3d0791c Mon Sep 17 00:00:00 2001 From: Manuel YGUEL Date: Thu, 25 Sep 2025 15:54:01 +0200 Subject: [PATCH 09/11] Fix yaml-cpp build --- .github/workflows/ci.yml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 5ad64970..06a1d556 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -40,7 +40,7 @@ jobs: apt-get update apt-get install -y python3-click || (apt-get install -y python3-pip && python3 -m pip install --break-system-packages click) # Install yaml-cpp - apt-get install -y libyaml-cpp-dev + apt-get install -y ros-jazzy-yaml-cpp-vendor . /opt/ros/jazzy/setup.sh rosdep install --ignore-src --from-paths . -y -r && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release @@ -56,7 +56,7 @@ jobs: apt-get update apt-get install -y python3-click || (apt-get install -y python3-pip && python3 -m pip install --break-system-packages click) # Install yaml-cpp - apt-get install -y libyaml-cpp-dev + apt-get install -y ros-jazzy-yaml-cpp-vendor . /opt/ros/jazzy/setup.sh rosdep install --ignore-src --from-paths . -y -r && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release From 8f85a1a2f5c990240577961616e6f0944c442bc4 Mon Sep 17 00:00:00 2001 From: Manuel YGUEL Date: Thu, 25 Sep 2025 17:33:28 +0200 Subject: [PATCH 10/11] Fix yaml-cpp build --- .github/workflows/ci.yml | 8 ++++---- ethercat_driver/CMakeLists.txt | 15 +++++++++++++++ 2 files changed, 19 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yml b/.github/workflows/ci.yml index 06a1d556..eb357e1b 100644 --- a/.github/workflows/ci.yml +++ b/.github/workflows/ci.yml @@ -39,8 +39,8 @@ jobs: # Install click, try via apt first, fallback to pip with --break-system-packages apt-get update apt-get install -y python3-click || (apt-get install -y python3-pip && python3 -m pip install --break-system-packages click) - # Install yaml-cpp - apt-get install -y ros-jazzy-yaml-cpp-vendor + # Install both system and ROS2 yaml-cpp packages + apt-get install -y libyaml-cpp-dev ros-jazzy-yaml-cpp-vendor pkg-config . /opt/ros/jazzy/setup.sh rosdep install --ignore-src --from-paths . -y -r && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release @@ -55,8 +55,8 @@ jobs: # Install click, try via apt first, fallback to pip with --break-system-packages apt-get update apt-get install -y python3-click || (apt-get install -y python3-pip && python3 -m pip install --break-system-packages click) - # Install yaml-cpp - apt-get install -y ros-jazzy-yaml-cpp-vendor + # Install both system and ROS2 yaml-cpp packages + apt-get install -y libyaml-cpp-dev ros-jazzy-yaml-cpp-vendor pkg-config . /opt/ros/jazzy/setup.sh rosdep install --ignore-src --from-paths . -y -r && \ colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release diff --git a/ethercat_driver/CMakeLists.txt b/ethercat_driver/CMakeLists.txt index 65134a45..9a6a4e19 100644 --- a/ethercat_driver/CMakeLists.txt +++ b/ethercat_driver/CMakeLists.txt @@ -16,6 +16,10 @@ find_package(rclcpp_lifecycle REQUIRED) find_package(ethercat_interface REQUIRED) find_package(yaml_cpp_vendor REQUIRED) +# Find system yaml-cpp for proper linking +find_package(PkgConfig REQUIRED) +pkg_check_modules(YAML_CPP REQUIRED yaml-cpp) + file(GLOB_RECURSE PLUGINS_SRC src/*.cpp) add_library(${PROJECT_NAME} ${PLUGINS_SRC}) @@ -37,6 +41,11 @@ ament_target_dependencies( yaml_cpp_vendor ) +# Explicitly link yaml-cpp +target_link_libraries(${PROJECT_NAME} ${YAML_CPP_LIBRARIES}) +target_include_directories(${PROJECT_NAME} PRIVATE ${YAML_CPP_INCLUDE_DIRS}) +target_compile_options(${PROJECT_NAME} PRIVATE ${YAML_CPP_CFLAGS_OTHER}) + # Causes the visibility macros to use dllexport rather than dllimport, # which is appropriate when building the dll but not consuming it. target_compile_definitions(${PROJECT_NAME} PRIVATE "ETHERCAT_DRIVER_BUILDING_LIBRARY") @@ -125,7 +134,9 @@ if(BUILD_TESTING) target_link_libraries(test_ethercat_safety_driver ${PROJECT_NAME} + ${YAML_CPP_LIBRARIES} ) + ament_target_dependencies(test_ethercat_safety_driver hardware_interface pluginlib @@ -134,6 +145,10 @@ if(BUILD_TESTING) ethercat_interface yaml_cpp_vendor ) + + # Add yaml-cpp to test target as well + target_include_directories(test_ethercat_safety_driver PRIVATE ${YAML_CPP_INCLUDE_DIRS}) + target_compile_options(test_ethercat_safety_driver PRIVATE ${YAML_CPP_CFLAGS_OTHER}) endif() ## EXPORTS From f89f24fc54c7f9b4ef4ac39ac9a58c9b16462dd8 Mon Sep 17 00:00:00 2001 From: Manuel YGUEL Date: Fri, 26 Sep 2025 19:00:39 +0200 Subject: [PATCH 11/11] Fix python flake8 CI problem --- .../configurations/update_slave_config_path.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/ethercat_driver/examples/configurations/update_slave_config_path.py b/ethercat_driver/examples/configurations/update_slave_config_path.py index da065bd7..947e35cb 100644 --- a/ethercat_driver/examples/configurations/update_slave_config_path.py +++ b/ethercat_driver/examples/configurations/update_slave_config_path.py @@ -14,10 +14,11 @@ # Executable python3 script to update the configuration of a slave device -from lxml import etree import os import sys + import click +from lxml import etree def prepend_slave_config_xml_tag(xml_file, prepend_path, output_file=None): @@ -32,13 +33,13 @@ def prepend_slave_config_xml_tag(xml_file, prepend_path, output_file=None): find_all_param = tree.findall('.//param') for param in find_all_param: if param.attrib.get('name') == 'slave_config': - print(f"Found slave_config path: {param.text}") + print(f'Found slave_config path: {param.text}') param.text = os.path.join(prepend_path, param.text) - print(f"Updated slave_config path: {param.text}") + print(f'Updated slave_config path: {param.text}') if param.attrib.get('name') == 'safety': - print(f"Found safety path: {param.text}") + print(f'Found safety path: {param.text}') param.text = os.path.join(prepend_path, param.text) - print(f"Updated safety path: {param.text}") + print(f'Updated safety path: {param.text}') if output_file: tree.write(output_file, pretty_print=True, xml_declaration=True, encoding='UTF-8') @@ -55,11 +56,11 @@ def prepend_slave_config_xml_tag(xml_file, prepend_path, output_file=None): help='Output file', default=None) def main(xml_file, prepend_path, output_file): prepend_slave_config_xml_tag(xml_file, prepend_path, output_file) - msg = f"Updated the slave config path in {xml_file} with {prepend_path}" + msg = f'Updated the slave config path in {xml_file} with {prepend_path}' if output_file: - msg += f" and saved to {output_file}" + msg += f' and saved to {output_file}' else: - msg += f" and saved to {xml_file}" + msg += f' and saved to {xml_file}' print(msg) return 0