├── .circleci └── config.yml ├── .gitignore ├── CODEOWNERS ├── LICENSE ├── README.md ├── aws.yml ├── doc ├── RosEtherCAT.dia └── RosEtherCAT.png ├── repository.rosinstall ├── ros_ethercat ├── CHANGELOG.rst ├── CMakeLists.txt └── package.xml ├── ros_ethercat_hardware ├── CHANGELOG.rst ├── CMakeLists.txt ├── cmake │ └── ros_ethercat_hardware-extras.cmake.em ├── include │ └── ros_ethercat_hardware │ │ ├── copyright_exclusions.cfg │ │ ├── ethercat_com.h │ │ ├── ethercat_device.h │ │ ├── ethercat_hardware.h │ │ └── ethernet_interface_info.h ├── mainpage.dox ├── package.xml └── src │ ├── copyright_exclusions.cfg │ ├── ethercat_com.cpp │ ├── ethercat_device.cpp │ ├── ethercat_hardware.cpp │ └── ethernet_interface_info.cpp ├── ros_ethercat_loop ├── CHANGELOG.rst ├── CMakeLists.txt ├── package.xml └── src │ ├── copyright_exclusions.cfg │ └── main.cpp └── ros_ethercat_model ├── CHANGELOG.rst ├── CMakeLists.txt ├── cmake └── ros_ethercat_model-extras.cmake.em ├── ethercat_robot_hw_plugin.xml ├── include └── ros_ethercat_model │ ├── chain.hpp │ ├── copyright_exclusions.cfg │ ├── hardware_interface.hpp │ ├── imu_state.hpp │ ├── joint.hpp │ ├── mech_stats_publisher.hpp │ ├── robot_state.hpp │ ├── robot_state_interface.hpp │ ├── ros_ethercat.hpp │ ├── transmission.hpp │ └── tree.hpp ├── launch ├── joint_state_controller.yaml └── joint_state_publisher.launch ├── package.xml └── src └── ros_ethercat.cpp /.circleci/config.yml: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # Copyright © 2022 belongs to Shadow Robot Company Ltd. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without modification, 6 | # are permitted provided that the following conditions are met: 7 | # 1. Redistributions of source code must retain the above copyright notice, 8 | # this list of conditions and the following disclaimer. 9 | # 2. Redistributions in binary form must reproduce the above copyright notice, 10 | # this list of conditions and the following disclaimer in the documentation 11 | # and/or other materials provided with the distribution. 12 | # 3. Neither the name of Shadow Robot Company Ltd nor the names of its contributors 13 | # may be used to endorse or promote products derived from this software without 14 | # specific prior written permission. 15 | # 16 | # This software is provided by Shadow Robot Company Ltd "as is" and any express 17 | # or implied warranties, including, but not limited to, the implied warranties of 18 | # merchantability and fitness for a particular purpose are disclaimed. In no event 19 | # shall the copyright holder be liable for any direct, indirect, incidental, special, 20 | # exemplary, or consequential damages (including, but not limited to, procurement of 21 | # substitute goods or services; loss of use, data, or profits; or business interruption) 22 | # however caused and on any theory of liability, whether in contract, strict liability, 23 | # or tort (including negligence or otherwise) arising in any way out of the use of this 24 | # software, even if advised of the possibility of such damage. 25 | 26 | version: 2 27 | jobs: 28 | build: 29 | working_directory: /tmp/repository 30 | docker: 31 | - image: public.ecr.aws/shadowrobot/build-tools:focal-noetic 32 | environment: 33 | toolset_branch: lint 34 | server_type: circle 35 | ros_release_name: noetic 36 | ubuntu_version_name: focal 37 | used_modules: check_cache,code_style_check 38 | steps: 39 | - checkout 40 | - run: echo 'export remote_shell_script="https://raw.githubusercontent.com/shadow-robot/sr-build-tools/$toolset_branch/bin/sr-run-ci-build.sh"' >> $BASH_ENV 41 | - run: wget -O /tmp/oneliner "$( echo "$remote_shell_script" | sed 's/#/%23/g' )" 42 | - run: chown -R $MY_USERNAME:$MY_USERNAME $CIRCLE_WORKING_DIRECTORY 43 | - run: chmod 755 /tmp/oneliner 44 | - run: gosu $MY_USERNAME /tmp/oneliner "$toolset_branch" $server_type $used_modules 45 | 46 | - store_test_results: 47 | path: test_results 48 | 49 | - store_artifacts: 50 | path: code_coverage_results 51 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | 6 | # Compiled Dynamic libraries 7 | *.so 8 | *.dylib 9 | 10 | # Compiled Static libraries 11 | *.lai 12 | *.la 13 | *.a 14 | 15 | CATKIN_IGNORE 16 | *~ 17 | 18 | # PyCharm files 19 | .idea 20 | -------------------------------------------------------------------------------- /CODEOWNERS: -------------------------------------------------------------------------------- 1 | * @shadow-robot/alpha_shadow @shadow-robot/omega_shadow @toliver 2 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 2-Clause License 2 | 3 | Copyright (c) 2019, The Shadow Robot Company 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | * Redistributions in binary form must reproduce the above copyright notice, 13 | this list of conditions and the following disclaimer in the documentation 14 | and/or other materials provided with the distribution. 15 | 16 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 17 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 18 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 19 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 20 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 21 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 22 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 23 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 24 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 25 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ros_ethercat 2 | ------------ 3 | ## CI Statuses 4 | 5 | Check | Status 6 | ---|--- 7 | Build|[](https://eu-west-2.console.aws.amazon.com/codesuite/codebuild/projects/auto_ros_ethercat_noetic-devel_install_check/) 8 | Style|[](https://eu-west-2.console.aws.amazon.com/codesuite/codebuild/projects/auto_ros_ethercat_noetic-devel_style_check/) 9 | Code Coverage|[](https://eu-west-2.console.aws.amazon.com/codesuite/codebuild/projects/auto_ros_ethercat_noetic-devel_code_coverage/) 10 | 11 | ## Overview 12 | 13 | This is a reimplementation of the main loop of pr2_ethercat without dependencies on PR2 software. It was mainly developed to be used by Shadow Robot Company. It may be useful to anyone people that has developed ROS software for their own robot and used etherCAT for communication. Most likely such software would be based on the software for Willow Garage's PR2. ros_ethercat reuses existing etherCAT drivers (eml package) and instantiates a ros_control controller manager. Part of the software in this repository comes from repositories pr2_mechanism, pr2_robot, pr2_ethercat_drivers. The parts specific to the pr2 robot only have been removed. Therefore, the same license (BSD) has been used. 14 | 15 | *pr2 packages no longer required* 16 | 17 | 1. ethercat_hardware 18 | 2. pr2_bringup 19 | 3. pr2_ethercat 20 | 4. pr2_mechanism 21 | 5. pr2_controller_interface 22 | 6. pr2_controller_manager 23 | 7. pr2_hardware_interface 24 | 8. pr2_mechanism_diagnostics 25 | 9. pr2_mechanism_model 26 | 27 | You can find the architecture diagram below for a closer look at how this works. 28 | 29 | ![Architecture Diagram](doc/RosEtherCAT.png) 30 | 31 | ## Compatibility 32 | 33 | In software with previous pr2 dependencies that is switched to this package, the following modifications are required. 34 | 35 | ### Controllers 36 | 37 | 1. `#include `
38 | instead of
`#include ` 39 | 2. `class MyController : public controller_interface::Controller`
40 | instead of
`class MyController : public pr2_controller_interface::Controller` 41 | 3. Controller's update function should be declared as
`virtual void update(const ros::Time&, const ros::Duration&)`
and starting function as
`virtual void starting(const ros::Time& time)`,br. 42 | 4. `controller_manager` and `controller_interface` should be used as dependencies in `CMakeLists.txt` and `package.xml` files instead of `pr2_controller_manager` and `pr2_controller_interface` respectively. 43 | 5. In `package.xml` file in export tag use `` 44 | 6. In `controller_plugins.xml` file use `base_class_type="controller_interface::ControllerBase" />` 45 | 46 | ### launch files 47 | 48 | 1. Replace `pr2_ethercat` with `ros_ethercat_loop` in launch files 49 | 2. Since pr2_controller_manager is no longer used, joint_states or mechanism_statistics are no longer published by it. Joint states are now published with the join_state_controller from ros_controllers. To start this controller this line need to be added in the main launch file
`` 50 | 3. `calibrate.py` file from pr2_bringup is now included in ros_ethercat_model package and renamed to just `calibrate` according to ROS convention. Launch files that execute `calibrate.py` should be modified to find it there. 51 | 52 | ### transmissions 53 | 54 | Transmissions are still defined in urdf in the pr2 style and inherit from `ros_ethercat_model::Transmission`. This class will initialize the actuators associated with this transmission. 55 | 56 | ## New features 57 | 1. ros_ethercat accepts a new argument `--period` which is the period of main ethercat loop in msec. If not given the default value is 1ms. 58 | 2. There is a helper bash script called ethercat_grant. This will grant to the ros_ethercat_loop executable the ability to be ran from a normal user without root privileges. E.g. of use
`rosrun ros_ethercat_loop ethercat_grant` 59 | -------------------------------------------------------------------------------- /aws.yml: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # Copyright © 2022 belongs to Shadow Robot Company Ltd. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without modification, 6 | # are permitted provided that the following conditions are met: 7 | # 1. Redistributions of source code must retain the above copyright notice, 8 | # this list of conditions and the following disclaimer. 9 | # 2. Redistributions in binary form must reproduce the above copyright notice, 10 | # this list of conditions and the following disclaimer in the documentation 11 | # and/or other materials provided with the distribution. 12 | # 3. Neither the name of Shadow Robot Company Ltd nor the names of its contributors 13 | # may be used to endorse or promote products derived from this software without 14 | # specific prior written permission. 15 | # 16 | # This software is provided by Shadow Robot Company Ltd "as is" and any express 17 | # or implied warranties, including, but not limited to, the implied warranties of 18 | # merchantability and fitness for a particular purpose are disclaimed. In no event 19 | # shall the copyright holder be liable for any direct, indirect, incidental, special, 20 | # exemplary, or consequential damages (including, but not limited to, procurement of 21 | # substitute goods or services; loss of use, data, or profits; or business interruption) 22 | # however caused and on any theory of liability, whether in contract, strict liability, 23 | # or tort (including negligence or otherwise) arising in any way out of the use of this 24 | # software, even if advised of the possibility of such damage. 25 | 26 | settings: 27 | machine_type: medium 28 | ubuntu: 29 | version: focal 30 | ros: 31 | release: noetic 32 | docker: 33 | image: public.ecr.aws/shadowrobot/build-tools 34 | tag: focal-noetic 35 | template_project_name: template_unit_tests_and_code_coverage 36 | toolset: 37 | branch: lint 38 | modules: 39 | - check_cache 40 | - code_coverage 41 | 42 | trunks: 43 | - name: noetic-devel 44 | jobs: 45 | - name: code_coverage 46 | - name: style_check 47 | settings: 48 | toolset: 49 | modules: 50 | - code_style_check 51 | - check_license 52 | - name: install_check 53 | settings: 54 | toolset: 55 | modules: 56 | - check_install 57 | -------------------------------------------------------------------------------- /doc/RosEtherCAT.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/shadow-robot/ros_ethercat/e0717bc84a507c1ac8dad1c1435ab9149545cb5b/doc/RosEtherCAT.png -------------------------------------------------------------------------------- /repository.rosinstall: -------------------------------------------------------------------------------- 1 | - git: 2 | local-name: sr_common 3 | uri: https://github.com/shadow-robot/sr_common.git 4 | version: noetic-devel 5 | 6 | - git: 7 | local-name: ros_ethercat_eml 8 | uri: https://github.com/shadow-robot/ros_ethercat_eml.git 9 | version: noetic-devel 10 | -------------------------------------------------------------------------------- /ros_ethercat/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ros_ethercat 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.1 (2019-02-26) 6 | ------------------ 7 | * ros_ethercat_eml moved out of this repository 8 | 9 | 0.2.0 (2015-04-07) 10 | ------------------ 11 | 12 | 0.1.8 (2014-07-18) 13 | ------------------ 14 | 15 | 0.1.7 (2014-05-23) 16 | ------------------ 17 | 18 | 0.1.6 (2014-05-15) 19 | ------------------ 20 | 21 | 0.1.5 (2014-05-14) 22 | ------------------ 23 | 24 | 0.1.4 (2014-05-14) 25 | ------------------ 26 | 27 | 0.1.3 (2014-05-13) 28 | ------------------ 29 | 30 | 0.1.2 (2014-05-13) 31 | ------------------ 32 | 33 | 0.1.1 (2014-05-12) 34 | ------------------ 35 | * first hydro release 36 | -------------------------------------------------------------------------------- /ros_ethercat/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ros_ethercat) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /ros_ethercat/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 26 | 27 | 28 | ros_ethercat 29 | 0.3.1 30 | A pr2 agnostic replacement for robots using EtherCAT 31 | Shadow Robot's software team 32 | 33 | BSD 34 | 35 | http://ros.org/wiki/ros_ethercat 36 | https://github.com/shadow-robot/ros_ethercat 37 | https://github.com/shadow-robot/ros_ethercat/issues 38 | 39 | Manos Nikolaidis 40 | 41 | 42 | catkin 43 | 44 | 45 | ros_ethercat_hardware 46 | ros_ethercat_loop 47 | ros_ethercat_model 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /ros_ethercat_hardware/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ros_ethercat_hardware 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.1 (2019-02-26) 6 | ------------------ 7 | * ros_ethercat_eml moved out of this repository 8 | 9 | 0.2.0 (2015-04-07) 10 | ------------------ 11 | * Delete motor halted message 12 | * Make the start address non-static (each ethercat port will have its own separate logical address space) 13 | * Add run dependency, otherwise catkin complains. 14 | * Enable rpath for ros_ethercat_hardware. We add a build dep on ros_ethercat_model to have the new cmake function available. 15 | * bug fixes in initialization 16 | 17 | 18 | 0.1.8 (2014-07-18) 19 | ------------------ 20 | 21 | 0.1.7 (2014-05-23) 22 | ------------------ 23 | 24 | 0.1.6 (2014-05-15) 25 | ------------------ 26 | 27 | 0.1.5 (2014-05-14) 28 | ------------------ 29 | 30 | 0.1.4 (2014-05-14) 31 | ------------------ 32 | 33 | 0.1.3 (2014-05-13) 34 | ------------------ 35 | 36 | 0.1.2 (2014-05-13) 37 | ------------------ 38 | 39 | 0.1.1 (2014-05-12) 40 | ------------------ 41 | * first hydro release 42 | -------------------------------------------------------------------------------- /ros_ethercat_hardware/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ros_ethercat_hardware) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | realtime_tools 7 | diagnostic_msgs 8 | diagnostic_updater 9 | pluginlib 10 | hardware_interface 11 | ros_ethercat_eml 12 | ) 13 | 14 | include_directories(include 15 | ${catkin_INCLUDE_DIRS} 16 | ) 17 | 18 | catkin_package( 19 | CATKIN_DEPENDS 20 | roscpp 21 | realtime_tools 22 | diagnostic_msgs 23 | diagnostic_updater 24 | pluginlib 25 | hardware_interface 26 | ros_ethercat_eml 27 | INCLUDE_DIRS include 28 | LIBRARIES ros_ethercat_hardware 29 | CFG_EXTRAS 30 | ${PROJECT_NAME}-extras.cmake 31 | ) 32 | 33 | add_library(${PROJECT_NAME} 34 | src/ethercat_com.cpp 35 | src/ethercat_device.cpp 36 | src/ethercat_hardware.cpp 37 | src/ethernet_interface_info.cpp 38 | ) 39 | add_dependencies(${PROJECT_NAME} 40 | ${catkin_EXPORTED_TARGETS} 41 | ) 42 | target_link_libraries(${PROJECT_NAME} 43 | ${catkin_LIBRARIES} 44 | ) 45 | 46 | include(cmake/${PROJECT_NAME}-extras.cmake.em) 47 | ros_enable_rpath(${PROJECT_NAME}) 48 | 49 | 50 | install(TARGETS ${PROJECT_NAME} 51 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 52 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 53 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 54 | ) 55 | 56 | install(DIRECTORY include/${PROJECT_NAME}/ 57 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 58 | ) 59 | -------------------------------------------------------------------------------- /ros_ethercat_hardware/cmake/ros_ethercat_hardware-extras.cmake.em: -------------------------------------------------------------------------------- 1 | 2 | function(ros_enable_rpath target) 3 | # Set ${target} with RPATH built in so that we can install it suid 4 | set_target_properties(${target} PROPERTIES SKIP_BUILD_RPATH FALSE) 5 | 6 | # Set the install RPATH to the install path 7 | set(RPATH "${CMAKE_INSTALL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}") 8 | 9 | # If LD_LIBRARY_PATH is set, add it to the install RPATH 10 | # this works in a normal catkin environment, but fails if the user unsets 11 | # their LD_LIBRARY_PATH manually for some reason 12 | if(DEFINED ENV{LD_LIBRARY_PATH}) 13 | set(RPATH "${RPATH}:$ENV{LD_LIBRARY_PATH}") 14 | endif() 15 | 16 | message(STATUS "Install RPATH for ${target} is ${RPATH}") 17 | 18 | # Apply our computed RPATH to the target 19 | set_target_properties(${target} PROPERTIES INSTALL_RPATH ${RPATH}) 20 | 21 | # Don't use the final RPATH in devel space 22 | set_target_properties(${target} PROPERTIES BUILD_WITH_INSTALL_RPATH FALSE) 23 | endfunction() 24 | -------------------------------------------------------------------------------- /ros_ethercat_hardware/include/ros_ethercat_hardware/copyright_exclusions.cfg: -------------------------------------------------------------------------------- 1 | exclude_files=/*.h -------------------------------------------------------------------------------- /ros_ethercat_hardware/include/ros_ethercat_hardware/ethercat_com.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef ROS_ETHERCAT_HARDWARE_ETHERCAT_COM_H 36 | #define ROS_ETHERCAT_HARDWARE_ETHERCAT_COM_H 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | class EthercatCom 45 | { 46 | protected: 47 | EthercatCom() 48 | { 49 | } 50 | 51 | public: 52 | virtual bool txandrx(struct EtherCAT_Frame * frame) = 0; 53 | virtual bool txandrx_once(struct EtherCAT_Frame * frame) = 0; 54 | virtual ~EthercatCom() 55 | { 56 | } 57 | }; 58 | 59 | class EthercatDirectCom : public EthercatCom 60 | { 61 | public: 62 | explicit EthercatDirectCom(EtherCAT_DataLinkLayer *dll) : dll_(dll) 63 | { 64 | } 65 | 66 | bool txandrx(struct EtherCAT_Frame * frame); 67 | bool txandrx_once(struct EtherCAT_Frame * frame); 68 | 69 | protected: 70 | EtherCAT_DataLinkLayer *dll_; 71 | }; 72 | 73 | class EthercatOobCom : public EthercatCom 74 | { 75 | public: 76 | explicit EthercatOobCom(struct netif *ni); 77 | 78 | bool txandrx(struct EtherCAT_Frame * frame); 79 | bool txandrx_once(struct EtherCAT_Frame * frame); 80 | 81 | void tx(); 82 | protected: 83 | bool lock(unsigned line); 84 | bool trylock(unsigned line); 85 | bool unlock(unsigned line); 86 | 87 | struct netif *ni_; 88 | pthread_mutex_t mutex_; 89 | pthread_cond_t share_cond_; 90 | pthread_cond_t busy_cond_; 91 | 92 | enum 93 | { 94 | IDLE = 0, READY_TO_SEND = 1, WAITING_TO_RECV = 2 95 | } 96 | state_; 97 | EtherCAT_Frame *frame_; 98 | int handle_; 99 | unsigned line_; 100 | }; 101 | 102 | #endif // ROS_ETHERCAT_HARDWARE_ETHERCAT_COM_H 103 | -------------------------------------------------------------------------------- /ros_ethercat_hardware/include/ros_ethercat_hardware/ethercat_device.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef ROS_ETHERCAT_HARDWARE_ETHERCAT_DEVICE_H 36 | #define ROS_ETHERCAT_HARDWARE_ETHERCAT_DEVICE_H 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include 47 | 48 | #include 49 | 50 | #include 51 | 52 | #include 53 | 54 | #include 55 | 56 | using namespace std; // NOLINT(build/namespaces) 57 | 58 | struct et1x00_error_counters 59 | { 60 | struct 61 | { 62 | uint8_t invalid_frame; 63 | uint8_t rx_error; 64 | } 65 | __attribute__((__packed__)) port[4]; 66 | uint8_t forwarded_rx_error[4]; 67 | uint8_t epu_error; 68 | uint8_t pdi_error; 69 | uint8_t res[2]; 70 | uint8_t lost_link[4]; 71 | static const uint16_t BASE_ADDR = 0x300; 72 | bool isGreaterThan(unsigned value) const; 73 | bool isGreaterThan(const et1x00_error_counters &value) const; 74 | void zero(); 75 | } 76 | __attribute__((__packed__)); 77 | 78 | struct et1x00_dl_status 79 | { 80 | uint16_t status; 81 | bool hasLink(unsigned port); 82 | bool isClosed(unsigned port); 83 | bool hasCommunication(unsigned port); 84 | static const uint16_t BASE_ADDR = 0x110; 85 | } 86 | __attribute__((__packed__)); 87 | 88 | struct EthercatPortDiagnostics 89 | { 90 | EthercatPortDiagnostics(); 91 | void zeroTotals(); 92 | bool hasLink; 93 | bool isClosed; 94 | bool hasCommunication; 95 | uint64_t rxErrorTotal; 96 | uint64_t invalidFrameTotal; 97 | uint64_t forwardedRxErrorTotal; 98 | uint64_t lostLinkTotal; 99 | }; 100 | 101 | struct EthercatDeviceDiagnostics 102 | { 103 | public: 104 | EthercatDeviceDiagnostics(); 105 | 106 | // Collects diagnostic data from specific ethercat slave, and updates object state 107 | // 108 | // com EtherCAT communication object is used send/recv packets to/from ethercat chain. 109 | // sh slaveHandler of device to collect Diagnostics from 110 | // prev previously collected diagnostics (can be pointer to this object) 111 | // 112 | // collectDiagnotics will send/receive multiple packets, and may considerable amount of time complete. 113 | // 114 | void collect(EthercatCom *com, EtherCAT_SlaveHandler *sh); 115 | 116 | // Puts previously diagnostic collected diagnostic state to DiagnosticStatus object 117 | // 118 | // d DiagnositcState to add diagnostics to. 119 | // numPorts Number of ports device is supposed to have. 4 is max, 1 is min. 120 | void publish(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts = 4) const; 121 | 122 | protected: 123 | void zeroTotals(); 124 | void accumulate(const et1x00_error_counters &next, const et1x00_error_counters &prev); 125 | uint64_t pdiErrorTotal_; 126 | uint64_t epuErrorTotal_; 127 | EthercatPortDiagnostics portDiagnostics_[4]; 128 | unsigned nodeAddress_; 129 | et1x00_error_counters errorCountersPrev_; 130 | bool errorCountersMayBeCleared_; 131 | 132 | bool diagnosticsFirst_; 133 | bool diagnosticsValid_; 134 | bool resetDetected_; 135 | int devicesRespondingToNodeAddress_; 136 | }; 137 | 138 | class EthercatDevice 139 | { 140 | public: 141 | //!< Construct EtherCAT device 142 | virtual void construct(EtherCAT_SlaveHandler *sh, int &start_address); 143 | 144 | //!< Construct non-EtherCAT device 145 | virtual void construct(ros::NodeHandle &nh) 146 | { 147 | } 148 | 149 | EthercatDevice(); 150 | virtual ~EthercatDevice() 151 | { 152 | delete sh_->get_fmmu_config(); 153 | delete sh_->get_pd_config(); 154 | } 155 | virtual int initialize(hardware_interface::HardwareInterface *hw, bool allow_unprogrammed = true) 156 | { 157 | return 0; 158 | } 159 | /** 160 | * \param reset when asserted this will clear diagnostic error conditions device safety disable 161 | * \param halt while asserted will disable actuator, usually by disabling H-bridge 162 | */ 163 | virtual void packCommand(unsigned char *buffer, bool halt, bool reset) 164 | { 165 | } 166 | virtual bool unpackState(unsigned char *this_buffer, unsigned char *prev_buffer) 167 | { 168 | return true; 169 | } 170 | 171 | /** 172 | * \brief For EtherCAT devices that publish more than one EtherCAT Status message. 173 | * If sub-class implements multiDiagnostics() then diagnostics() is not used. 174 | * \param vec Vector of diagnostics status messages. Slave appends one or more new diagnostic status'. 175 | * \param buffer Pointer to slave process data.\ 176 | */ 177 | virtual void multiDiagnostics(vector &vec, 178 | unsigned char *buffer); 179 | 180 | /** 181 | * \brief For EtherCAT device that only publish one EtherCAT Status message. 182 | * If sub-class implements multiDiagnostics() then diagnostics() is not used. 183 | * \param d Diagnostics status wrapper. 184 | * \param buffer Pointer to slave process data. 185 | */ 186 | virtual void diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer); 187 | 188 | /** 189 | * \brief Adds diagnostic information for EtherCAT ports. 190 | * \param d EtherCAT port diagnostics information will be appended. 191 | * \param buffer Number of communication ports slave has. 192 | */ 193 | void ethercatDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts); 194 | 195 | virtual void collectDiagnostics(EthercatCom *com); 196 | /** 197 | * \brief Asks device to publish (motor) trace. Only works for devices that support it. 198 | * \param reason Message to put in trace as reason. 199 | * \param level Level to put in trace (aka ERROR=2, WARN=1, OK=0) 200 | * \param delay Publish trace after delay cyles. For 1kHz realtime loop 1cycle = 1ms. 201 | * \return Return true if device support publishing trace. False, if not. 202 | */ 203 | virtual bool publishTrace(const string &reason, unsigned level, unsigned delay) 204 | { 205 | return false; 206 | } 207 | 208 | enum AddrMode 209 | { 210 | FIXED_ADDR = 0, POSITIONAL_ADDR = 1 211 | }; 212 | 213 | /*! 214 | * \brief Write data to device ESC. 215 | */ 216 | static int writeData(EthercatCom *com, EtherCAT_SlaveHandler *sh, uint16_t address, 217 | void const* buffer, uint16_t length, AddrMode addrMode = FIXED_ADDR); 218 | inline int writeData(EthercatCom *com, uint16_t address, void const* buffer, uint16_t length, 219 | AddrMode addrMode) 220 | { 221 | return writeData(com, sh_, address, buffer, length, addrMode); 222 | } 223 | 224 | /*! 225 | * \brief Read data from device ESC. 226 | */ 227 | static int readData(EthercatCom *com, EtherCAT_SlaveHandler *sh, uint16_t address, void *buffer, 228 | uint16_t length, AddrMode addrMode = FIXED_ADDR); 229 | inline int readData(EthercatCom *com, uint16_t address, void *buffer, uint16_t length, 230 | AddrMode addrMode) 231 | { 232 | return readData(com, sh_, address, buffer, length, addrMode); 233 | } 234 | 235 | /*! 236 | * \brief Read then write data to ESC. 237 | */ 238 | static int readWriteData(EthercatCom *com, EtherCAT_SlaveHandler *sh, uint16_t address, 239 | void *buffer, uint16_t length, AddrMode addrMode = FIXED_ADDR); 240 | inline int readWriteData(EthercatCom *com, uint16_t address, void *buffer, uint16_t length, 241 | AddrMode addrMode) 242 | { 243 | return readWriteData(com, sh_, address, buffer, length, addrMode); 244 | } 245 | 246 | bool use_ros_; 247 | 248 | EtherCAT_SlaveHandler *sh_; 249 | unsigned int command_size_; 250 | unsigned int status_size_; 251 | 252 | // The device diagnostics are collected with a non-readtime thread that calls collectDiagnostics() 253 | // The device published from the realtime loop by indirectly invoking ethercatDiagnostics() 254 | // To avoid blocking of the realtime thread (for long) a double buffer is used the 255 | // The publisher thread will lock newDiagnosticsIndex when publishing data. 256 | // The collection thread will lock deviceDiagnostics when updating deviceDiagnostics 257 | // The collection thread will also lock newDiagnosticsIndex at end of update, just before swapping buffers. 258 | unsigned newDiagnosticsIndex_; 259 | pthread_mutex_t newDiagnosticsIndexLock_; 260 | EthercatDeviceDiagnostics deviceDiagnostics[2]; 261 | pthread_mutex_t diagnosticsLock_; 262 | 263 | // Keep diagnostics status as cache. Avoids a lot of construction/destruction of status object. 264 | diagnostic_updater::DiagnosticStatusWrapper diagnostic_status_; 265 | }; 266 | 267 | #endif // ROS_ETHERCAT_HARDWARE_ETHERCAT_DEVICE_H 268 | -------------------------------------------------------------------------------- /ros_ethercat_hardware/include/ros_ethercat_hardware/ethercat_hardware.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef ROS_ETHERCAT_HARDWARE_ETHERCAT_HARDWARE_H 36 | #define ROS_ETHERCAT_HARDWARE_ETHERCAT_HARDWARE_H 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #include "ros_ethercat_hardware/ethercat_device.h" 47 | #include "ros_ethercat_hardware/ethercat_com.h" 48 | #include "ros_ethercat_hardware/ethernet_interface_info.h" 49 | 50 | #include 51 | 52 | #include 53 | #include 54 | #include 55 | #include 56 | 57 | #include 58 | #include 59 | #include 60 | 61 | #include 62 | #include 63 | #include 64 | #include 65 | #include 66 | 67 | using namespace boost::accumulators; // NOLINT(build/namespaces) 68 | 69 | struct EthercatHardwareDiagnostics 70 | { 71 | EthercatHardwareDiagnostics(); 72 | void resetMaxTiming(); 73 | //!< time taken by all devices packCommand functions 74 | accumulator_set > pack_command_acc_; 75 | //!< time taken by to transmit and recieve process data 76 | accumulator_set > txandrx_acc_; 77 | //!< time taken by all devices updateState functions 78 | accumulator_set > unpack_state_acc_; 79 | //!< time taken by any publishing step in main loop 80 | accumulator_set > publish_acc_; 81 | double max_pack_command_; 82 | double max_txandrx_; 83 | double max_unpack_state_; 84 | double max_publish_; 85 | int txandrx_errors_; 86 | unsigned device_count_; 87 | bool pd_error_; 88 | bool halt_after_reset_; //!< True if motor halt soon after motor reset 89 | unsigned reset_motors_service_count_; //!< Number of times reset_motor service has been used 90 | unsigned halt_motors_service_count_; //!< Number of time halt_motor service call is used 91 | unsigned halt_motors_error_count_; //!< Number of transitions into halt state due to device error 92 | struct netif_counters counters_; 93 | bool input_thread_is_stopped_; 94 | bool motors_halted_; //!< True if motors are halted 95 | const char* motors_halted_reason_; //!< reason that motors first halted 96 | 97 | static const bool collect_extra_timing_ = true; 98 | }; 99 | 100 | /*! 101 | * \brief Publishes EthercatHardware diagnostics. 102 | * 103 | * All the string formating used for creating diagnostics is too 104 | * slow to be run in the realtime thread. Instead, a copy of the raw 105 | * diagnostics data is made and a separate thread does the string conversion 106 | * and publishing. 107 | * Previously, the diagnostics data used by publishing thread was contained 108 | * in the EthercatHardware class. However, this allowed the publishing thread 109 | * access to other non thread-safe data. 110 | * This class keeps the diagnostics data used by the publish thread explicitly 111 | * separate. 112 | */ 113 | class EthercatHardwareDiagnosticsPublisher 114 | { 115 | public: 116 | explicit EthercatHardwareDiagnosticsPublisher(ros::NodeHandle &node); 117 | ~EthercatHardwareDiagnosticsPublisher(); 118 | 119 | /*! 120 | * \brief Initializes hardware publish. 121 | * \param buffer_size size of process data buffer 122 | * \param number of EtherCAT slave devices 123 | */ 124 | void initialize(const string &interface, unsigned int buffer_size, 125 | const std::vector > &slaves, 126 | unsigned int num_ethercat_devices_, 127 | unsigned timeout, 128 | unsigned max_pd_retries); 129 | 130 | /*! 131 | * \brief Triggers publishing of new diagnostics data 132 | * 133 | * Makes copy of diagnostics data and triggers internal thread to 134 | * started conversion and publish of data. 135 | * This function will not block. 136 | */ 137 | void publish(const unsigned char *buffer, const EthercatHardwareDiagnostics &diagnostics); 138 | 139 | /*! 140 | * \brief Stops publishing thread. May block. 141 | */ 142 | void stop(); 143 | 144 | private: 145 | /*! 146 | * \brief Publishes diagnostics 147 | * 148 | * Takes internally saved diagnostics data and converts to a ROS 149 | * diagnostics status message. 150 | * This function performs a lot of string formatting, so it is slow. 151 | */ 152 | void publishDiagnostics(); 153 | 154 | /*! 155 | * \brief Publishing thread main loop 156 | * 157 | * Waits for condition variable to start publishing internal data. 158 | */ 159 | void diagnosticsThreadFunc(); 160 | 161 | /*! 162 | * \brief Helper function for converting timing for diagnostics 163 | */ 164 | static void timingInformation(diagnostic_updater::DiagnosticStatusWrapper &status, 165 | const string &key, 166 | const accumulator_set > &acc, 167 | double max); 168 | 169 | ros::NodeHandle node_; 170 | 171 | boost::mutex diagnostics_mutex_; //!< mutex protects all class data and cond variable 172 | boost::condition_variable diagnostics_cond_; 173 | bool diagnostics_ready_; 174 | boost::thread diagnostics_thread_; 175 | 176 | ros::Publisher publisher_; 177 | 178 | EthercatHardwareDiagnostics diagnostics_; //!< Diagnostics information use by publish function 179 | unsigned char *diagnostics_buffer_; 180 | unsigned int buffer_size_; 181 | std::vector > slaves_; 182 | unsigned int num_ethercat_devices_; 183 | string interface_; 184 | 185 | //! Timeout controls how long EtherCAT driver waits for packet before declaring it as dropped. 186 | unsigned timeout_; 187 | //! Number of times (in a row) to retry sending process data (realtime data) before halting motors 188 | unsigned max_pd_retries_; 189 | 190 | //! Count of dropped packets last diagnostics cycle 191 | uint64_t last_dropped_packet_count_; 192 | //! Time last packet was dropped 0 otherwise. Used for warning about dropped packets. 193 | ros::Time last_dropped_packet_time_; 194 | //! Number of seconds since late dropped packet to keep warning 195 | static const unsigned dropped_packet_warning_hold_time_ = 10; // keep warning up for 10 seconds 196 | 197 | diagnostic_msgs::DiagnosticArray diagnostic_array_; 198 | //! Information about Ethernet interface used for EtherCAT communication 199 | EthernetInterfaceInfo ethernet_interface_info_; 200 | vector values_; 201 | diagnostic_updater::DiagnosticStatusWrapper status_; 202 | }; 203 | 204 | class EthercatHardware 205 | { 206 | public: 207 | /*! 208 | * \brief Scans the network and gives a list of detected devices on a given ethercat port 209 | * \param eth is the thernet port to be scanned 210 | */ 211 | static std::vector scanPort(const std::string& eth); 212 | 213 | 214 | /*! 215 | * \brief Constructor 216 | */ 217 | EthercatHardware(const std::string& name, hardware_interface::HardwareInterface *hw, 218 | const string ð, bool allow_unprogrammed); 219 | 220 | /*! 221 | * \brief Destructor 222 | */ 223 | ~EthercatHardware(); 224 | 225 | /*! 226 | * \brief Send most recent motor commands and retrieve updates. This command must be run at a sufficient rate or else the motors will be disabled. 227 | * \param reset A boolean indicating if the motor controller boards should be reset 228 | * \param halt A boolean indicating if the motors should be halted 229 | */ 230 | void update(bool reset, bool halt); 231 | 232 | /*! 233 | * \brief Initialize the EtherCAT Master Library. 234 | */ 235 | void init(); 236 | 237 | /*! 238 | * \brief Collects diagnostics from all devices. 239 | */ 240 | void collectDiagnostics(); 241 | 242 | void printCounters(std::ostream &os = std::cout); 243 | 244 | /*! 245 | * \brief Send process data 246 | */ 247 | bool txandrx_PD(unsigned buffer_size, unsigned char* buffer, unsigned tries); 248 | 249 | /*! 250 | * \brief Ask one or all EtherCAT devices to publish (motor) traces 251 | * \param position device ring position to publish trace for. Use -1 to trigger all devices. 252 | * \param reason Message to put in trace as reason. 253 | * \param level Level to put in trace (aka ERROR=2, WARN=1, OK=0) 254 | * \param delay Publish trace after delay cycles. For 1kHz realtime loop 1cycle = 1ms. 255 | * \return Return true if device supports publishing trace. False, if not. 256 | * If all devices are triggered, returns true if any device publishes trace. 257 | */ 258 | bool publishTrace(int position, const string &reason, unsigned level, unsigned delay); 259 | 260 | hardware_interface::HardwareInterface *hw_; 261 | 262 | // Attributes required to run multi-threaded calls to HardwareInterface.update (for multiple HardwareInterface's) 263 | boost::mutex update_mutex; 264 | boost::condition_variable start_of_work_condition_eth_hw_read; 265 | boost::condition_variable end_of_work_condition_eth_hw_read; 266 | std::atomic can_run_eth_hw_read_; 267 | std::atomic eth_hw_read_done_; 268 | 269 | const std::vector > getSlaves() const 270 | { 271 | return std::vector >(slaves_.begin(), slaves_.end()); 272 | } 273 | 274 | private: 275 | static void changeState(EtherCAT_SlaveHandler *sh, EC_State new_state); 276 | 277 | void loadNonEthercatDevices(); 278 | boost::shared_ptr configNonEthercatDevice(const std::string &product_id, 279 | const std::string &data); 280 | 281 | void haltMotors(bool error, const char* reason); 282 | 283 | void publishDiagnostics(); //!< Collects raw diagnostics data and passes it to diagnostics_publisher 284 | static void updateAccMax(double &max, const accumulator_set > &acc); 285 | boost::shared_ptr configSlave(EtherCAT_SlaveHandler *sh); 286 | bool setRouterToSlaveHandlers(); 287 | 288 | ros::NodeHandle node_; 289 | 290 | struct netif *ni_; 291 | 292 | string interface_; //!< The socket interface that is connected to the EtherCAT devices (e.g., eth0) 293 | 294 | EtherCAT_DataLinkLayer m_dll_instance_; 295 | EC_Logic m_logic_instance_; 296 | EtherCAT_PD_Buffer pd_buffer_; 297 | EtherCAT_AL *application_layer_; 298 | EtherCAT_Router *m_router_; 299 | EtherCAT_Master *ethercat_master_; 300 | 301 | std::vector > slaves_; 302 | unsigned int num_ethercat_devices_; 303 | 304 | unsigned char *this_buffer_; 305 | unsigned char *prev_buffer_; 306 | unsigned char *buffers_; 307 | unsigned int buffer_size_; 308 | 309 | bool halt_motors_; 310 | unsigned int reset_state_; 311 | 312 | unsigned timeout_; //!< Timeout (in microseconds) to used for sending/recieving packets once in realtime mode. 313 | unsigned max_pd_retries_; //!< Max number of times to retry sending process data before halting motors 314 | 315 | EthercatHardwareDiagnostics diagnostics_; 316 | EthercatHardwareDiagnosticsPublisher diagnostics_publisher_; 317 | ros::Time last_published_; 318 | ros::Time last_reset_; 319 | 320 | realtime_tools::RealtimePublisher motor_publisher_; 321 | 322 | EthercatOobCom *oob_com_; 323 | 324 | pluginlib::ClassLoader device_loader_; 325 | 326 | bool allow_unprogrammed_; //!< if the driver should treat the discovery of unprogrammed boards as a fatal error. 327 | // Set to 'true' during board configuration, and set to 'false' otherwise. 328 | 329 | int start_address_; 330 | }; 331 | 332 | #endif // ROS_ETHERCAT_HARDWARE_ETHERCAT_HARDWARE_H 333 | -------------------------------------------------------------------------------- /ros_ethercat_hardware/include/ros_ethercat_hardware/ethernet_interface_info.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef ROS_ETHERCAT_HARDWARE_ETHERNET_INTERFACE_INFO_H 36 | #define ROS_ETHERCAT_HARDWARE_ETHERNET_INTERFACE_INFO_H 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | struct EthtoolStats 44 | { 45 | EthtoolStats(); 46 | uint64_t rx_errors_; 47 | uint64_t rx_crc_errors_; 48 | uint64_t rx_frame_errors_; 49 | uint64_t rx_align_errors_; 50 | 51 | EthtoolStats& operator-=(const EthtoolStats& right); 52 | }; 53 | 54 | struct InterfaceState 55 | { 56 | InterfaceState() : 57 | up_(false), running_(false) 58 | { 59 | } 60 | bool up_; 61 | bool running_; 62 | }; 63 | 64 | class EthernetInterfaceInfo 65 | { 66 | public: 67 | EthernetInterfaceInfo(); 68 | void initialize(const std::string &interface); 69 | ~EthernetInterfaceInfo(); 70 | 71 | /** 72 | * \brief Collect and append ethernet interface diagnostics 73 | * 74 | * \param d Diagnostics status wrapper. 75 | */ 76 | void publishDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d); 77 | 78 | protected: 79 | //! Get ethtool stats from interface 80 | bool getEthtoolStats(EthtoolStats &stats); 81 | 82 | //! Get state (up,running) of interface 83 | bool getInterfaceState(InterfaceState &state); 84 | 85 | //! name of network interface (for example : eth0) 86 | std::string interface_; 87 | //! network socket for making ioctl requests 88 | int sock_; 89 | //! Number of stats available from ethtool ioctl 90 | unsigned n_stats_; 91 | //! buffer for NIC statistic values 92 | char *ethtool_stats_buf_; 93 | 94 | // Indexes of statistics that come from ethtool ioctl 95 | // An index of -1 indicates that statistic is not available from network driver 96 | int rx_error_index_; 97 | int rx_crc_error_index_; 98 | int rx_frame_error_index_; 99 | int rx_align_error_index_; 100 | 101 | //! Number of time master link went down 102 | unsigned lost_link_count_; 103 | 104 | //! Original statistics counts when initialize() was called. 105 | EthtoolStats orig_stats_; 106 | InterfaceState last_state_; 107 | }; 108 | 109 | #endif // ROS_ETHERCAT_HARDWARE_ETHERNET_INTERFACE_INFO_H 110 | -------------------------------------------------------------------------------- /ros_ethercat_hardware/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** \mainpage 2 | 3 | @htmlinclude manifest.html 4 | 5 | @section summary Summary 6 | This package provides a wrapper/driver for EtherCAT master library. 7 | 8 | @section usage Usage 9 | This package exposes a single class with a simple API to initialize all EtherCAT devices found on an interface (EthercatHardware::init), and a second call to update those devices on a periodic basis (EthercatHardware::update). An application wishing to control EtherCAT devices should create a single instance of the EthercatHardware driver. 10 | 11 | - \ref EthercatHardware::EthercatHardware 12 | 13 | */ 14 | -------------------------------------------------------------------------------- /ros_ethercat_hardware/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 26 | 27 | 28 | ros_ethercat_hardware 29 | 0.3.1 30 | Package for creating a hardware interface to the robot using the EtherCAT motor controller/driver 31 | Shadow Robot's software team 32 | 33 | BSD 34 | 35 | http://ros.org/wiki/ros_ethercat_hardware 36 | https://github.com/shadow-robot/ros_ethercat 37 | https://github.com/shadow-robot/ros_ethercat/issues 38 | 39 | Rob Wheeler 40 | Derek King 41 | Manos Nikolaidis 42 | 43 | catkin 44 | 45 | ros_ethercat_eml 46 | roscpp 47 | realtime_tools 48 | diagnostic_msgs 49 | diagnostic_updater 50 | pluginlib 51 | hardware_interface 52 | 53 | ros_ethercat_eml 54 | roscpp 55 | realtime_tools 56 | diagnostic_msgs 57 | diagnostic_updater 58 | pluginlib 59 | hardware_interface 60 | 61 | 62 | -------------------------------------------------------------------------------- /ros_ethercat_hardware/src/copyright_exclusions.cfg: -------------------------------------------------------------------------------- 1 | exclude_files=/*. -------------------------------------------------------------------------------- /ros_ethercat_hardware/src/ethercat_com.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include "ros_ethercat_hardware/ethercat_com.h" 36 | #include 37 | #include 38 | #include 39 | 40 | bool EthercatDirectCom::txandrx_once(struct EtherCAT_Frame * frame) 41 | { 42 | assert(frame != NULL); 43 | int handle = dll_->tx(frame); 44 | if (handle < 0) 45 | return false; 46 | return dll_->rx(frame, handle); 47 | } 48 | 49 | bool EthercatDirectCom::txandrx(struct EtherCAT_Frame * frame) 50 | { 51 | return dll_->txandrx(frame); 52 | } 53 | 54 | EthercatOobCom::EthercatOobCom(struct netif *ni) : 55 | ni_(ni), 56 | state_(IDLE), 57 | frame_(NULL), 58 | handle_(-1), 59 | line_(0) 60 | { 61 | assert(ni_ != NULL); 62 | 63 | pthread_mutexattr_t mutex_attr; 64 | int error = pthread_mutexattr_init(&mutex_attr); 65 | if (error != 0) 66 | { 67 | fprintf(stderr, "%s : Initializing mutex attr failed : %d\n", __func__, error); 68 | return; 69 | } 70 | error = pthread_mutexattr_settype(&mutex_attr, PTHREAD_MUTEX_ERRORCHECK_NP); 71 | if (error != 0) 72 | { 73 | fprintf(stderr, "%s : Setting type of mutex attr failed : %d\n", __func__, error); 74 | return; 75 | } 76 | error = pthread_mutex_init(&mutex_, &mutex_attr); 77 | if (error != 0) 78 | { 79 | fprintf(stderr, "%s : Initializing mutex failed : %d\n", __func__, error); 80 | return; 81 | } 82 | error = pthread_cond_init(&share_cond_, NULL); 83 | if (error != 0) 84 | { 85 | fprintf(stderr, "%s : Initializing share condition failed : %d\n", __func__, error); 86 | return; 87 | } 88 | error = pthread_cond_init(&busy_cond_, NULL); 89 | if (error != 0) 90 | fprintf(stderr, "%s : Initializing busy condition failed : %d\n", __func__, error); 91 | return; 92 | } 93 | 94 | bool EthercatOobCom::lock(unsigned line) 95 | { 96 | int error; 97 | if (0 != (error = pthread_mutex_lock(&mutex_))) 98 | { 99 | fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line); 100 | return false; 101 | } 102 | line_ = line; 103 | return true; 104 | } 105 | 106 | bool EthercatOobCom::trylock(unsigned line) 107 | { 108 | int error; 109 | if (0 != (error = pthread_mutex_trylock(&mutex_))) 110 | { 111 | if (error != EBUSY) 112 | fprintf(stderr, "%s : lock %d at %d\n", __func__, error, line); 113 | return false; 114 | } 115 | line_ = line; 116 | return true; 117 | } 118 | 119 | bool EthercatOobCom::unlock(unsigned line) 120 | { 121 | int error; 122 | if (0 != (error = pthread_mutex_unlock(&mutex_))) 123 | { 124 | fprintf(stderr, "%s : unlock %d at %d\n", __func__, error, line); 125 | return false; 126 | } 127 | line_ = 0; 128 | return true; 129 | } 130 | 131 | // OOB replacement for netif->txandrx() 132 | // Returns true for success, false for dropped packet 133 | 134 | bool EthercatOobCom::txandrx_once(struct EtherCAT_Frame * frame) 135 | { 136 | assert(frame != NULL); 137 | 138 | if (!lock(__LINE__)) 139 | return false; 140 | 141 | // Wait for an opening to send frame 142 | while (state_ != IDLE) 143 | { 144 | pthread_cond_wait(&share_cond_, &mutex_); 145 | } 146 | frame_ = frame; 147 | state_ = READY_TO_SEND; 148 | 149 | // RT control loop will send frame 150 | do 151 | { 152 | pthread_cond_wait(&busy_cond_, &mutex_); 153 | } 154 | while (state_ != WAITING_TO_RECV); 155 | 156 | // Packet has been sent, wait for recv 157 | bool success = false; 158 | if (handle_ >= 0) 159 | success = ni_->rx(frame_, ni_, handle_); 160 | handle_ = -1; 161 | 162 | // Allow other threads to send data 163 | assert(frame_ == frame); 164 | state_ = IDLE; 165 | pthread_cond_signal(&share_cond_); 166 | 167 | unlock(__LINE__); 168 | 169 | return success; 170 | } 171 | 172 | bool EthercatOobCom::txandrx(struct EtherCAT_Frame * frame) 173 | { 174 | static const unsigned MAX_TRIES = 10; 175 | for (unsigned tries = 0; tries < MAX_TRIES; ++tries) 176 | { 177 | if (this->txandrx_once(frame)) 178 | return true; 179 | } 180 | return false; 181 | } 182 | 183 | // Called by RT control loop to send oob data 184 | 185 | void EthercatOobCom::tx() 186 | { 187 | if (!trylock(__LINE__)) 188 | return; 189 | 190 | if (state_ == READY_TO_SEND) 191 | { 192 | // Packet is in need of being sent 193 | assert(frame_ != NULL); 194 | handle_ = ni_->tx(frame_, ni_); 195 | state_ = WAITING_TO_RECV; 196 | pthread_cond_signal(&busy_cond_); 197 | } 198 | 199 | unlock(__LINE__); 200 | } 201 | -------------------------------------------------------------------------------- /ros_ethercat_hardware/src/ethercat_device.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include "ros_ethercat_hardware/ethercat_device.h" 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | bool et1x00_error_counters::isGreaterThan(unsigned value) const 42 | { 43 | if ((pdi_error > value) || (epu_error > value)) 44 | { 45 | return true; 46 | } 47 | 48 | for (unsigned i = 0; i < 4; ++i) 49 | { 50 | if ((port[i].rx_error > value) || 51 | (forwarded_rx_error[i] > value) || 52 | (lost_link[i] > value) || 53 | (port[i].invalid_frame > value)) 54 | { 55 | return true; 56 | } 57 | } 58 | return false; 59 | } 60 | 61 | bool et1x00_error_counters::isGreaterThan(const et1x00_error_counters &v) const 62 | { 63 | if ((pdi_error > v.pdi_error) || (epu_error > v.epu_error)) 64 | { 65 | return true; 66 | } 67 | 68 | for (unsigned i = 0; i < 4; ++i) 69 | { 70 | if ((port[i].rx_error > v.port[i].rx_error) || 71 | (forwarded_rx_error[i] > v.forwarded_rx_error[i]) || 72 | (lost_link[i] > v.lost_link[i]) || 73 | (port[i].invalid_frame > v.port[i].invalid_frame)) 74 | { 75 | return true; 76 | } 77 | } 78 | return false; 79 | } 80 | 81 | bool et1x00_dl_status::hasLink(unsigned port) 82 | { 83 | assert(port < 4); 84 | return status & (1 << (4 + port)); 85 | } 86 | 87 | bool et1x00_dl_status::hasCommunication(unsigned port) 88 | { 89 | assert(port < 4); 90 | return status & (1 << (9 + port * 2)); 91 | } 92 | 93 | bool et1x00_dl_status::isClosed(unsigned port) 94 | { 95 | assert(port < 4); 96 | return status & (1 << (8 + port * 2)); 97 | } 98 | 99 | void et1x00_error_counters::zero() 100 | { 101 | memset(this, 0, sizeof (et1x00_error_counters)); 102 | } 103 | 104 | EthercatPortDiagnostics::EthercatPortDiagnostics() : 105 | hasLink(false), 106 | isClosed(false), 107 | hasCommunication(false) 108 | { 109 | zeroTotals(); 110 | } 111 | 112 | void EthercatPortDiagnostics::zeroTotals() 113 | { 114 | rxErrorTotal = 0; 115 | invalidFrameTotal = 0; 116 | forwardedRxErrorTotal = 0; 117 | lostLinkTotal = 0; 118 | } 119 | 120 | EthercatDeviceDiagnostics::EthercatDeviceDiagnostics() : 121 | errorCountersMayBeCleared_(false), 122 | diagnosticsFirst_(true), 123 | diagnosticsValid_(false), 124 | resetDetected_(false), 125 | devicesRespondingToNodeAddress_(-1) 126 | { 127 | zeroTotals(); 128 | errorCountersPrev_.zero(); 129 | } 130 | 131 | void EthercatDeviceDiagnostics::zeroTotals() 132 | { 133 | pdiErrorTotal_ = 0; 134 | epuErrorTotal_ = 0; 135 | portDiagnostics_[0].zeroTotals(); 136 | portDiagnostics_[1].zeroTotals(); 137 | portDiagnostics_[2].zeroTotals(); 138 | portDiagnostics_[3].zeroTotals(); 139 | } 140 | 141 | void EthercatDeviceDiagnostics::accumulate(const et1x00_error_counters &n, const et1x00_error_counters &p) 142 | { 143 | pdiErrorTotal_ += n.pdi_error - p.pdi_error; 144 | epuErrorTotal_ += n.epu_error - p.epu_error; 145 | for (unsigned i = 0; i < 4; ++i) 146 | { 147 | EthercatPortDiagnostics & pt(portDiagnostics_[i]); 148 | pt.rxErrorTotal += n.port[i].rx_error - p.port[i].rx_error; 149 | pt.forwardedRxErrorTotal += n.forwarded_rx_error[i] - p.forwarded_rx_error[i]; 150 | pt.lostLinkTotal += n.lost_link[i] - p.lost_link[i]; 151 | pt.invalidFrameTotal += n.port[i].invalid_frame - p.port[i].invalid_frame; 152 | } 153 | } 154 | 155 | void EthercatDeviceDiagnostics::collect(EthercatCom *com, EtherCAT_SlaveHandler *sh) 156 | { 157 | diagnosticsValid_ = false; 158 | diagnosticsFirst_ = false; 159 | 160 | // Check if device has been reset/power cycled using its node address 161 | // Node address initialize to 0 after device reset. 162 | // EML library will configure each node address to non-zero when it first starts 163 | // Device should respond to its node address, if it does not either: 164 | // 1. communication to device is not possible (lost/broken link) 165 | // 2. device was reset, and its fixed address setting is now 0 166 | { 167 | // Send a packet with both a Fixed address read (NPRW) and a positional read (APRD) 168 | // If the NPRD has a working counter == 0, but the APRD sees the correct number of devices, 169 | // then the node has likely been reset. 170 | // Also, get DL status register with nprd telegram 171 | 172 | // This is for the case of non-ethercat device where SlaveHandler is NULL 173 | if (sh == NULL) 174 | { 175 | diagnosticsValid_ = true; 176 | return; 177 | } 178 | 179 | EC_Logic *logic = sh->m_logic_instance; 180 | et1x00_dl_status dl_status; 181 | NPRD_Telegram nprd_telegram(logic->get_idx(), 182 | sh->get_station_address(), 183 | dl_status.BASE_ADDR, 184 | logic->get_wkc(), 185 | sizeof (dl_status), 186 | (unsigned char*) &dl_status); 187 | // Use positional read to re-count number of devices on chain 188 | unsigned char buf[1]; 189 | uint16_t address = 0x0000; 190 | APRD_Telegram aprd_telegram(logic->get_idx(), // Index 191 | 0, // Slave position on ethercat chain (auto increment address) ( 192 | address, // ESC physical memory address (start address) 193 | logic->get_wkc(), // Working counter 194 | sizeof (buf), // Data Length, 195 | buf); // Buffer to put read result into 196 | 197 | 198 | 199 | // Chain both telegrams together 200 | nprd_telegram.attach(&aprd_telegram); 201 | 202 | EC_Ethernet_Frame frame(&nprd_telegram); 203 | 204 | // Send/Recv data from slave 205 | if (!com->txandrx_once(&frame)) 206 | { 207 | // no response - broken link to device 208 | goto end; 209 | } 210 | 211 | devicesRespondingToNodeAddress_ = nprd_telegram.get_wkc(); 212 | if (devicesRespondingToNodeAddress_ == 0) 213 | { 214 | // Device has not responded to its node address. 215 | if (aprd_telegram.get_adp() >= sh->m_router_instance->m_al_instance->get_num_slaves()) 216 | { 217 | resetDetected_ = true; 218 | goto end; 219 | } 220 | } 221 | else if (devicesRespondingToNodeAddress_ > 1) 222 | { 223 | // Can't determine much if two (or more) devices are responding to same request. 224 | goto end; 225 | } 226 | else 227 | { 228 | resetDetected_ = false; 229 | } 230 | 231 | // fill in port status information 232 | for (unsigned i = 0; i < 4; ++i) 233 | { 234 | EthercatPortDiagnostics & pt(portDiagnostics_[i]); 235 | pt.hasLink = dl_status.hasLink(i); 236 | pt.isClosed = dl_status.isClosed(i); 237 | pt.hasCommunication = dl_status.hasCommunication(i); 238 | } 239 | } 240 | 241 | { // read and accumulate communication error counters 242 | et1x00_error_counters e; 243 | assert(sizeof (e) == (0x314 - 0x300)); 244 | if (0 != EthercatDevice::readData(com, sh, e.BASE_ADDR, &e, sizeof (e), EthercatDevice::FIXED_ADDR)) 245 | { 246 | goto end; 247 | } 248 | 249 | // If this previously tried to clear the error counters but d/n get a response 250 | // then use the newly read values to guess if they got cleared or not. 251 | if (errorCountersMayBeCleared_) 252 | { 253 | if (!e.isGreaterThan(errorCountersPrev_)) 254 | { 255 | errorCountersPrev_.zero(); 256 | } 257 | errorCountersMayBeCleared_ = false; 258 | } 259 | if (errorCountersPrev_.isGreaterThan(e)) 260 | { 261 | ROS_ERROR("Device %d : previous port error counters less current values", sh->get_ring_position()); 262 | } 263 | 264 | // Accumulate 265 | this->accumulate(e, errorCountersPrev_); 266 | errorCountersPrev_ = e; 267 | 268 | // re-read and clear communication error counters 269 | if (e.isGreaterThan(50)) 270 | { 271 | if (0 != EthercatDevice::readWriteData(com, sh, e.BASE_ADDR, &e, sizeof (e), EthercatDevice::FIXED_ADDR)) 272 | { 273 | // Packet got lost... Can't know for sure that error counters got cleared 274 | errorCountersMayBeCleared_ = true; 275 | goto end; 276 | } 277 | // We read and cleared error counters in same packet, accumulate any error counter differences 278 | this->accumulate(e, errorCountersPrev_); 279 | errorCountersPrev_.zero(); 280 | } 281 | } 282 | 283 | // Everything was read successfully 284 | diagnosticsValid_ = true; 285 | 286 | end: 287 | return; 288 | } 289 | 290 | void EthercatDeviceDiagnostics::publish(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts) const 291 | { 292 | if (numPorts > 4) 293 | { 294 | assert(numPorts < 4); 295 | numPorts = 4; 296 | } 297 | assert(numPorts > 0); 298 | 299 | d.addf("Reset detected", "%s", (resetDetected_ ? "Yes" : "No")); 300 | d.addf("Valid", "%s", (diagnosticsValid_ ? "Yes" : "No")); 301 | 302 | d.addf("EPU Errors", "%lld", epuErrorTotal_); 303 | d.addf("PDI Errors", "%lld", pdiErrorTotal_); 304 | ostringstream os, port; 305 | for (unsigned i = 0; i < numPorts; ++i) 306 | { 307 | const EthercatPortDiagnostics & pt(portDiagnostics_[i]); 308 | port.str(""); 309 | port << " Port " << i; 310 | os.str(""); 311 | os << "Status" << port.str(); 312 | d.addf(os.str(), "%s Link, %s, %s Comm", 313 | pt.hasLink ? "Has" : "No", 314 | pt.isClosed ? "Closed" : "Open", 315 | pt.hasCommunication ? "Has" : "No"); 316 | os.str(""); 317 | os << "RX Error" << port.str(); 318 | d.addf(os.str(), "%lld", pt.rxErrorTotal); 319 | os.str(""); 320 | os << "Forwarded RX Error" << port.str(); 321 | d.addf(os.str(), "%lld", pt.forwardedRxErrorTotal); 322 | os.str(""); 323 | os << "Invalid Frame" << port.str(); 324 | d.addf(os.str(), "%lld", pt.invalidFrameTotal); 325 | os.str(""); 326 | os << "Lost Link" << port.str(); 327 | d.addf(os.str(), "%lld", pt.lostLinkTotal); 328 | } 329 | 330 | if (resetDetected_) 331 | { 332 | d.mergeSummaryf(d.ERROR, "Device reset likely"); 333 | } 334 | else if (devicesRespondingToNodeAddress_ > 1) 335 | { 336 | d.mergeSummaryf(d.ERROR, "More than one device (%d) responded to node address", devicesRespondingToNodeAddress_); 337 | } 338 | else 339 | { 340 | if (diagnosticsFirst_) 341 | { 342 | d.mergeSummaryf(d.WARN, "Have not yet collected diagnostics"); 343 | } 344 | else if (!diagnosticsValid_) 345 | { 346 | d.mergeSummaryf(d.WARN, "Could not collect diagnostics"); 347 | } 348 | else 349 | { 350 | if (!portDiagnostics_[0].hasLink) 351 | { 352 | d.mergeSummaryf(d.WARN, "No link on port 0"); 353 | } 354 | } 355 | } 356 | } 357 | 358 | void EthercatDevice::construct(EtherCAT_SlaveHandler *sh, int &start_address) 359 | { 360 | sh_ = sh; 361 | sh->set_fmmu_config(new EtherCAT_FMMU_Config(0)); 362 | sh->set_pd_config(new EtherCAT_PD_Config(0)); 363 | } 364 | 365 | EthercatDevice::EthercatDevice() : use_ros_(true) 366 | { 367 | sh_ = NULL; 368 | command_size_ = 0; 369 | status_size_ = 0; 370 | newDiagnosticsIndex_ = 0; 371 | 372 | int error = pthread_mutex_init(&newDiagnosticsIndexLock_, NULL); 373 | if (error != 0) 374 | { 375 | ROS_FATAL("Initializing indexLock failed : %s", strerror(error)); 376 | sleep(1); // wait for ros to flush rosconsole output 377 | exit(EXIT_FAILURE); 378 | } 379 | 380 | error = pthread_mutex_init(&diagnosticsLock_, NULL); 381 | if (error != 0) 382 | { 383 | ROS_FATAL("Initializing diagnositcsLock failed : %s", strerror(error)); 384 | sleep(1); // wait for ros to flush rosconsole output 385 | exit(EXIT_FAILURE); 386 | } 387 | } 388 | 389 | void EthercatDevice::collectDiagnostics(EthercatCom *com) 390 | { 391 | // Really, should not need this lock, since there should only be one thread updating diagnostics. 392 | pthread_mutex_lock(&diagnosticsLock_); 393 | 394 | // Get references to diagnostics... code is easier to read 395 | unsigned oldDiagnosticsIndex = (newDiagnosticsIndex_ + 1)&1; 396 | const EthercatDeviceDiagnostics &newDiag = deviceDiagnostics[newDiagnosticsIndex_]; 397 | EthercatDeviceDiagnostics &oldDiag = deviceDiagnostics[oldDiagnosticsIndex]; 398 | 399 | // copy new diagnostics values in old diagnostics, because diagnostic data use accumumlators 400 | oldDiag = newDiag; 401 | 402 | // Collect diagnostics data into "old" buffer. 403 | // This way the "new" buffer is never changed while the publishing thread may be using it. 404 | oldDiag.collect(com, sh_); 405 | 406 | // Got new diagnostics... swap buffers. 407 | // Publisher thread uses "new" buffer. New to lock while swapping buffers. 408 | // Note : this is just changing an integer value, so it only takes a couple of instructions. 409 | pthread_mutex_lock(&newDiagnosticsIndexLock_); 410 | newDiagnosticsIndex_ = oldDiagnosticsIndex; 411 | pthread_mutex_unlock(&newDiagnosticsIndexLock_); 412 | 413 | // Done, unlock 414 | pthread_mutex_unlock(&diagnosticsLock_); 415 | } 416 | 417 | int EthercatDevice::readWriteData(EthercatCom *com, EtherCAT_SlaveHandler *sh, uint16_t address, 418 | void* buffer, uint16_t length, AddrMode addrMode) 419 | { 420 | unsigned char *p = (unsigned char *) buffer; 421 | EC_Logic *logic = sh->m_logic_instance; 422 | 423 | NPRW_Telegram nprw_telegram(logic->get_idx(), 424 | sh->get_station_address(), 425 | address, 426 | logic->get_wkc(), 427 | length, 428 | p); 429 | 430 | APRW_Telegram aprw_telegram(logic->get_idx(), // Index 431 | -sh->get_ring_position(), // Slave position on ethercat chain (auto increment address) ( 432 | address, // ESC physical memory address (start address) 433 | logic->get_wkc(), // Working counter 434 | length, // Data Length, 435 | p); // Buffer to put read result into 436 | 437 | // Put read telegram in ros_ethercat_eml/ethernet frame 438 | EC_Telegram * telegram = NULL; 439 | if (addrMode == FIXED_ADDR) 440 | { 441 | telegram = &nprw_telegram; 442 | } 443 | else if (addrMode == POSITIONAL_ADDR) 444 | { 445 | telegram = &aprw_telegram; 446 | } 447 | else 448 | { 449 | assert(0); 450 | return -1; 451 | } 452 | 453 | // Put telegram in ros_ethercat_eml/ethernet frame 454 | EC_Ethernet_Frame frame(telegram); 455 | 456 | // Send/Recv data from slave 457 | if (!com->txandrx_once(&frame)) 458 | { 459 | return -1; 460 | } 461 | 462 | // In some cases (clearing status mailbox) this is expected to occur 463 | if (telegram->get_wkc() != 3) 464 | { 465 | return -2; 466 | } 467 | 468 | return 0; 469 | } 470 | 471 | int EthercatDevice::readData(EthercatCom *com, EtherCAT_SlaveHandler *sh, uint16_t address, 472 | void* buffer, uint16_t length, AddrMode addrMode) 473 | { 474 | unsigned char *p = (unsigned char *) buffer; 475 | EC_Logic *logic = sh->m_logic_instance; 476 | 477 | NPRD_Telegram nprd_telegram(logic->get_idx(), 478 | sh->get_station_address(), 479 | address, 480 | logic->get_wkc(), 481 | length, 482 | p); 483 | 484 | APRD_Telegram aprd_telegram(logic->get_idx(), // Index 485 | -sh->get_ring_position(), // Slave position on ethercat chain (auto increment address) ( 486 | address, // ESC physical memory address (start address) 487 | logic->get_wkc(), // Working counter 488 | length, // Data Length, 489 | p); // Buffer to put read result into 490 | 491 | // Put read telegram in ros_ethercat_eml/ethernet frame 492 | EC_Telegram * telegram = NULL; 493 | if (addrMode == FIXED_ADDR) 494 | { 495 | telegram = &nprd_telegram; 496 | } 497 | else if (addrMode == POSITIONAL_ADDR) 498 | { 499 | telegram = &aprd_telegram; 500 | } 501 | else 502 | { 503 | assert(0); 504 | return -1; 505 | } 506 | 507 | // Put telegram in ros_ethercat_eml/ethernet frame 508 | EC_Ethernet_Frame frame(telegram); 509 | 510 | // Send/Recv data from slave 511 | if (!com->txandrx(&frame)) 512 | { 513 | return -1; 514 | } 515 | 516 | // In some cases (clearing status mailbox) this is expected to occur 517 | if (telegram->get_wkc() != 1) 518 | { 519 | return -2; 520 | } 521 | 522 | return 0; 523 | } 524 | 525 | int EthercatDevice::writeData(EthercatCom *com, EtherCAT_SlaveHandler *sh, uint16_t address, 526 | void const* buffer, uint16_t length, AddrMode addrMode) 527 | { 528 | unsigned char const *p = (unsigned char const*) buffer; 529 | EC_Logic *logic = sh->m_logic_instance; 530 | 531 | NPWR_Telegram npwr_telegram(logic->get_idx(), 532 | sh->get_station_address(), 533 | address, 534 | logic->get_wkc(), 535 | length, 536 | p); 537 | 538 | APWR_Telegram apwr_telegram(logic->get_idx(), // Index 539 | -sh->get_ring_position(), // Slave position on ethercat chain (auto increment address) ( 540 | address, // ESC physical memory address (start address) 541 | logic->get_wkc(), // Working counter 542 | length, // Data Length, 543 | p); // Buffer to put read result into 544 | 545 | // Put read telegram in ros_ethercat_eml/ethernet frame 546 | EC_Telegram * telegram = NULL; 547 | if (addrMode == FIXED_ADDR) 548 | { 549 | telegram = &npwr_telegram; 550 | } 551 | else if (addrMode == POSITIONAL_ADDR) 552 | { 553 | telegram = &apwr_telegram; 554 | } 555 | else 556 | { 557 | assert(0); 558 | return -1; 559 | } 560 | 561 | // Put telegram in ros_ethercat_eml/ethernet frame 562 | EC_Ethernet_Frame frame(telegram); 563 | 564 | // Send/Recv data from slave 565 | if (!com->txandrx(&frame)) 566 | { 567 | return -1; 568 | } 569 | 570 | if (telegram->get_wkc() != 1) 571 | { 572 | return -2; 573 | } 574 | 575 | return 0; 576 | } 577 | 578 | void EthercatDevice::ethercatDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned numPorts) 579 | { 580 | if (numPorts > 4) 581 | { 582 | assert(numPorts < 4); 583 | numPorts = 4; 584 | } 585 | 586 | // By locking index, diagnostics double-buffer cannot be swapped. 587 | // Update thread is only allowed to change 'old' diagnostics buffer, 588 | // so new buffer data cannot be changed while index lock is held. 589 | pthread_mutex_lock(&newDiagnosticsIndexLock_); 590 | 591 | const EthercatDeviceDiagnostics &newDiag = deviceDiagnostics[newDiagnosticsIndex_]; 592 | newDiag.publish(d, numPorts); 593 | 594 | pthread_mutex_unlock(&newDiagnosticsIndexLock_); 595 | } 596 | 597 | void EthercatDevice::diagnostics(diagnostic_updater::DiagnosticStatusWrapper &d, unsigned char *buffer) 598 | { 599 | stringstream str; 600 | str << "EtherCAT Device (" << std::setw(2) << std::setfill('0') << sh_->get_ring_position() << ")"; 601 | d.name = str.str(); 602 | str.str(""); 603 | str << sh_->get_product_code() << '-' << sh_->get_serial(); 604 | d.hardware_id = str.str(); 605 | 606 | d.message = ""; 607 | d.level = 0; 608 | 609 | d.clear(); 610 | d.addf("Position", "%02d", sh_->get_ring_position()); 611 | d.addf("Product code", "%08x", sh_->get_product_code()); 612 | d.addf("Serial", "%08x", sh_->get_serial()); 613 | d.addf("Revision", "%08x", sh_->get_revision()); 614 | 615 | this->ethercatDiagnostics(d, 4); // assume unknown device has 4 ports 616 | } 617 | 618 | void EthercatDevice::multiDiagnostics(vector &vec, unsigned char *buffer) 619 | { 620 | // Clean up recycled status object before reusing it. 621 | diagnostic_status_.clearSummary(); 622 | diagnostic_status_.clear(); 623 | 624 | // If child-class does not implement multiDiagnostics(), fall back to using slave's diagnostic() function 625 | diagnostics(diagnostic_status_, buffer); 626 | vec.push_back(diagnostic_status_); 627 | } 628 | -------------------------------------------------------------------------------- /ros_ethercat_hardware/src/ethernet_interface_info.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #include "ros_ethercat_hardware/ethernet_interface_info.h" 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | EthtoolStats::EthtoolStats() : 44 | rx_errors_(0), 45 | rx_crc_errors_(0), 46 | rx_frame_errors_(0), 47 | rx_align_errors_(0) 48 | { 49 | // empty 50 | } 51 | 52 | EthtoolStats& EthtoolStats::operator-=(const EthtoolStats& right) 53 | { 54 | this->rx_errors_ -= right.rx_errors_; 55 | this->rx_crc_errors_ -= right.rx_crc_errors_; 56 | this->rx_frame_errors_ -= right.rx_frame_errors_; 57 | this->rx_align_errors_ -= right.rx_align_errors_; 58 | return *this; 59 | } 60 | 61 | EthernetInterfaceInfo::EthernetInterfaceInfo() : 62 | sock_(-1), 63 | n_stats_(0), 64 | ethtool_stats_buf_(NULL), 65 | rx_error_index_(-1), 66 | rx_crc_error_index_(-1), 67 | rx_frame_error_index_(-1), 68 | rx_align_error_index_(-1) 69 | { 70 | } 71 | 72 | EthernetInterfaceInfo::~EthernetInterfaceInfo() 73 | { 74 | delete[] ethtool_stats_buf_; 75 | ethtool_stats_buf_ = NULL; 76 | if (sock_ >= 0) 77 | close(sock_); 78 | } 79 | 80 | void EthernetInterfaceInfo::initialize(const std::string &interface) 81 | { 82 | interface_ = interface; 83 | 84 | // Need network socket to make interface requests ioctls 85 | sock_ = socket(AF_INET, SOCK_DGRAM, 0); 86 | if (sock_ < 0) 87 | { 88 | ROS_WARN("Cannot get control socket for ioctls : %s", strerror(errno)); 89 | return; 90 | } 91 | 92 | // Get initial interface state 93 | getInterfaceState(last_state_); 94 | 95 | struct ifreq ifr; 96 | memset(&ifr, 0, sizeof (ifr)); 97 | strncpy(ifr.ifr_name, interface_.c_str(), sizeof (ifr.ifr_name)); 98 | 99 | // Determine number of statictics available from network interface 100 | struct ethtool_drvinfo drvinfo; 101 | drvinfo.cmd = ETHTOOL_GDRVINFO; 102 | ifr.ifr_data = (caddr_t) & drvinfo; 103 | int result = ioctl(sock_, SIOCETHTOOL, &ifr); 104 | if (result < 0) 105 | { 106 | ROS_WARN("Cannot get driver information for %s : %s", interface_.c_str(), strerror(errno)); 107 | return; 108 | } 109 | n_stats_ = drvinfo.n_stats; 110 | if (n_stats_ < 1) 111 | { 112 | ROS_WARN("No NIC statistics available for %s", interface_.c_str()); 113 | return; 114 | } 115 | 116 | unsigned strings_len = sizeof (ethtool_gstrings) + n_stats_ * ETH_GSTRING_LEN; 117 | char *strings_buf = new char[strings_len]; 118 | memset(strings_buf, 0, strings_len); 119 | ethtool_gstrings* strings = (ethtool_gstrings*) strings_buf; // NOLINT(readability/casting) 120 | 121 | strings->cmd = ETHTOOL_GSTRINGS; 122 | strings->string_set = ETH_SS_STATS; 123 | strings->len = n_stats_; 124 | ifr.ifr_data = (caddr_t) strings; 125 | result = ioctl(sock_, SIOCETHTOOL, &ifr); 126 | if (result < 0) 127 | { 128 | ROS_WARN("Cannot get statistics strings for %s : %s", interface_.c_str(), strerror(errno)); 129 | delete[] strings_buf; 130 | return; 131 | } 132 | 133 | for (unsigned i = 0; i < n_stats_; ++i) 134 | { 135 | if (false) 136 | { 137 | char s[ETH_GSTRING_LEN + 1]; 138 | strncpy(s, (const char*) &strings->data[i * ETH_GSTRING_LEN], ETH_GSTRING_LEN); 139 | s[ETH_GSTRING_LEN] = '\0'; 140 | ROS_WARN("Stat %i : %s", i, s); 141 | } 142 | const char *stat_name = (const char*) &strings->data[i * ETH_GSTRING_LEN]; 143 | if (strncmp("rx_errors", stat_name, ETH_GSTRING_LEN) == 0) 144 | { 145 | rx_error_index_ = i; 146 | } 147 | else if (strncmp("rx_crc_errors", stat_name, ETH_GSTRING_LEN) == 0) 148 | { 149 | rx_crc_error_index_ = i; 150 | } 151 | else if (strncmp("rx_frame_errors", stat_name, ETH_GSTRING_LEN) == 0) 152 | { 153 | rx_frame_error_index_ = i; 154 | } 155 | else if (strncmp("rx_align_errors", stat_name, ETH_GSTRING_LEN) == 0) 156 | { 157 | rx_align_error_index_ = i; 158 | } 159 | } 160 | 161 | // Everything is complete, allocate memory for ethtool_stats_ buffer 162 | // Since not all NICs provide ethtool statistics, use the presence of 163 | // ethtool_stats_ buffer to indicate initialization was a success. 164 | unsigned ethtool_stats_buf_len = sizeof (struct ethtool_stats) +n_stats_ * sizeof (uint64_t); 165 | ethtool_stats_buf_ = new char[ethtool_stats_buf_len]; 166 | 167 | if (!getEthtoolStats(orig_stats_)) 168 | { 169 | // Don't run if we can't get initial statitics 170 | ROS_WARN("Error collecting intial ethernet interface statistics"); 171 | delete[] ethtool_stats_buf_; 172 | ethtool_stats_buf_ = NULL; 173 | } 174 | } 175 | 176 | bool EthernetInterfaceInfo::getInterfaceState(InterfaceState &state) 177 | { 178 | struct ifreq ifr; 179 | memset(&ifr, 0, sizeof (ifr)); 180 | strncpy(ifr.ifr_name, interface_.c_str(), sizeof (ifr.ifr_name)); 181 | 182 | if (ioctl(sock_, SIOCGIFFLAGS, &ifr) < 0) 183 | { 184 | ROS_WARN("Cannot get interface flags for %s: %s", interface_.c_str(), strerror(errno)); 185 | return false; 186 | } 187 | 188 | state.up_ = static_cast(ifr.ifr_flags & IFF_UP); 189 | state.running_ = static_cast(ifr.ifr_flags & IFF_RUNNING); 190 | return true; 191 | } 192 | 193 | bool EthernetInterfaceInfo::getEthtoolStats(EthtoolStats &s) 194 | { 195 | if (!ethtool_stats_buf_) 196 | return false; 197 | 198 | struct ifreq ifr; 199 | memset(&ifr, 0, sizeof (ifr)); 200 | strncpy(ifr.ifr_name, interface_.c_str(), sizeof (ifr.ifr_name)); 201 | 202 | struct ethtool_stats *stats = (struct ethtool_stats *) ethtool_stats_buf_; 203 | stats->cmd = ETHTOOL_GSTATS; 204 | stats->n_stats = n_stats_; 205 | ifr.ifr_data = (caddr_t) stats; 206 | if (ioctl(sock_, SIOCETHTOOL, &ifr) < 0) 207 | { 208 | ROS_WARN("Cannot get NIC stats information for %s : %s", interface_.c_str(), strerror(errno)); 209 | return false; 210 | } 211 | 212 | if (rx_error_index_ >= 0) 213 | { 214 | s.rx_errors_ = stats->data[rx_error_index_]; 215 | } 216 | if (rx_crc_error_index_ >= 0) 217 | { 218 | s.rx_crc_errors_ = stats->data[rx_crc_error_index_]; 219 | } 220 | if (rx_frame_error_index_ >= 0) 221 | { 222 | s.rx_frame_errors_ = stats->data[rx_frame_error_index_]; 223 | } 224 | if (rx_align_error_index_ >= 0) 225 | { 226 | s.rx_align_errors_ = stats->data[rx_align_error_index_]; 227 | } 228 | 229 | return true; 230 | } 231 | 232 | void EthernetInterfaceInfo::publishDiagnostics(diagnostic_updater::DiagnosticStatusWrapper &d) 233 | { 234 | d.add("Interface", interface_); 235 | 236 | // TODO(maintainer): collect and publish information on whether interface is up/running 237 | InterfaceState state; 238 | if (getInterfaceState(state)) 239 | { 240 | if (!state.running_ && last_state_.running_) 241 | { 242 | ++lost_link_count_; 243 | } 244 | 245 | if (state.up_ && !state.running_) 246 | { 247 | d.mergeSummary(d.ERROR, "No link"); 248 | } 249 | else if (!state.up_) 250 | { 251 | d.mergeSummary(d.ERROR, "Interface down"); 252 | } 253 | 254 | d.addf("Interface State", "%s UP, %s RUNNING", state.up_ ? "" : "NOT", 255 | state.running_ ? "" : "NOT"); 256 | last_state_ = state; 257 | } 258 | else 259 | { 260 | d.add("Iface State", "ERROR"); 261 | } 262 | d.add("Lost Links", lost_link_count_); 263 | 264 | EthtoolStats stats; 265 | bool have_stats = getEthtoolStats(stats); 266 | stats -= orig_stats_; // subtract off orignal counter values 267 | 268 | if (have_stats && (rx_error_index_ >= 0)) 269 | d.addf("RX Errors", "%llu", stats.rx_errors_); 270 | else 271 | d.add("RX Errors", "N/A"); 272 | 273 | if (have_stats && (rx_crc_error_index_ >= 0)) 274 | d.addf("RX CRC Errors", "%llu", stats.rx_crc_errors_); 275 | else 276 | d.add("RX CRC Errors", "N/A"); 277 | 278 | if (have_stats && (rx_frame_error_index_ >= 0)) 279 | d.addf("RX Frame Errors", "%llu", stats.rx_frame_errors_); 280 | else 281 | d.add("RX Frame Errors", "N/A"); 282 | 283 | if (have_stats && (rx_align_error_index_ >= 0)) 284 | d.addf("RX Align Errors", "%llu", stats.rx_align_errors_); 285 | else 286 | d.add("RX Align Errors", "N/A"); 287 | } 288 | -------------------------------------------------------------------------------- /ros_ethercat_loop/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ros_ethercat_loop 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.1 (2019-02-26) 6 | ------------------ 7 | * ros_ethercat_eml moved out of this repository 8 | 9 | 0.2.0 (2015-04-07) 10 | ------------------ 11 | * Check if a port has been defined before starting EtherCAT 12 | * Check if an ethercat port has been defined 13 | * Enable rpath in ros_ethercat_loop. 14 | 15 | 0.1.8 (2014-07-18) 16 | ------------------ 17 | 18 | 0.1.7 (2014-05-23) 19 | ------------------ 20 | 21 | 0.1.6 (2014-05-15) 22 | ------------------ 23 | 24 | 0.1.5 (2014-05-14) 25 | ------------------ 26 | 27 | 0.1.4 (2014-05-14) 28 | ------------------ 29 | 30 | 0.1.3 (2014-05-13) 31 | ------------------ 32 | 33 | 0.1.2 (2014-05-13) 34 | ------------------ 35 | 36 | 0.1.1 (2014-05-12) 37 | ------------------ 38 | * first hydro release 39 | -------------------------------------------------------------------------------- /ros_ethercat_loop/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(ros_ethercat_loop) 3 | 4 | # Load catkin and all dependencies required for this package 5 | find_package(catkin REQUIRED COMPONENTS 6 | roscpp 7 | std_msgs 8 | controller_manager 9 | ros_ethercat_hardware 10 | ros_ethercat_model 11 | realtime_tools 12 | diagnostic_updater 13 | diagnostic_msgs 14 | pluginlib 15 | rosconsole_bridge 16 | ) 17 | 18 | include_directories(${catkin_INCLUDE_DIRS}) 19 | 20 | catkin_package( 21 | DEPENDS 22 | CATKIN_DEPENDS 23 | roscpp 24 | std_msgs 25 | controller_manager 26 | ros_ethercat_hardware 27 | ros_ethercat_model 28 | realtime_tools 29 | diagnostic_updater 30 | diagnostic_msgs 31 | pluginlib 32 | rosconsole_bridge 33 | ) 34 | 35 | # needed to circumvent LD_LIBRARY_PATH being emptied through ethercat_grant 36 | # in addition to not propagating ros_ethercat_loop RUNPATH to dependencies, in contrast to RPATH 37 | SET(GCC_NEWDTAGS_LINK_FLAGS "-Wl,--disable-new-dtags") 38 | SET(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} ${GCC_NEWDTAGS_LINK_FLAGS}") 39 | 40 | add_executable(${PROJECT_NAME} src/main.cpp) 41 | target_link_libraries(${PROJECT_NAME} rt ${catkin_LIBRARIES}) 42 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 43 | ros_enable_rpath(${PROJECT_NAME}) 44 | 45 | install(TARGETS ${PROJECT_NAME} 46 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 47 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 48 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 49 | ) 50 | -------------------------------------------------------------------------------- /ros_ethercat_loop/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 26 | 27 | 28 | ros_ethercat_loop 29 | 0.3.1 30 | Main loop to run EtherCAT robot hardware. 31 | Shadow Robot's software team 32 | 33 | BSD 34 | 35 | http://www.shadowrobot.com/ 36 | 37 | Manos Nikolaidis 38 | 39 | catkin 40 | 41 | roscpp 42 | std_msgs 43 | controller_manager 44 | ros_ethercat_hardware 45 | ros_ethercat_model 46 | realtime_tools 47 | diagnostic_updater 48 | diagnostic_msgs 49 | rosconsole_bridge 50 | pluginlib 51 | 52 | roscpp 53 | std_msgs 54 | controller_manager 55 | ros_ethercat_hardware 56 | ros_ethercat_model 57 | realtime_tools 58 | diagnostic_updater 59 | diagnostic_msgs 60 | pluginlib 61 | rosconsole_bridge 62 | ethercat_grant 63 | 64 | -------------------------------------------------------------------------------- /ros_ethercat_loop/src/copyright_exclusions.cfg: -------------------------------------------------------------------------------- 1 | exclude_files=/*. -------------------------------------------------------------------------------- /ros_ethercat_loop/src/main.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Modified 2014, by Shadow Robot Company Ltd. 8 | * 9 | * Redistribution and use in source and binary forms, with or without 10 | * modification, are permitted provided that the following conditions 11 | * are met: 12 | * 13 | * * Redistributions of source code must retain the above copyright 14 | * notice, this list of conditions and the following disclaimer. 15 | * * Redistributions in binary form must reproduce the above 16 | * copyright notice, this list of conditions and the following 17 | * disclaimer in the documentation and/or other materials provided 18 | * with the distribution. 19 | * * Neither the name of the Willow Garage nor the names of its 20 | * contributors may be used to endorse or promote products derived 21 | * from this software without specific prior written permission. 22 | * 23 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 | * POSSIBILITY OF SUCH DAMAGE. 35 | *********************************************************************/ 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include "ros_ethercat_model/ros_ethercat.hpp" 53 | #include 54 | #include 55 | #include 56 | #include 57 | 58 | using namespace boost::accumulators; // NOLINT(build/namespaces) 59 | using boost::ptr_vector; 60 | using std::string; 61 | using std::vector; 62 | using std::accumulate; 63 | using realtime_tools::RealtimePublisher; 64 | 65 | static struct 66 | { 67 | char *program_; 68 | char *interface_; 69 | char *rosparam_; 70 | bool allow_unprogrammed_; 71 | bool stats_; 72 | double period; 73 | } 74 | g_options; 75 | 76 | string g_robot_desc; // NOLINT(runtime/string) 77 | 78 | void Usage(const string &msg = "") 79 | { 80 | fprintf(stderr, "Usage: %s [options]\n", g_options.program_); 81 | fprintf(stderr, " Available options\n"); 82 | fprintf(stderr, " -i, --interface Connect to EtherCAT devices on this interface\n"); 83 | fprintf(stderr, " -p, --period RT loop period in msec\n"); 84 | fprintf(stderr, " -s, --stats Publish statistics on the RT loop jitter on \"ros_ros_ethercat_eml/realtime\" in seconds\n"); // NOLINT(whitespace/line_length) 85 | fprintf(stderr, " -r, --rosparam Load the robot description from this parameter name\n"); 86 | fprintf(stderr, " -u, --allow_unprogrammed Allow control loop to run with unprogrammed devices\n"); 87 | fprintf(stderr, " -h, --help Print this message and exit\n"); 88 | if (msg != "") 89 | { 90 | fprintf(stderr, "Error: %s\n", msg.c_str()); 91 | exit(-1); 92 | } 93 | else 94 | exit(0); 95 | } 96 | 97 | static int g_quit = 0; 98 | static const int SEC_2_NSEC = 1e+9; 99 | static const int SEC_2_USEC = 1e6; 100 | 101 | static struct 102 | { 103 | accumulator_set > ec_acc; 104 | accumulator_set > cm_acc; 105 | accumulator_set > loop_acc; 106 | accumulator_set > jitter_acc; 107 | int overruns; 108 | int recent_overruns; 109 | int last_overrun; 110 | int last_severe_overrun; 111 | double overrun_loop_sec; 112 | double overrun_ec; 113 | double overrun_cm; 114 | 115 | // These values are set when realtime loop does not meet performance expectations 116 | bool rt_loop_not_making_timing; 117 | double halt_rt_loop_frequency; 118 | double rt_loop_frequency; 119 | } 120 | g_stats; 121 | 122 | static void publishDiagnostics(RealtimePublisher &publisher) 123 | { 124 | if (publisher.trylock()) 125 | { 126 | accumulator_set > zero; 127 | vector statuses; 128 | diagnostic_updater::DiagnosticStatusWrapper status; 129 | 130 | static double max_ec = 0, max_cm = 0, max_loop = 0, max_jitter = 0; 131 | double avg_ec, avg_cm, avg_loop, avg_jitter; 132 | 133 | avg_ec = extract_result(g_stats.ec_acc); 134 | avg_cm = extract_result(g_stats.cm_acc); 135 | avg_loop = extract_result(g_stats.loop_acc); 136 | max_ec = std::max(max_ec, extract_result(g_stats.ec_acc)); 137 | max_cm = std::max(max_cm, extract_result(g_stats.cm_acc)); 138 | max_loop = std::max(max_loop, extract_result(g_stats.loop_acc)); 139 | g_stats.ec_acc = zero; 140 | g_stats.cm_acc = zero; 141 | g_stats.loop_acc = zero; 142 | 143 | // Publish average loop jitter 144 | avg_jitter = extract_result(g_stats.jitter_acc); 145 | max_jitter = std::max(max_jitter, extract_result(g_stats.jitter_acc)); 146 | g_stats.jitter_acc = zero; 147 | 148 | static bool first = true; 149 | if (first) 150 | { 151 | first = false; 152 | status.add("Robot Description", g_robot_desc); 153 | } 154 | 155 | status.addf("Max EtherCAT roundtrip (us)", "%.2f", max_ec * SEC_2_USEC); 156 | status.addf("Avg EtherCAT roundtrip (us)", "%.2f", avg_ec * SEC_2_USEC); 157 | status.addf("Max Controller Manager roundtrip (us)", "%.2f", max_cm * SEC_2_USEC); 158 | status.addf("Avg Controller Manager roundtrip (us)", "%.2f", avg_cm * SEC_2_USEC); 159 | status.addf("Max Total Loop roundtrip (us)", "%.2f", max_loop * SEC_2_USEC); 160 | status.addf("Avg Total Loop roundtrip (us)", "%.2f", avg_loop * SEC_2_USEC); 161 | status.addf("Max Loop Jitter (us)", "%.2f", max_jitter * SEC_2_USEC); 162 | status.addf("Avg Loop Jitter (us)", "%.2f", avg_jitter * SEC_2_USEC); 163 | status.addf("Control Loop Overruns", "%d", g_stats.overruns); 164 | status.addf("Recent Control Loop Overruns", "%d", g_stats.recent_overruns); 165 | status.addf("Last Control Loop Overrun Cause", "ec: %.2fus, cm: %.2fus", 166 | g_stats.overrun_ec*SEC_2_USEC, g_stats.overrun_cm * SEC_2_USEC); 167 | status.addf("Last Overrun Loop Time (us)", "%.2f", g_stats.overrun_loop_sec * SEC_2_USEC); 168 | status.addf("Realtime Loop Frequency", "%.4f", g_stats.rt_loop_frequency); 169 | 170 | status.name = "Realtime Control Loop"; 171 | if (g_stats.overruns > 0 && g_stats.last_overrun < 30) 172 | { 173 | if (g_stats.last_severe_overrun < 30) 174 | status.level = 1; 175 | else 176 | status.level = 0; 177 | status.message = "Realtime loop used too much time in the last 30 seconds."; 178 | } 179 | else 180 | { 181 | status.level = 0; 182 | status.message = "OK"; 183 | } 184 | g_stats.recent_overruns = 0; 185 | ++g_stats.last_overrun; 186 | ++g_stats.last_severe_overrun; 187 | 188 | if (g_stats.rt_loop_not_making_timing) 189 | status.mergeSummaryf(status.ERROR, "realtime loop only ran at %.4f Hz", g_stats.halt_rt_loop_frequency); 190 | 191 | statuses.push_back(status); 192 | publisher.msg_.status = statuses; 193 | publisher.msg_.header.stamp = ros::Time::now(); 194 | publisher.unlockAndPublish(); 195 | } 196 | } 197 | 198 | static inline double now() 199 | { 200 | struct timespec n; 201 | clock_gettime(CLOCK_MONOTONIC, &n); 202 | return static_cast(n.tv_nsec) / SEC_2_NSEC + n.tv_sec; 203 | } 204 | 205 | void *diagnosticLoop(void *args) 206 | { 207 | ptr_vector* ec = (ptr_vector*) args; 208 | struct timespec tick; 209 | clock_gettime(CLOCK_MONOTONIC, &tick); 210 | while (!g_quit) 211 | { 212 | for (ptr_vector::iterator eh = ec->begin(); eh != ec->end(); ++eh) 213 | { 214 | eh->collectDiagnostics(); 215 | } 216 | ++tick.tv_sec; 217 | clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &tick, NULL); 218 | } 219 | return NULL; 220 | } 221 | 222 | static void timespecInc(struct timespec &tick, int nsec) 223 | { 224 | tick.tv_nsec += nsec; 225 | while (tick.tv_nsec >= SEC_2_NSEC) 226 | { 227 | tick.tv_nsec -= SEC_2_NSEC; 228 | ++tick.tv_sec; 229 | } 230 | } 231 | 232 | class RTLoopHistory 233 | { 234 | public: 235 | RTLoopHistory(unsigned length, double default_value) : 236 | index_(0), 237 | length_(length), 238 | history_(length, default_value) 239 | { 240 | } 241 | 242 | void sample(double value) 243 | { 244 | index_ = (index_ + 1) % length_; 245 | history_[index_] = value; 246 | } 247 | 248 | double average() const 249 | { 250 | return accumulate(history_.begin(), history_.end(), 0.0) / (double) length_; // NOLINT(readability/casting) 251 | } 252 | 253 | protected: 254 | unsigned index_; 255 | unsigned length_; 256 | vector history_; 257 | }; 258 | 259 | static void* terminate_control(RealtimePublisher *publisher, 260 | RealtimePublisher *rtpublisher, 261 | const char* message, 262 | const char* data = NULL) 263 | { 264 | ROS_FATAL(message, data); 265 | publisher->stop(); 266 | delete rtpublisher; 267 | ros::shutdown(); 268 | return (void*) - 1; // NOLINT(readability/casting) 269 | } 270 | 271 | void *controlLoop(void *) // NOLINT(readability/casting) 272 | { 273 | double last_published, last_loop_start; 274 | int policy; 275 | tinyxml2::XMLElement *root; 276 | tinyxml2::XMLElement *root_element; 277 | 278 | ros::NodeHandle node(name); 279 | 280 | RealtimePublisher publisher(node, "/diagnostics", 2); 281 | RealtimePublisher *rtpublisher = NULL; 282 | 283 | // Realtime loop should be running at least 3/4 of given frequency 284 | // or at specified min acceptable frequency 285 | double period_in_secs = 1e+9 * g_options.period; 286 | double given_frequency = 1 / period_in_secs; 287 | double min_acceptable_rt_loop_frequency = 0.75 * given_frequency; 288 | if (node.getParam("min_acceptable_rt_loop_frequency", min_acceptable_rt_loop_frequency)) 289 | ROS_WARN("min_acceptable_rt_loop_frequency changed to %f", min_acceptable_rt_loop_frequency); 290 | 291 | unsigned rt_cycle_count = 0; 292 | double last_rt_monitor_time; 293 | 294 | // Calculate realtime loop frequency every 200msec 295 | double rt_loop_monitor_period = 0.2; 296 | // Keep history of last 3 calculation intervals. 297 | RTLoopHistory rt_loop_history(3, 1000.0); 298 | 299 | if (g_options.stats_) 300 | rtpublisher = new RealtimePublisher(node, "realtime", 2); 301 | 302 | // Load robot description 303 | tinyxml2::XMLDocument xml; 304 | struct stat st; 305 | 306 | if (ros::param::get(g_options.rosparam_, g_robot_desc)) 307 | xml.Parse(g_robot_desc.c_str()); 308 | else 309 | return terminate_control(&publisher, rtpublisher, 310 | "Could not load the xml from parameter server: %s", g_options.rosparam_); 311 | 312 | root_element = xml.RootElement(); 313 | root = xml.FirstChildElement("robot"); 314 | if (!root || !root_element) 315 | return terminate_control(&publisher, rtpublisher, "Failed to parse the xml from %s", g_options.rosparam_); 316 | 317 | // Initialize the hardware interface 318 | ros::NodeHandle nh; 319 | RosEthercat seth(nh, g_options.interface_, g_options.allow_unprogrammed_, root); 320 | 321 | // Create controller manager 322 | controller_manager::ControllerManager cm(&seth); 323 | 324 | // Publish one-time before entering real-time to pre-allocate message vectors 325 | publishDiagnostics(publisher); 326 | 327 | // Start Non-realtime diagnostic thread 328 | static pthread_t diagnosticThread; 329 | int rv = pthread_create(&diagnosticThread, NULL, diagnosticLoop, &seth.ethercat_hardware_); 330 | if (rv != 0) 331 | return terminate_control(&publisher, rtpublisher, 332 | "Unable to create control thread: rv = %s", boost::lexical_cast(rv).c_str()); 333 | 334 | // Set to realtime scheduler for this thread 335 | struct sched_param thread_param; 336 | policy = SCHED_FIFO; 337 | thread_param.sched_priority = sched_get_priority_max(policy); 338 | pthread_setschedparam(pthread_self(), policy, &thread_param); 339 | 340 | struct timespec tick; 341 | clock_gettime(CLOCK_REALTIME, &tick); 342 | ros::Duration durp(g_options.period / 1e+9); 343 | 344 | // Snap to the nearest second 345 | tick.tv_nsec = (tick.tv_nsec / g_options.period + 1) * g_options.period; 346 | clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tick, NULL); 347 | 348 | last_published = now(); 349 | last_rt_monitor_time = now(); 350 | last_loop_start = now(); 351 | while (!g_quit) 352 | { 353 | // Track how long the actual loop takes 354 | double this_loop_start = now(); 355 | g_stats.loop_acc(this_loop_start - last_loop_start); 356 | last_loop_start = this_loop_start; 357 | 358 | double start = now(); 359 | 360 | ros::Time this_moment(tick.tv_sec, tick.tv_nsec); 361 | seth.read(this_moment, durp); 362 | double after_ec = now(); 363 | cm.update(this_moment, durp); 364 | seth.write(this_moment, durp); 365 | double end = now(); 366 | 367 | g_stats.ec_acc(after_ec - start); 368 | g_stats.cm_acc(end - after_ec); 369 | 370 | if ((end - last_published) > 1.0) 371 | { 372 | publishDiagnostics(publisher); 373 | last_published = end; 374 | } 375 | 376 | // Realtime loop should run about with the set frequency by default 1000Hz. 377 | // Missing timing on control cycles usually causes a controller glitch and actuators to jerk. 378 | // When realtime loop misses a lot of cycles controllers will perform poorly and may cause robot to shake. 379 | ++rt_cycle_count; 380 | if ((start - last_rt_monitor_time) > rt_loop_monitor_period) 381 | { 382 | // Calculate new average rt loop frequency 383 | double rt_loop_frequency = static_cast(rt_cycle_count) / rt_loop_monitor_period; 384 | 385 | // Use last X samples of frequency when deciding whether or not to halt 386 | rt_loop_history.sample(rt_loop_frequency); 387 | double avg_rt_loop_frequency = rt_loop_history.average(); 388 | if (avg_rt_loop_frequency < min_acceptable_rt_loop_frequency) 389 | { 390 | if (!g_stats.rt_loop_not_making_timing) 391 | { 392 | // Only update this value if motors when this first occurs (used for diagnostics error message) 393 | g_stats.halt_rt_loop_frequency = avg_rt_loop_frequency; 394 | } 395 | g_stats.rt_loop_not_making_timing = true; 396 | } 397 | g_stats.rt_loop_frequency = avg_rt_loop_frequency; 398 | rt_cycle_count = 0; 399 | last_rt_monitor_time = start; 400 | } 401 | 402 | // Compute end of next g_options.period 403 | timespecInc(tick, g_options.period); 404 | 405 | struct timespec before; 406 | clock_gettime(CLOCK_REALTIME, &before); 407 | if ((before.tv_sec + static_cast(before.tv_nsec) / SEC_2_NSEC) > (tick.tv_sec + static_cast(tick.tv_nsec) / SEC_2_NSEC)) // NOLINT(whitespace/line_length) 408 | { 409 | // Total amount of time the loop took to run 410 | g_stats.overrun_loop_sec = (before.tv_sec + static_cast(before.tv_nsec) / SEC_2_NSEC) - 411 | (tick.tv_sec + static_cast(tick.tv_nsec) / SEC_2_NSEC); 412 | 413 | // We overran, snap to next "g_options.period" 414 | tick.tv_sec = before.tv_sec; 415 | tick.tv_nsec = (before.tv_nsec / g_options.period) * g_options.period; 416 | timespecInc(tick, g_options.period); 417 | 418 | // initialize overruns 419 | if (g_stats.overruns == 0) 420 | { 421 | g_stats.last_overrun = 1000; 422 | g_stats.last_severe_overrun = 1000; 423 | } 424 | // check for overruns 425 | if (g_stats.recent_overruns > 10) 426 | g_stats.last_severe_overrun = 0; 427 | g_stats.last_overrun = 0; 428 | 429 | ++g_stats.overruns; 430 | ++g_stats.recent_overruns; 431 | g_stats.overrun_ec = after_ec - start; 432 | g_stats.overrun_cm = end - after_ec; 433 | } 434 | 435 | // Sleep until end of g_options.period 436 | clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tick, NULL); 437 | 438 | // Calculate RT loop jitter 439 | struct timespec after; 440 | clock_gettime(CLOCK_REALTIME, &after); 441 | double jitter = (after.tv_sec - tick.tv_sec + static_cast(after.tv_nsec - tick.tv_nsec) / SEC_2_NSEC); 442 | 443 | g_stats.jitter_acc(jitter); 444 | 445 | // Publish realtime loops statistics, if requested 446 | if (rtpublisher && rtpublisher->trylock()) 447 | { 448 | rtpublisher->msg_.data = jitter; 449 | rtpublisher->unlockAndPublish(); 450 | } 451 | } 452 | 453 | // Shutdown all of the motors on exit 454 | seth.shutdown(); 455 | 456 | publisher.stop(); 457 | delete rtpublisher; 458 | ros::shutdown(); 459 | return NULL; 460 | } 461 | 462 | void quitRequested(int sig) 463 | { 464 | g_quit = 1; 465 | } 466 | 467 | static int lock_fd(int fd) 468 | { 469 | struct flock lock; 470 | 471 | lock.l_type = F_WRLCK; 472 | lock.l_whence = SEEK_SET; 473 | lock.l_start = 0; 474 | lock.l_len = 0; 475 | 476 | return fcntl(fd, F_SETLK, &lock); 477 | } 478 | 479 | 480 | static const char* PIDDIR = "/var/tmp/run/"; 481 | 482 | string generatePIDFilename(const char* interface) 483 | { 484 | string filename; 485 | filename = string(PIDDIR) + "EtherCAT_" + string(interface) + ".pid"; 486 | return filename; 487 | } 488 | 489 | static int setupPidFile(const char* interface) 490 | { 491 | pid_t pid; 492 | int fd; 493 | FILE *fp = NULL; 494 | 495 | string filename = generatePIDFilename(interface); 496 | 497 | umask(0); 498 | mkdir(PIDDIR, 0777); 499 | int PID_FLAGS = O_RDWR | O_CREAT | O_EXCL; 500 | int PID_MODE = S_IWUSR | S_IRUSR | S_IWGRP | S_IRGRP | S_IWOTH | S_IROTH; 501 | fd = open(filename.c_str(), PID_FLAGS, PID_MODE); 502 | if (fd == -1) 503 | { 504 | if (errno != EEXIST) 505 | { 506 | ROS_FATAL("Unable to create pid file '%s': %s", filename.c_str(), strerror(errno)); 507 | return -1; 508 | } 509 | 510 | if ((fd = open(filename.c_str(), O_RDWR)) < 0) 511 | { 512 | ROS_FATAL("Unable to open pid file '%s': %s", filename.c_str(), strerror(errno)); 513 | return -1; 514 | } 515 | 516 | if ((fp = fdopen(fd, "rw")) == NULL) 517 | { 518 | ROS_FATAL("Can't read from '%s': %s", filename.c_str(), strerror(errno)); 519 | return -1; 520 | } 521 | pid = -1; 522 | if ((fscanf(fp, "%d", &pid) != 1) || (pid == getpid()) || (lock_fd(fileno(fp)) == 0)) 523 | { 524 | int rc; 525 | 526 | if ((rc = unlink(filename.c_str())) == -1) 527 | { 528 | ROS_FATAL("Can't remove stale pid file '%s': %s", filename.c_str(), strerror(errno)); 529 | return -1; 530 | } 531 | } 532 | else 533 | { 534 | ROS_FATAL("Another instance of ros_ethercat is already running with pid: %d", pid); 535 | return -1; 536 | } 537 | } 538 | 539 | unlink(filename.c_str()); 540 | fd = open(filename.c_str(), PID_FLAGS, PID_MODE); 541 | 542 | if (fd == -1) 543 | { 544 | ROS_FATAL("Unable to open pid file '%s': %s", filename.c_str(), strerror(errno)); 545 | return -1; 546 | } 547 | 548 | if (lock_fd(fd) == -1) 549 | { 550 | ROS_FATAL("Unable to lock pid file '%s': %s", filename.c_str(), strerror(errno)); 551 | return -1; 552 | } 553 | 554 | if ((fp = fdopen(fd, "w")) == NULL) 555 | { 556 | ROS_FATAL("fdopen failed: %s", strerror(errno)); 557 | return -1; 558 | } 559 | 560 | fprintf(fp, "%d\n", getpid()); 561 | 562 | /* We do NOT close fd, since we want to keep the lock. */ 563 | fflush(fp); 564 | fcntl(fd, F_SETFD, (long) 1); // NOLINT(runtime/int) 565 | 566 | return 0; 567 | } 568 | 569 | static void cleanupPidFile(const char* interface) 570 | { 571 | string filename = generatePIDFilename(interface); 572 | unlink(filename.c_str()); 573 | } 574 | 575 | #define CLOCK_PRIO 0 576 | #define CONTROL_PRIO 0 577 | 578 | REGISTER_ROSCONSOLE_BRIDGE; 579 | 580 | static pthread_t controlThread; 581 | static pthread_attr_t controlThreadAttr; 582 | 583 | int main(int argc, char *argv[]) 584 | { 585 | // Keep the kernel from swapping us out 586 | if (mlockall(MCL_CURRENT | MCL_FUTURE) < 0) 587 | { 588 | perror("Failed to lock memory. It is recommended to do rosrun ros_ethercat_loop ethercat_grant"); 589 | exit(EXIT_FAILURE); 590 | } 591 | 592 | // Initialize ROS and parse command-line arguments 593 | ros::init(argc, argv, "realtime_loop"); 594 | 595 | // Parse options 596 | g_options.program_ = argv[0]; 597 | g_options.rosparam_ = NULL; 598 | g_options.period = 1e+6; // 1 ms in nanoseconds 599 | 600 | while (true) 601 | { 602 | static struct option long_options[] = 603 | { 604 | {"help", no_argument, 0, 'h'}, 605 | {"stats", no_argument, 0, 's'}, 606 | {"allow_unprogrammed", no_argument, 0, 'u'}, 607 | {"interface", required_argument, 0, 'i'}, 608 | {"rosparam", required_argument, 0, 'r'}, 609 | {"period", no_argument, 0, 'p'}, 610 | }; 611 | int option_index = 0; 612 | int c = getopt_long(argc, argv, "hi:usx:r:", long_options, &option_index); 613 | if (c == -1) 614 | break; 615 | 616 | switch (c) 617 | { 618 | case 'h': 619 | Usage(); 620 | break; 621 | case 'u': 622 | g_options.allow_unprogrammed_ = true; 623 | break; 624 | case 'i': 625 | g_options.interface_ = optarg; 626 | break; 627 | case 'r': 628 | g_options.rosparam_ = optarg; 629 | break; 630 | case 's': 631 | g_options.stats_ = true; 632 | break; 633 | case 'p': 634 | // convert period given in msec to nsec 635 | g_options.period = fabs(atof(optarg))*1e+6; 636 | break; 637 | } 638 | } 639 | if (optind < argc) 640 | Usage("Extra arguments"); 641 | 642 | if (!g_options.interface_) 643 | Usage("You must specify a network interface"); 644 | if (!g_options.rosparam_) 645 | Usage("You must specify a rosparam for robot description"); 646 | 647 | // EtherCAT lock for this interface (e.g. Ethernet port) 648 | if (setupPidFile(g_options.interface_) < 0) 649 | exit(EXIT_FAILURE); 650 | 651 | ros::NodeHandle node(name); 652 | 653 | // Catch attempts to quit 654 | signal(SIGTERM, quitRequested); 655 | signal(SIGINT, quitRequested); 656 | signal(SIGHUP, quitRequested); 657 | 658 | // Start thread 659 | int rv = pthread_create(&controlThread, &controlThreadAttr, controlLoop, 0); 660 | if (rv != 0) 661 | { 662 | ROS_FATAL("Unable to create control thread: rv = %d", rv); 663 | exit(EXIT_FAILURE); 664 | } 665 | 666 | ros::spin(); 667 | pthread_join(controlThread, (void **) &rv); // NOLINT(readability/casting) 668 | 669 | // Cleanup pid files 670 | cleanupPidFile(NULL); 671 | cleanupPidFile(g_options.interface_); 672 | 673 | return rv; 674 | } 675 | 676 | -------------------------------------------------------------------------------- /ros_ethercat_model/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ros_ethercat_model 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.3.1 (2019-02-26) 6 | ------------------ 7 | * ros_ethercat_eml moved out of this repository 8 | 9 | 0.3.0 (2015-07-20) 10 | ------------------ 11 | * checking type as an element as well as attribute, for new transmission style 12 | * ignore transmissions with no type instead of crashing 13 | 14 | 0.2.0 (2015-04-07) 15 | ------------------ 16 | * Added shutdown-timeout=1.0 to improve controller shutdown time 17 | * Check if a port has been defined before starting EtherCAT 18 | * Check if an ethercat port has been defined 19 | * Fix wait for calibration problem 20 | * Add cmake function to enable rpath 21 | * bug fixes in initialization 22 | * added and registered JointCommandInterfaces 23 | * added install directives 24 | 25 | 0.1.8 (2014-07-18) 26 | ------------------ 27 | 28 | 0.1.7 (2014-05-23) 29 | ------------------ 30 | 31 | 0.1.6 (2014-05-15) 32 | ------------------ 33 | 34 | 0.1.5 (2014-05-14) 35 | ------------------ 36 | 37 | 0.1.4 (2014-05-14) 38 | ------------------ 39 | 40 | 0.1.3 (2014-05-13) 41 | ------------------ 42 | 43 | 0.1.2 (2014-05-13) 44 | ------------------ 45 | 46 | 0.1.1 (2014-05-12) 47 | ------------------ 48 | * first hydro release 49 | -------------------------------------------------------------------------------- /ros_ethercat_model/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # http://ros.org/doc/groovy/api/catkin/html/user_guide/supposed.html 2 | cmake_minimum_required(VERSION 3.0.2) 3 | project(ros_ethercat_model) 4 | 5 | # Load catkin and all dependencies required for this package 6 | find_package(catkin REQUIRED COMPONENTS 7 | roscpp 8 | hardware_interface 9 | pluginlib 10 | urdf 11 | kdl_parser 12 | sr_robot_msgs 13 | cmake_modules 14 | realtime_tools 15 | ros_ethercat_hardware 16 | ) 17 | 18 | find_package(Boost REQUIRED COMPONENTS system thread) 19 | find_package(TinyXML2 REQUIRED) 20 | 21 | set( TinyXML2_INCLUDE_DIRS ${TinyXML2_INCLUDE_DIR} ) 22 | set( TinyXML2_LIBRARIES ${TinyXML2_LIBRARY} ) 23 | 24 | include_directories(include 25 | ${Boost_INCLUDE_DIRS} 26 | ${catkin_INCLUDE_DIRS} 27 | ${TinyXML2_INCLUDE_DIRS}) 28 | 29 | catkin_package( 30 | DEPENDS 31 | TinyXML2 32 | Boost 33 | CATKIN_DEPENDS 34 | roscpp 35 | hardware_interface 36 | pluginlib 37 | urdf 38 | kdl_parser 39 | sr_robot_msgs 40 | realtime_tools 41 | ros_ethercat_hardware 42 | INCLUDE_DIRS 43 | include 44 | LIBRARIES 45 | ${PROJECT_NAME} 46 | CFG_EXTRAS 47 | ${PROJECT_NAME}-extras.cmake 48 | ) 49 | 50 | add_library(${PROJECT_NAME} src/ros_ethercat.cpp) 51 | 52 | add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 53 | 54 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${TinyXML2_LIBRARIES}) 55 | 56 | 57 | install(DIRECTORY include/${PROJECT_NAME}/ 58 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 59 | ) 60 | 61 | install(DIRECTORY launch/ 62 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 63 | ) 64 | 65 | install(FILES ethercat_robot_hw_plugin.xml 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 67 | 68 | install(TARGETS ${PROJECT_NAME} 69 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 70 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 71 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}) 72 | -------------------------------------------------------------------------------- /ros_ethercat_model/cmake/ros_ethercat_model-extras.cmake.em: -------------------------------------------------------------------------------- 1 | 2 | function(ros_enable_rpath target) 3 | # Set ${target} with RPATH built in so that we can install it suid 4 | set_target_properties(${target} PROPERTIES SKIP_BUILD_RPATH FALSE) 5 | 6 | # Set the install RPATH to the install path 7 | set(RPATH "${CMAKE_INSTALL_PREFIX}/${CATKIN_PACKAGE_LIB_DESTINATION}") 8 | 9 | # If LD_LIBRARY_PATH is set, add it to the install RPATH 10 | # this works in a normal catkin environment, but fails if the user unsets 11 | # their LD_LIBRARY_PATH manually for some reason 12 | if(DEFINED ENV{LD_LIBRARY_PATH}) 13 | set(RPATH "${RPATH}:$ENV{LD_LIBRARY_PATH}") 14 | endif() 15 | 16 | message(STATUS "Install RPATH for ${target} is ${RPATH}") 17 | 18 | # Apply our computed RPATH to the target 19 | set_target_properties(${target} PROPERTIES INSTALL_RPATH ${RPATH}) 20 | 21 | # Don't use the final RPATH in devel space 22 | set_target_properties(${target} PROPERTIES BUILD_WITH_INSTALL_RPATH FALSE) 23 | endfunction() 24 | -------------------------------------------------------------------------------- /ros_ethercat_model/ethercat_robot_hw_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 26 | 27 | 28 | 29 | 30 | A RobotHW class that deals with etherCAT devices 31 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /ros_ethercat_model/include/ros_ethercat_model/chain.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2008, Willow Garage, Inc. 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | // Author: Stuart Glaser 31 | 32 | #ifndef ROS_ETHERCAT_MODEL_CHAIN_H 33 | #define ROS_ETHERCAT_MODEL_CHAIN_H 34 | 35 | #include "ros_ethercat_model/robot_state.hpp" 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | namespace ros_ethercat_model 46 | { 47 | 48 | class Chain 49 | { 50 | public: 51 | /* \brief initialize the chain object 52 | * 53 | * \param robot_state the robot state object containing the robot model and the state of each joint in the robot 54 | * \param root the name of the root link of the chain 55 | * \param tip the name of the tip link of the chain 56 | * 57 | */ 58 | bool init(RobotState *robot_state, const std::string &root, const std::string &tip) 59 | { 60 | robot_state_ = robot_state; 61 | // Constructs the kdl chain 62 | KDL::Tree kdl_tree; 63 | if (!kdl_parser::treeFromUrdfModel(robot_state->robot_model_, kdl_tree)) 64 | { 65 | ROS_ERROR("Could not convert urdf into kdl tree"); 66 | return false; 67 | } 68 | bool res; 69 | try 70 | { 71 | res = kdl_tree.getChain(root, tip, kdl_chain_); 72 | } 73 | catch (...) 74 | { 75 | res = false; 76 | } 77 | if (!res) 78 | { 79 | ROS_ERROR("Could not extract chain between %s and %s from kdl tree", 80 | root.c_str(), tip.c_str()); 81 | return false; 82 | } 83 | // Pulls out all the joint indices 84 | joints_.clear(); 85 | for (size_t i = 0; i < kdl_chain_.getNrOfSegments(); i++) 86 | { 87 | if (kdl_chain_.getSegment(i).getJoint().getType() != KDL::Joint::None) 88 | { 89 | JointState* jnt = robot_state->getJointState(kdl_chain_.getSegment(i).getJoint().getName()); 90 | if (!jnt) 91 | { 92 | ROS_ERROR("Joint '%s' is not found in joint state vector", 93 | kdl_chain_.getSegment(i).getJoint().getName().c_str()); 94 | return false; 95 | } 96 | joints_.push_back(jnt); 97 | } 98 | } 99 | ROS_DEBUG("Added %i joints", static_cast(joints_.size())); 100 | return true; 101 | } 102 | void getPositions(std::vector &positions) 103 | { 104 | positions.clear(); 105 | for (unsigned int i = 0; i < joints_.size(); ++i) 106 | positions.push_back(joints_[i]->position_); 107 | } 108 | void getPositions(KDL::JntArray &a) 109 | { 110 | assert(a.rows() == joints_.size()); 111 | for (unsigned int i = 0; i < joints_.size(); ++i) 112 | a(i) = joints_[i]->position_; 113 | } 114 | 115 | /// gets the joint positions of the chain as any type with size() and [] 116 | template void getPositions(Vec &v) 117 | { 118 | assert((static_cast) v.size() == (static_cast) joints_.size()); 119 | for (size_t i = 0; i < joints_.size(); ++i) 120 | v[i] = joints_[i]->position_; 121 | } 122 | 123 | /// get the joint velocities of the chain as a std vector 124 | void getVelocities(std::vector &velocities) 125 | { 126 | velocities.clear(); 127 | for (unsigned int i = 0; i < joints_.size(); ++i) 128 | velocities.push_back(joints_[i]->velocity_); 129 | } 130 | /// get the joint velocities and position of the chain as a kdl jnt array vel. Fills in the positions too. 131 | void getVelocities(KDL::JntArrayVel &a) 132 | { 133 | assert(a.q.rows() == joints_.size()); 134 | assert(a.qdot.rows() == joints_.size()); 135 | for (unsigned int i = 0; i < joints_.size(); ++i) 136 | { 137 | a.q(i) = joints_[i]->position_; 138 | a.qdot(i) = joints_[i]->velocity_; 139 | } 140 | } 141 | 142 | /// gets the joint velocities of the chain as any type with size() and [] 143 | template void getVelocities(Vec &v) 144 | { 145 | assert((static_cast) v.size() == (static_cast) joints_.size()); 146 | for (size_t i = 0; i < joints_.size(); ++i) 147 | v[i] = joints_[i]->velocity_; 148 | } 149 | void getEfforts(std::vector &efforts) 150 | { 151 | efforts.clear(); 152 | for (unsigned int i = 0; i < joints_.size(); ++i) 153 | efforts.push_back(joints_[i]->effort_); 154 | } 155 | 156 | /// get the measured joint efforts of the chain as a kdl jnt array 157 | void getEfforts(KDL::JntArray &a) 158 | { 159 | assert(a.rows() == joints_.size()); 160 | for (unsigned int i = 0; i < joints_.size(); ++i) 161 | a(i) = joints_[i]->effort_; 162 | } 163 | 164 | /// set the commanded joint efforts of the chain as a std vector 165 | void setEfforts(KDL::JntArray &a) 166 | { 167 | assert(a.rows() == joints_.size()); 168 | for (unsigned int i = 0; i < joints_.size(); ++i) 169 | joints_[i]->commanded_effort_ = a(i); 170 | } 171 | 172 | /// set the commanded joint efforts of the chain as a kdl jnt array 173 | void addEfforts(KDL::JntArray &a) 174 | { 175 | assert(a.rows() == joints_.size()); 176 | for (unsigned int i = 0; i < joints_.size(); ++i) 177 | joints_[i]->commanded_effort_ += a(i); 178 | } 179 | 180 | /// Adds efforts from any type that implements size() and [] lookup. 181 | template void addEfforts(const Vec& v) 182 | { 183 | assert((static_cast) v.size() == (static_cast) joints_.size()); 184 | for (size_t i = 0; i < joints_.size(); ++i) 185 | joints_[i]->commanded_effort_ += v[i]; 186 | } 187 | 188 | /// get a kdl chain object that represents the chain from root to tip 189 | void toKDL(KDL::Chain &chain) 190 | { 191 | chain = kdl_chain_; 192 | } 193 | /* \brief get a joint state of an actuated joint of the chain. 194 | * 195 | * the actuated_joint_i index starts at zero 196 | * fixed joints are ignored in the list of actuated joints 197 | */ 198 | JointState* getJoint(unsigned int actuated_joint_i) 199 | { 200 | if (actuated_joint_i >= joints_.size()) 201 | return NULL; 202 | else 203 | return joints_[actuated_joint_i]; 204 | } 205 | 206 | /// Returns the number of actuated joints in the chain 207 | int size() const 208 | { 209 | return joints_.size(); 210 | } 211 | 212 | private: 213 | RobotState *robot_state_; 214 | KDL::Chain kdl_chain_; 215 | 216 | std::vector< JointState* > joints_; // ONLY joints that can be actuated (not fixed joints) 217 | }; 218 | } // namespace ros_ethercat_model 219 | 220 | #endif 221 | -------------------------------------------------------------------------------- /ros_ethercat_model/include/ros_ethercat_model/copyright_exclusions.cfg: -------------------------------------------------------------------------------- 1 | exclude_files=/*. -------------------------------------------------------------------------------- /ros_ethercat_model/include/ros_ethercat_model/hardware_interface.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef ROS_ETHERCAT_MODEL_HARDWARE_INTERFACE_H 36 | #define ROS_ETHERCAT_MODEL_HARDWARE_INTERFACE_H 37 | 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | 44 | namespace ros_ethercat_model 45 | { 46 | 47 | class ActuatorState 48 | { 49 | public: 50 | ActuatorState() : 51 | device_id_(0), 52 | position_(0), 53 | clutch_position_(0), 54 | velocity_(0), 55 | effort_(0), 56 | commanded_effort_(0), 57 | last_commanded_current_(0.0), 58 | last_measured_current_(0.0), 59 | last_commanded_effort_(0.0), 60 | last_measured_effort_(0.0), 61 | max_effort_(0.0), 62 | motor_voltage_(0.0), 63 | flags_(0) 64 | { 65 | } 66 | 67 | int device_id_; //!< Position in EtherCAT chain 68 | 69 | double position_; //!< The position of the motor (in radians) 70 | double velocity_; //!< The velocity in radians per second 71 | double effort_; //!< Measured effort in Nm 72 | int effort_raw_; //!< Raw adc coming from effort sensor; 73 | double current_; //!< Current in Amps 74 | double commanded_effort_; 75 | 76 | double temperature_; //!< Measured motor temperature in degrees C 77 | unsigned int flags_; //!< Motor state 78 | 79 | double clutch_position_; //!< Position of output of actuator, distally to the clutch. 80 | 81 | 82 | double last_commanded_current_; //!< Current computed based on effort specified in ActuatorCommand (in amps) 83 | double last_measured_current_; //!< The measured current (in amps) 84 | 85 | double last_commanded_effort_; //!< The torque requested in the previous ActuatorCommand (in Nm) 86 | double last_measured_effort_; //!< The measured torque (in Nm) 87 | 88 | double max_effort_; //!< Absolute torque limit for actuator (derived from motor current limit). (in Nm) 89 | 90 | double motor_voltage_; //!< Motor voltage (in volts) 91 | }; 92 | 93 | class ActuatorCommand 94 | { 95 | public: 96 | ActuatorCommand() : 97 | enable_(0), 98 | effort_(0) 99 | { 100 | } 101 | 102 | bool enable_; //!< Enable this actuator 103 | double effort_; //!< Force to apply (in Nm) 104 | }; 105 | 106 | /*! 107 | * \class Actuator 108 | * The Actuator class provides an interface for the motor controller 109 | * 110 | * The ActuatorCommand class is used to enable the motor and set the commanded 111 | * efforts of the motor (in Nm). 112 | * 113 | * The ActuatorState class reports back on the state of the motor 114 | */ 115 | class Actuator 116 | { 117 | public: 118 | std::string name_; 119 | ActuatorState state_; 120 | ActuatorCommand command_; 121 | }; 122 | 123 | /*! 124 | * \class CustomHW 125 | * The CustomHW class provides an easy way to add more hardware to the HardwareInterface. 126 | * Inherit from that class to add a new type of hardware, containing the data you want. 127 | * If the hardware doesn't use EtherCAT, constructor and destructor 128 | * should initialize drivers to communicate with hardware and 129 | * read and write functions must be implemented accordingly. 130 | * The read and write functions will be called by RosEthercat functions with same names 131 | */ 132 | class CustomHW 133 | { 134 | public: 135 | virtual ~CustomHW() {} 136 | virtual void read(const ros::Time &time) {} 137 | virtual void write(const ros::Time &time) {} 138 | }; 139 | 140 | } // namespace ros_ethercat_model 141 | 142 | #endif 143 | -------------------------------------------------------------------------------- /ros_ethercat_model/include/ros_ethercat_model/imu_state.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * robot_state.hpp 3 | * 4 | * Created on: 23 October 2017 5 | * Author: Daniel Greenwald 6 | * 7 | * Software License Agreement (BSD License) 8 | * 9 | * Copyright (c) 2017, Shadow Robot Company Ltd. 10 | * All rights reserved. 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Willow Garage nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | *********************************************************************/ 39 | 40 | #ifndef ROS_ETHERCAT_MODEL_IMU_STATE_HPP_ 41 | #define ROS_ETHERCAT_MODEL_IMU_STATE_HPP_ 42 | 43 | #include 44 | #include 45 | 46 | using std::string; 47 | 48 | namespace ros_ethercat_model 49 | { 50 | 51 | class ImuState 52 | { 53 | public: 54 | double orientation_[4]; 55 | double angular_velocity_[3]; 56 | double linear_acceleration_[3]; 57 | double orientation_covariance_[9]; 58 | double angular_velocity_covariance_[9]; 59 | double linear_acceleration_covariance_[9]; 60 | hardware_interface::ImuSensorHandle::Data data_; 61 | 62 | ImuState(string name, string frame_id) 63 | { 64 | data_.name = name; 65 | data_.frame_id = frame_id; 66 | data_.orientation = orientation_; 67 | data_.orientation_covariance = orientation_covariance_; 68 | data_.angular_velocity = angular_velocity_; 69 | data_.angular_velocity_covariance = angular_velocity_covariance_; 70 | data_.linear_acceleration = linear_acceleration_; 71 | data_.linear_acceleration_covariance = linear_acceleration_covariance_; 72 | }; 73 | ImuState(){} 74 | }; 75 | }; // namespace ros_ethercat_model 76 | 77 | #endif // ROS_ETHERCAT_MODEL_IMU_STATE_HPP_ 78 | -------------------------------------------------------------------------------- /ros_ethercat_model/include/ros_ethercat_model/joint.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef ROS_ETHERCAT_MODEL_JOINT_H 36 | #define ROS_ETHERCAT_MODEL_JOINT_H 37 | 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | namespace ros_ethercat_model 48 | { 49 | 50 | enum 51 | { 52 | JOINT_NONE, 53 | JOINT_ROTARY, 54 | JOINT_CONTINUOUS, 55 | JOINT_PRISMATIC, 56 | JOINT_FIXED, 57 | JOINT_PLANAR, 58 | JOINT_TYPES_MAX 59 | }; 60 | 61 | class JointState; 62 | 63 | class JointStatistics 64 | { 65 | public: 66 | JointStatistics() : 67 | min_position_(0), max_position_(0), 68 | max_abs_velocity_(0.0), max_abs_effort_(0.0), 69 | violated_limits_(false), initialized_(false) 70 | { 71 | } 72 | 73 | void update(JointState *jnt); 74 | void reset() 75 | { 76 | double tmp = min_position_; 77 | min_position_ = max_position_; 78 | max_position_ = tmp; 79 | max_abs_velocity_ = 0.0; 80 | max_abs_effort_ = 0.0; 81 | violated_limits_ = false; 82 | } 83 | 84 | double min_position_, max_position_; 85 | double max_abs_velocity_; 86 | double max_abs_effort_; 87 | bool violated_limits_; 88 | 89 | private: 90 | bool initialized_; 91 | double old_position_; 92 | }; 93 | 94 | class JointState 95 | { 96 | public: 97 | /// Modify the commanded_effort_ of the joint state so that the joint limits are satisfied 98 | void enforceLimits() 99 | { 100 | double effort_high, effort_low; 101 | 102 | getLimits(effort_low, effort_high); 103 | 104 | // limit the commanded effort based on position, velocity and effort limits 105 | commanded_effort_ = std::min(std::max(commanded_effort_, effort_low), effort_high); 106 | } 107 | 108 | /// Returns the safety effort limits given the current position and velocity. 109 | void getLimits(double &effort_low, double &effort_high) 110 | { 111 | // only enforce joints that specify joint limits and safety code 112 | if (!joint_->safety || !joint_->limits) 113 | { 114 | effort_low = -std::numeric_limits::max(); 115 | effort_high = std::numeric_limits::max(); 116 | return; 117 | } 118 | 119 | double vel_high = joint_->limits->velocity; 120 | double vel_low = -joint_->limits->velocity; 121 | effort_high = joint_->limits->effort; 122 | effort_low = -joint_->limits->effort; 123 | 124 | // enforce position bounds on rotary and prismatic joints that are calibrated 125 | if (joint_->type == urdf::Joint::REVOLUTE || joint_->type == urdf::Joint::PRISMATIC) 126 | { 127 | // Computes the velocity bounds based on the absolute limit and the 128 | // proximity to the joint limit. 129 | vel_high = std::max(-joint_->limits->velocity, 130 | std::min(joint_->limits->velocity, 131 | -joint_->safety->k_position * (position_ - joint_->safety->soft_upper_limit))); 132 | vel_low = std::min(joint_->limits->velocity, 133 | std::max(-joint_->limits->velocity, 134 | -joint_->safety->k_position * (position_ - joint_->safety->soft_lower_limit))); 135 | } 136 | 137 | // Computes the effort bounds based on the velocity and effort bounds. 138 | effort_high = std::max(-joint_->limits->effort, 139 | std::min(joint_->limits->effort, 140 | -joint_->safety->k_velocity * (velocity_ - vel_high))); 141 | effort_low = std::min(joint_->limits->effort, 142 | std::max(-joint_->limits->effort, 143 | -joint_->safety->k_velocity * (velocity_ - vel_low))); 144 | } 145 | 146 | /// A pointer to the corresponding urdf::Joint from the urdf::Model 147 | urdf::JointConstSharedPtr joint_; 148 | 149 | /// The joint position in radians or meters (read-only variable) 150 | double position_; 151 | 152 | /// The joint velocity in radians/sec or meters/sec (read-only variable) 153 | double velocity_; 154 | 155 | /// The joint effort in Nm or N (read-only variable) 156 | double effort_; 157 | 158 | // joint statistics 159 | JointStatistics joint_statistics_; 160 | 161 | /// The position the joint should move to in radians or meters (write-to variable) 162 | double commanded_position_; 163 | 164 | /// The velocity the joint should move with in radians/sec or meters/sec (write-to variable) 165 | double commanded_velocity_; 166 | 167 | /// The effort the joint should apply in Nm or N (write-to variable) 168 | double commanded_effort_; 169 | 170 | /// The position of the optical flag that was used to calibrate this joint 171 | double reference_position_; 172 | 173 | /// Constructor 174 | JointState() : 175 | position_(0.0), 176 | velocity_(0.0), 177 | effort_(0.0), 178 | commanded_position_(0.0), 179 | commanded_velocity_(0.0), 180 | commanded_effort_(0.0), 181 | reference_position_(0.0) 182 | { 183 | } 184 | }; 185 | inline void JointStatistics::update(JointState *jnt) 186 | { 187 | if (initialized_) 188 | { 189 | if (jnt->joint_->safety && jnt->joint_->limits 190 | && (fabs(jnt->commanded_effort_) > fabs(jnt->effort_))) 191 | violated_limits_ = true; 192 | min_position_ = fmin(jnt->position_, min_position_); 193 | max_position_ = fmax(jnt->position_, max_position_); 194 | max_abs_velocity_ = fmax(fabs(jnt->velocity_), max_abs_velocity_); 195 | max_abs_effort_ = fmax(fabs(jnt->effort_), max_abs_effort_); 196 | } 197 | else 198 | { 199 | min_position_ = jnt->position_; 200 | max_position_ = jnt->position_; 201 | initialized_ = true; 202 | } 203 | old_position_ = jnt->position_; 204 | } 205 | 206 | } // namespace ros_ethercat_model 207 | 208 | #endif /* JOINT_H */ 209 | -------------------------------------------------------------------------------- /ros_ethercat_model/include/ros_ethercat_model/mech_stats_publisher.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * mech_stats_publisher.hpp 3 | * 4 | * Created on: 15 April 2014 5 | * Author: Manos Nikolaidis 6 | * 7 | * Software License Agreement (BSD License) 8 | * 9 | * Copyright (c) 2014, Shadow Robot Company Ltd. 10 | * All rights reserved. 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Willow Garage nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | *********************************************************************/ 39 | 40 | #ifndef ROS_ETHERCAT_MODEL_MECH_STATS_PUBLISHER_HPP_ 41 | #define ROS_ETHERCAT_MODEL_MECH_STATS_PUBLISHER_HPP_ 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | using std::string; 52 | using std::vector; 53 | using boost::unordered_map; 54 | using boost::ptr_vector; 55 | using boost::ptr_unordered_map; 56 | using ros::Duration; 57 | using ros::Time; 58 | using ros::NodeHandle; 59 | using realtime_tools::RealtimePublisher; 60 | using sr_robot_msgs::MechanismStatistics; 61 | using sr_robot_msgs::JointStatistics; 62 | using sr_robot_msgs::ActuatorStatistics; 63 | using ros_ethercat_model::JointState; 64 | using ros_ethercat_model::Actuator; 65 | using ros_ethercat_model::ActuatorState; 66 | using ros_ethercat_model::RobotState; 67 | using ros_ethercat_model::Transmission; 68 | 69 | class MechStatsPublisher 70 | { 71 | public: 72 | MechStatsPublisher(NodeHandle &nh, RobotState &state) : 73 | state_(state), 74 | pub_mech_stats_(nh, "mechanism_statistics", 1), 75 | last_published_mechanism_stats_(Time::now()) 76 | { 77 | pub_mech_stats_.msg_.joint_statistics.resize(state_.joint_states_.size()); 78 | pub_mech_stats_.msg_.actuator_statistics.resize(state_.transmissions_.size()); 79 | 80 | double publish_rate_mechanism_stats; 81 | nh.param("mechanism_statistics_publish_rate", publish_rate_mechanism_stats, 1.0); 82 | publish_period_mechanism_stats_ = Duration(1.0 / fmax(0.000001, publish_rate_mechanism_stats)); 83 | } 84 | void publish(const ros::Time &now) 85 | { 86 | if (now > last_published_mechanism_stats_ + publish_period_mechanism_stats_) 87 | { 88 | if (pub_mech_stats_.trylock()) 89 | { 90 | while (last_published_mechanism_stats_ + publish_period_mechanism_stats_ < now) 91 | last_published_mechanism_stats_ = last_published_mechanism_stats_ + publish_period_mechanism_stats_; 92 | 93 | ptr_unordered_map::iterator jin = state_.joint_states_.begin(); 94 | vector::iterator jout = pub_mech_stats_.msg_.joint_statistics.begin(); 95 | while (jin != state_.joint_states_.end() && 96 | jout != pub_mech_stats_.msg_.joint_statistics.end()) 97 | { 98 | int type = jin->second->joint_->type; 99 | if (type != urdf::Joint::REVOLUTE && type != urdf::Joint::CONTINUOUS && type != urdf::Joint::PRISMATIC) 100 | continue; 101 | jout->timestamp = now; 102 | jout->name = jin->second->joint_->name; 103 | jout->position = jin->second->position_; 104 | jout->velocity = jin->second->velocity_; 105 | jout->measured_effort = jin->second->effort_; 106 | jout->commanded_effort = jin->second->commanded_effort_; 107 | jout->violated_limits = jin->second->joint_statistics_.violated_limits_; 108 | jout->min_position = jin->second->joint_statistics_.min_position_; 109 | jout->max_position = jin->second->joint_statistics_.max_position_; 110 | jout->max_abs_velocity = jin->second->joint_statistics_.max_abs_velocity_; 111 | jout->max_abs_effort = jin->second->joint_statistics_.max_abs_effort_; 112 | jin->second->joint_statistics_.reset(); 113 | ++jin; 114 | ++jout; 115 | } 116 | 117 | ptr_vector::iterator tin = state_.transmissions_.begin(); 118 | vector::iterator aout = pub_mech_stats_.msg_.actuator_statistics.begin(); 119 | while (tin != state_.transmissions_.end() && 120 | aout != pub_mech_stats_.msg_.actuator_statistics.end()) 121 | { 122 | aout->timestamp = now; 123 | aout->name = tin->actuator_->name_; 124 | aout->position = tin->actuator_->state_.position_; 125 | aout->device_id = tin->actuator_->state_.device_id_; 126 | aout->velocity = tin->actuator_->state_.velocity_; 127 | aout->is_enabled = true; 128 | aout->last_commanded_current = tin->actuator_->state_.last_commanded_current_; 129 | aout->last_measured_current = tin->actuator_->state_.last_measured_current_; 130 | aout->last_commanded_effort = tin->actuator_->state_.last_commanded_effort_; 131 | aout->last_measured_effort = tin->actuator_->state_.last_measured_effort_; 132 | aout->motor_voltage = tin->actuator_->state_.motor_voltage_; 133 | ++tin; 134 | ++aout; 135 | } 136 | 137 | pub_mech_stats_.msg_.header.stamp = Time::now(); 138 | pub_mech_stats_.unlockAndPublish(); 139 | } 140 | } 141 | } 142 | 143 | RobotState &state_; 144 | RealtimePublisher pub_mech_stats_; 145 | Duration publish_period_mechanism_stats_; 146 | Time last_published_mechanism_stats_; 147 | }; 148 | #endif 149 | -------------------------------------------------------------------------------- /ros_ethercat_model/include/ros_ethercat_model/robot_state.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * robot_state.hpp 3 | * 4 | * Created on: 7 Jan 2014 5 | * Author: Manos Nikolaidis 6 | * 7 | * Software License Agreement (BSD License) 8 | * 9 | * Copyright (c) 2014, Shadow Robot Company Ltd. 10 | * All rights reserved. 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Willow Garage nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | *********************************************************************/ 39 | 40 | #ifndef ROS_ETHERCAT_MODEL_ROBOTSTATE_HPP 41 | #define ROS_ETHERCAT_MODEL_ROBOTSTATE_HPP 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include "ros_ethercat_model/joint.hpp" 49 | #include "ros_ethercat_model/imu_state.hpp" 50 | 51 | #include "ros_ethercat_model/transmission.hpp" 52 | #include "ros_ethercat_model/hardware_interface.hpp" 53 | #include 54 | #include 55 | #include 56 | 57 | namespace ros_ethercat_model 58 | { 59 | 60 | /* \brief This class provides the controllers with an interface to the robot state 61 | * 62 | * Most controllers that need the robot state should use the joint states, to get 63 | * access to the joint position/velocity/effort, and to command the effort a joint 64 | * should apply. Controllers can get access to the hard realtime clock through current_time_ 65 | */ 66 | 67 | using std::vector; 68 | using std::string; 69 | 70 | class RobotState : public hardware_interface::HardwareInterface 71 | { 72 | public: 73 | explicit RobotState(tinyxml2::XMLElement *root = NULL, vector joint_filter = vector()) 74 | : joint_filter_(joint_filter), transmission_loader_("ros_ethercat_model", "ros_ethercat_model::Transmission") 75 | { 76 | if (root) 77 | initXml(root); 78 | } 79 | 80 | ActuatorState* addActuatorState(string name) 81 | { 82 | actuator_states_[name] = Actuator().state_; 83 | return &actuator_states_[name]; 84 | } 85 | 86 | void initXml(tinyxml2::XMLElement *root) 87 | { 88 | try 89 | { 90 | if (!robot_model_.initXml(root)) 91 | throw std::runtime_error("Failed to load robot_model_"); 92 | for (std::map::const_iterator it = robot_model_.joints_.begin(); 93 | it != robot_model_.joints_.end(); 94 | ++it) 95 | { 96 | if (use_joint_(it->second->name) && (it->second->type == urdf::Joint::PRISMATIC || 97 | it->second->type == urdf::Joint::REVOLUTE)) 98 | { 99 | // URDF sensor implementation is incomplete, so cant get list of named imus. 100 | // find the prefix of the imu from the joint names instead 101 | std::string prefix = it->first.substr(0, it->first.find("_")); 102 | if (!getImuState(prefix + "_imu")) 103 | { 104 | imu_states_[prefix + "_imu"] = ImuState(prefix + "_imu", prefix + "_palm"); 105 | } 106 | joint_states_[it->first].joint_ = it->second; 107 | } 108 | } 109 | 110 | for (tinyxml2::XMLElement *xit = root->FirstChildElement("transmission"); 111 | xit; 112 | xit = xit->NextSiblingElement("transmission")) 113 | { 114 | std::string type, joint_name; 115 | 116 | if (xit->Attribute("type")) 117 | { 118 | type = xit->Attribute("type"); 119 | } // new transmissions have type as an element instead of attribute 120 | else if (xit->FirstChildElement("type")) 121 | { 122 | type = std::string(xit->FirstChildElement("type")->GetText()); 123 | } 124 | 125 | joint_name = string(xit->FirstChildElement("joint")->Attribute("name")); 126 | if (joint_name.empty()) 127 | { 128 | ROS_FATAL_STREAM("Transmission specified without joint element."); 129 | } 130 | 131 | if (!type.empty() && use_joint_(joint_name)) 132 | { 133 | Transmission *t = transmission_loader_.createUnmanagedInstance(type); 134 | if (!t || !t->initXml(xit, this)) 135 | throw std::runtime_error(std::string("Failed to initialize transmission type: ") + type); 136 | 137 | transmissions_.push_back(t); 138 | } 139 | } 140 | } 141 | catch (const std::runtime_error &ex) 142 | { 143 | ROS_FATAL_STREAM("ros_ethercat_model failed to parse the URDF xml into a robot model\n" << ex.what()); 144 | } 145 | } 146 | 147 | /// Propagate the actuator positions, through the transmissions, to the joint positions 148 | void propagateActuatorPositionToJointPosition() 149 | { 150 | for (size_t i = 0; i < transmissions_.size(); ++i) 151 | transmissions_[i].propagatePosition(); 152 | } 153 | 154 | /// Propagate the joint efforts, through the transmissions, to the actuator efforts 155 | void propagateJointEffortToActuatorEffort() 156 | { 157 | for (size_t i = 0; i < transmissions_.size(); ++i) 158 | transmissions_[i].propagateEffort(); 159 | } 160 | 161 | /// get an actuator by actuator name or NULL on failure 162 | Actuator* getActuator(const std::string &name) 163 | { 164 | for (size_t i = 0; i < transmissions_.size(); ++i) 165 | if (transmissions_[i].actuator_->name_ == name) 166 | return transmissions_[i].actuator_; 167 | return NULL; 168 | } 169 | 170 | /// Get Custom Hardware device by name or NULL on failure 171 | CustomHW* getCustomHW(const std::string &name) 172 | { 173 | return custom_hws_.count(name) ? &custom_hws_[name] : NULL; 174 | } 175 | 176 | /// Get a joint state by name or NULL on failure 177 | JointState* getJointState(const std::string &name) 178 | { 179 | return joint_states_.count(name) ? &joint_states_[name] : NULL; 180 | } 181 | 182 | ActuatorState* getActuatorState(const std::string &name) 183 | { 184 | return actuator_states_.count(name) ? &actuator_states_[name] : NULL; 185 | } 186 | 187 | /// Get a joint state by name or NULL on failure 188 | ImuState* getImuState(const std::string &name) 189 | { 190 | return imu_states_.count(name) ? & imu_states_[name] : NULL; 191 | } 192 | 193 | 194 | /// return the current time of the control loop 195 | ros::Time getTime() 196 | { 197 | return current_time_; 198 | } 199 | 200 | /// The time at which the commands were sent to the hardware 201 | ros::Time current_time_; 202 | 203 | /// The joint states mapped to the joint names 204 | boost::ptr_unordered_map joint_states_; 205 | 206 | boost::ptr_unordered_map actuator_states_; 207 | 208 | boost::ptr_unordered_map imu_states_; 209 | 210 | /// Custom hardware structures mapped to their names 211 | boost::ptr_unordered_map custom_hws_; 212 | 213 | /// The kinematic/dynamic model of the robot 214 | urdf::Model robot_model_; 215 | 216 | /// the robot's transmissions 217 | boost::ptr_vector transmissions_; 218 | 219 | /// the transmission's loader 220 | pluginlib::ClassLoader transmission_loader_; 221 | 222 | // map between the hardware name and its product code 223 | std::map device_to_name_map_; 224 | 225 | vector joint_filter_; 226 | bool use_joint_(string joint_name) 227 | { 228 | if (0 == joint_filter_.size()) 229 | { 230 | return true; 231 | } 232 | for (vector::iterator filter_it = joint_filter_.begin(); filter_it != joint_filter_.end(); ++filter_it) 233 | { 234 | const char *filter = (*filter_it).c_str(); 235 | if (!strncmp(joint_name.c_str(), filter, strlen(filter))) // strncmp returns 0 if joint name starts with filter 236 | { 237 | return true; 238 | } 239 | } 240 | return false; 241 | } 242 | }; 243 | } // namespace ros_ethercat_model 244 | #endif 245 | -------------------------------------------------------------------------------- /ros_ethercat_model/include/ros_ethercat_model/robot_state_interface.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * robot_state.hpp 3 | * 4 | * Created on: 19 Feb 2016 5 | * Author: Toni Oliver 6 | * 7 | * Software License Agreement (BSD License) 8 | * 9 | * Copyright (c) 2016, Shadow Robot Company Ltd. 10 | * All rights reserved. 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Willow Garage nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | *********************************************************************/ 39 | 40 | #ifndef ROS_ETHERCAT_MODEL_ROBOT_STATE_INTERFACE_HPP 41 | #define ROS_ETHERCAT_MODEL_ROBOT_STATE_INTERFACE_HPP 42 | 43 | #include 44 | #include "ros_ethercat_model/robot_state.hpp" 45 | #include 46 | 47 | namespace ros_ethercat_model 48 | { 49 | 50 | /* A handle used to read a single RobotState. */ 51 | class RobotStateHandle 52 | { 53 | public: 54 | RobotStateHandle() : name_(), state_(0) {} 55 | 56 | /* 57 | * \param name The name of the joint 58 | * \param state A pointer to the RobotState structure 59 | */ 60 | RobotStateHandle(const std::string& name, RobotState* state) 61 | : name_(name), state_(state) 62 | { 63 | if (!state) 64 | { 65 | throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + name + "'. RobtState data pointer is null."); // NOLINT(whitespace/line_length) 66 | } 67 | } 68 | 69 | std::string getName() const {return name_;} 70 | RobotState* getState() const {assert(state_); return state_;} 71 | 72 | private: 73 | std::string name_; 74 | RobotState* state_; 75 | }; 76 | 77 | /* \brief Hardware interface to support reading an array of RobotStates 78 | * 79 | * This \ref HardwareInterface supports reading an array of named RobotStates 80 | * 81 | */ 82 | class RobotStateInterface : public hardware_interface::HardwareResourceManager {}; 83 | 84 | } // namespace ros_ethercat_model 85 | 86 | #endif 87 | -------------------------------------------------------------------------------- /ros_ethercat_model/include/ros_ethercat_model/ros_ethercat.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * ros_ethercat.hpp 3 | * 4 | * Created on: 7 Jan 2014 5 | * Author: Manos Nikolaidis 6 | * 7 | * Software License Agreement (BSD License) 8 | * 9 | * Copyright (c) 2014, 2024 Shadow Robot Company Ltd. 10 | * All rights reserved. 11 | * 12 | * Redistribution and use in source and binary forms, with or without 13 | * modification, are permitted provided that the following conditions 14 | * are met: 15 | * 16 | * * Redistributions of source code must retain the above copyright 17 | * notice, this list of conditions and the following disclaimer. 18 | * * Redistributions in binary form must reproduce the above 19 | * copyright notice, this list of conditions and the following 20 | * disclaimer in the documentation and/or other materials provided 21 | * with the distribution. 22 | * * Neither the name of the Willow Garage nor the names of its 23 | * contributors may be used to endorse or promote products derived 24 | * from this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | * POSSIBILITY OF SUCH DAMAGE. 38 | *********************************************************************/ 39 | 40 | #ifndef ROS_ETHERCAT_MODEL_ROS_ETHERCAT_HPP_ 41 | #define ROS_ETHERCAT_MODEL_ROS_ETHERCAT_HPP_ 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | #include 56 | #include "ros_ethercat_model/robot_state.hpp" 57 | #include "ros_ethercat_model/robot_state_interface.hpp" 58 | #include "ros_ethercat_model/mech_stats_publisher.hpp" 59 | #include "ros_ethercat_hardware/ethercat_hardware.h" 60 | #include "ros_ethercat_model/imu_state.hpp" 61 | 62 | 63 | /* \brief Contains robot state information and init, read, write function. 64 | * 65 | * The robot state is contained in ros_ethercat_model::RobotStateState object 66 | * as used by pr2_controller object. eNvertheless, the main loop in main.cpp 67 | * instantiates a ros_control controller_manager. So a pr2_controller with few modifications 68 | * may be loaded with controller_manager with RobotState as a custom interface. 69 | * 70 | * ros_control interfaces are exposed alongside RobotState. So controllers from 71 | * ros_controllers package may also be loaded. These new interfaces contain pointers 72 | * to data in RobotState so there is no copying or data redundancy. 73 | * 74 | * The read and write functions will call the propagate functions of pr2_transmissions. 75 | * Hardware read and write takes place in the EthercatHardware object in main.cpp 76 | * 77 | * initXml, read and write should be called inside main.cpp 78 | */ 79 | 80 | #include 81 | #include 82 | 83 | using std::string; 84 | using std::vector; 85 | using boost::ptr_unordered_map; 86 | using boost::ptr_vector; 87 | using ros_ethercat_model::JointState; 88 | using ros_ethercat_model::Actuator; 89 | using ros_ethercat_model::Transmission; 90 | using ros_ethercat_model::CustomHW; 91 | 92 | static const string name = "ros_ethercat"; // NOLINT(runtime/string) 93 | 94 | static const string ethercat_pid_dir = "/var/tmp/run/"; // NOLINT(runtime/string) 95 | 96 | class RosEthercat : public hardware_interface::RobotHW 97 | { 98 | public: 99 | RosEthercat() : 100 | compatibility_mode_(false), 101 | collect_diagnostics_running_(false), 102 | run_diagnostics_(false) 103 | { 104 | } 105 | 106 | RosEthercat(ros::NodeHandle &nh, const string ð, bool allow, tinyxml2::XMLElement* config) : 107 | compatibility_mode_(true), 108 | collect_diagnostics_running_(false), 109 | run_diagnostics_(false) 110 | { 111 | model_.reset(new RobotState(config)); 112 | vector port_names; 113 | boost::split(port_names, eth, boost::is_any_of("_, ")); 114 | for (vector::const_iterator port_name = port_names.begin(); 115 | port_name != port_names.end(); 116 | ++port_name) 117 | { 118 | if (!port_name->empty()) 119 | { 120 | ethercat_hardware_.push_back(new EthercatHardware(name, 121 | static_cast (model_.get()), // NOLINT(whitespace/line_length) 122 | *port_name, 123 | allow)); 124 | ROS_INFO_STREAM("Added Ethernet port " << *port_name); 125 | } 126 | } 127 | 128 | for (ptr_unordered_map::iterator it = model_->imu_states_.begin(); 129 | it != model_->imu_states_.end(); ++it) 130 | { 131 | ROS_INFO_STREAM("IMU State Interface for IMU " << it->first); 132 | hardware_interface::ImuSensorHandle imu_sh(it->second->data_); 133 | imu_sensor_interface_.registerHandle(imu_sh); 134 | } 135 | 136 | for (ptr_unordered_map::iterator it = model_->joint_states_.begin(); 137 | it != model_->joint_states_.end(); 138 | ++it) 139 | { 140 | hardware_interface::JointStateHandle jsh(it->first, 141 | &it->second->position_, 142 | &it->second->velocity_, 143 | &it->second->effort_); 144 | joint_state_interface_.registerHandle(jsh); 145 | 146 | joint_position_command_interface_.registerHandle(hardware_interface::JointHandle(jsh, 147 | & it->second->commanded_position_)); // NOLINT(whitespace/line_length) 148 | joint_velocity_command_interface_.registerHandle(hardware_interface::JointHandle(jsh, 149 | & it->second->commanded_velocity_)); // NOLINT(whitespace/line_length) 150 | joint_effort_command_interface_.registerHandle(hardware_interface::JointHandle(jsh, 151 | & it->second->commanded_effort_)); 152 | } 153 | 154 | if (!model_->joint_states_.empty()) 155 | mech_stats_publisher_.reset(new MechStatsPublisher(nh, *model_)); 156 | 157 | robot_state_interface_.registerHandle(ros_ethercat_model::RobotStateHandle("unique_robot_hw", model_.get())); 158 | 159 | registerInterface(&robot_state_interface_); 160 | registerInterface(&joint_state_interface_); 161 | registerInterface(&joint_position_command_interface_); 162 | registerInterface(&joint_velocity_command_interface_); 163 | registerInterface(&joint_effort_command_interface_); 164 | registerInterface(&imu_sensor_interface_); 165 | 166 | // Start a thread to collect diagnostics. This could actually be inside the EthercatHardware class 167 | // but until we remove the compatibility mode this will do. 168 | collect_diagnostics_thread_ = boost::thread(&RosEthercat::collect_diagnostics_loop, this); 169 | 170 | // If we are running more than one ethercat hardware, spin up multiple threads 171 | if (ethercat_hardware_.size() > 1) 172 | { 173 | hardware_update_thread_.reserve(ethercat_hardware_.size()); 174 | 175 | for (ptr_vector::iterator eh = ethercat_hardware_.begin(); 176 | eh != ethercat_hardware_.end(); 177 | ++eh) 178 | { 179 | EthercatHardware* current_eth = &(*eh); 180 | current_eth->can_run_eth_hw_read_.store(false); 181 | current_eth->eth_hw_read_done_.store(false); 182 | 183 | auto functor = boost::bind(&RosEthercat::ethercat_update_thread, this, current_eth); 184 | hardware_update_thread_.push_back(new boost::thread(functor)); 185 | updateThreadPriority(*hardware_update_thread_.back()); 186 | } 187 | } 188 | } 189 | 190 | virtual ~RosEthercat() 191 | { 192 | if (!compatibility_mode_) 193 | { 194 | // Shutdown all of the motors on exit 195 | shutdown(); 196 | // Cleanup pid files 197 | cleanupPidFile(NULL); 198 | cleanupPidFile(eth_.c_str()); 199 | 200 | stop_collect_diagnostics(); 201 | while (is_collect_diagnostics_running()) 202 | { 203 | usleep(100); 204 | } 205 | } 206 | } 207 | 208 | /// Update priority of provided thread 209 | bool updateThreadPriority(boost::thread& a_thread) 210 | { 211 | int policy; 212 | struct sched_param param; 213 | 214 | pthread_t threadID = (pthread_t) a_thread.native_handle(); 215 | 216 | policy = SCHED_FIFO; 217 | param.sched_priority = sched_get_priority_max(policy); 218 | 219 | if (pthread_setschedparam(threadID, policy, ¶m) != 0) 220 | { 221 | ROS_ERROR("Error setting policy/priority of Ethercat hardware threads. Please restart the system"); 222 | return false; 223 | } 224 | return true; 225 | } 226 | 227 | /// Thread that calls EthercatHardware.update 228 | void ethercat_update_thread(EthercatHardware * eh) 229 | { 230 | while (true) 231 | { 232 | { 233 | boost::unique_lock lock(eh->update_mutex); 234 | 235 | while (!eh->can_run_eth_hw_read_.load()) 236 | { 237 | eh->start_of_work_condition_eth_hw_read.wait(lock); 238 | } 239 | } 240 | 241 | eh->update(false, false); 242 | 243 | { 244 | boost::lock_guard lock(eh->update_mutex); 245 | 246 | eh->can_run_eth_hw_read_.store(false); 247 | eh->eth_hw_read_done_.store(true); 248 | } 249 | eh->end_of_work_condition_eth_hw_read.notify_one(); 250 | } 251 | return; 252 | } 253 | 254 | 255 | virtual bool init(ros::NodeHandle& root_nh, ros::NodeHandle &robot_hw_nh) 256 | { 257 | // Load robot description 258 | tinyxml2::XMLDocument xml; 259 | tinyxml2::XMLElement *root; 260 | tinyxml2::XMLElement *root_element; 261 | 262 | std::string robot_description; 263 | std::string robot_description_param; 264 | bool allow; 265 | 266 | robot_state_name_ = robot_hw_nh.getNamespace().substr(1); // remove the leading slash of the namespace 267 | ROS_INFO_STREAM("Robot State Name: " << robot_state_name_); 268 | 269 | if (!robot_hw_nh.getParam("robot_description_param", robot_description_param)) 270 | { 271 | ROS_ERROR("robot_description_param not found (namespace: %s)", robot_hw_nh.getNamespace().c_str()); 272 | return false; 273 | } 274 | 275 | if (robot_description_param == "None") 276 | { 277 | root = NULL; 278 | } 279 | else 280 | { 281 | if (!root_nh.getParam(robot_description_param, robot_description)) 282 | { 283 | ROS_ERROR("Robot description: %s not found (namespace: %s)", robot_description_param.c_str(), root_nh.getNamespace().c_str()); // NOLINT(whitespace/line_length) 284 | return false; 285 | } 286 | xml.Parse(robot_description.c_str()); 287 | root_element = xml.RootElement(); 288 | root = xml.FirstChildElement("robot"); 289 | if (!root || !root_element) 290 | { 291 | ROS_ERROR("Robot description %s has no root", robot_description_param.c_str()); 292 | return false; 293 | } 294 | } 295 | 296 | vector joint_filter; 297 | !robot_hw_nh.getParam("joint_filter", joint_filter); 298 | 299 | model_.reset(new RobotState(root, joint_filter)); 300 | 301 | if (!robot_hw_nh.getParam("ethercat_port", eth_)) 302 | { 303 | ROS_ERROR("ethercat_port param not found (namespace: %s)", robot_hw_nh.getNamespace().c_str()); 304 | return false; 305 | } 306 | 307 | // EtherCAT lock for this interface (e.g. Ethernet port) 308 | if (setupPidFile(eth_.c_str()) < 0) 309 | { 310 | return false; 311 | } 312 | 313 | robot_hw_nh.param("allow_unprogrammed", allow, false); 314 | 315 | vector port_names; 316 | boost::split(port_names, eth_, boost::is_any_of("_, ")); 317 | for (vector::const_iterator port_name = port_names.begin(); 318 | port_name != port_names.end(); 319 | ++port_name) 320 | { 321 | if (!port_name->empty()) 322 | { 323 | ethercat_hardware_.push_back(new EthercatHardware(name, 324 | static_cast (model_.get()), // NOLINT(whitespace/line_length) 325 | *port_name, 326 | allow)); 327 | ROS_INFO_STREAM("Added Ethernet port " << *port_name); 328 | } 329 | } 330 | 331 | for (ptr_unordered_map::iterator it = model_->imu_states_.begin(); 332 | it != model_->imu_states_.end(); ++it) 333 | { 334 | ROS_INFO_STREAM("IMU State Interface for IMU " << it->first); 335 | hardware_interface::ImuSensorHandle imu_sh(it->second->data_); 336 | imu_sensor_interface_.registerHandle(imu_sh); 337 | } 338 | 339 | for (ptr_unordered_map::iterator it = model_->joint_states_.begin(); 340 | it != model_->joint_states_.end(); 341 | ++it) 342 | { 343 | // joints have already had filter applied in initialisation of robot model 344 | ROS_INFO_STREAM("Joint state interface for hand joint " << it->first); 345 | hardware_interface::JointStateHandle jsh(it->first, 346 | &it->second->position_, 347 | &it->second->velocity_, 348 | &it->second->effort_); 349 | joint_state_interface_.registerHandle(jsh); 350 | 351 | joint_position_command_interface_.registerHandle(hardware_interface::JointHandle(jsh, 352 | &it->second->commanded_position_)); 353 | joint_velocity_command_interface_.registerHandle(hardware_interface::JointHandle(jsh, 354 | &it->second->commanded_velocity_)); 355 | joint_effort_command_interface_.registerHandle(hardware_interface::JointHandle(jsh, 356 | &it->second->commanded_effort_)); 357 | } 358 | 359 | if (!model_->joint_states_.empty()) 360 | mech_stats_publisher_.reset(new MechStatsPublisher(root_nh, *model_)); 361 | 362 | robot_state_interface_.registerHandle(ros_ethercat_model::RobotStateHandle(robot_state_name_, model_.get())); 363 | 364 | registerInterface(&robot_state_interface_); 365 | registerInterface(&joint_state_interface_); 366 | registerInterface(&joint_position_command_interface_); 367 | registerInterface(&joint_velocity_command_interface_); 368 | registerInterface(&joint_effort_command_interface_); 369 | registerInterface(&imu_sensor_interface_); 370 | 371 | // Start a thread to collect diagnostics. This could actually be inside the EthercatHardware class 372 | // but until we remove the compatibility mode this will do. 373 | collect_diagnostics_thread_ = boost::thread(&RosEthercat::collect_diagnostics_loop, this); 374 | 375 | // If we are running more than one ethercat hardware, spin up multiple threads 376 | if (ethercat_hardware_.size() > 1) 377 | { 378 | hardware_update_thread_.reserve(ethercat_hardware_.size()); 379 | 380 | for (ptr_vector::iterator eh = ethercat_hardware_.begin(); 381 | eh != ethercat_hardware_.end(); 382 | ++eh) 383 | { 384 | EthercatHardware* current_eth = &(*eh); 385 | current_eth->can_run_eth_hw_read_.store(false); 386 | current_eth->eth_hw_read_done_.store(false); 387 | 388 | auto functor = boost::bind(&RosEthercat::ethercat_update_thread, this, current_eth); 389 | hardware_update_thread_.push_back(new boost::thread(functor)); 390 | if (!updateThreadPriority(*hardware_update_thread_.back())) 391 | { 392 | return false; 393 | } 394 | } 395 | } 396 | 397 | return true; 398 | } 399 | 400 | /// propagate position actuator -> joint and set commands to zero 401 | void read(const ros::Time &time, const ros::Duration& period) 402 | { 403 | if (ethercat_hardware_.size() == 1) 404 | { 405 | ethercat_hardware_[0].update(false, false); 406 | } 407 | // If we are running multiple Ethercat devices, parallelise calls to EthercatHardware.update 408 | else 409 | { 410 | for (ptr_vector::iterator eh = ethercat_hardware_.begin(); 411 | eh != ethercat_hardware_.end(); 412 | ++eh) 413 | { 414 | { 415 | boost::lock_guard lock(eh->update_mutex); 416 | eh->can_run_eth_hw_read_.store(true); 417 | eh->eth_hw_read_done_.store(false); 418 | } 419 | eh->start_of_work_condition_eth_hw_read.notify_one(); 420 | } 421 | 422 | for (ptr_vector::iterator eh = ethercat_hardware_.begin(); 423 | eh != ethercat_hardware_.end(); 424 | ++eh) 425 | { 426 | { 427 | boost::unique_lock lock(eh->update_mutex); 428 | while (!eh->eth_hw_read_done_.load()) 429 | { 430 | eh->end_of_work_condition_eth_hw_read.wait(lock); 431 | } 432 | } 433 | } 434 | } 435 | 436 | model_->current_time_ = time; 437 | model_->propagateActuatorPositionToJointPosition(); 438 | 439 | for (ptr_unordered_map::iterator it = model_->custom_hws_.begin(); 440 | it != model_->custom_hws_.end(); 441 | ++it) 442 | { 443 | it->second->read(time); 444 | } 445 | 446 | for (ptr_unordered_map::iterator it = model_->joint_states_.begin(); 447 | it != model_->joint_states_.end(); 448 | ++it) 449 | { 450 | it->second->joint_statistics_.update(it->second); 451 | it->second->commanded_effort_ = 0; 452 | } 453 | } 454 | 455 | /// propagate effort joint -> actuator and enforce safety limits 456 | void write(const ros::Time &time, const ros::Duration& period) 457 | { 458 | /// Modify the commanded_effort_ of each joint state so that the joint limits are satisfied 459 | for (ptr_unordered_map::iterator it = model_->joint_states_.begin(); 460 | it != model_->joint_states_.end(); 461 | ++it) 462 | { 463 | it->second->enforceLimits(); 464 | } 465 | 466 | model_->propagateJointEffortToActuatorEffort(); 467 | 468 | for (ptr_unordered_map::iterator it = model_->custom_hws_.begin(); 469 | it != model_->custom_hws_.end(); 470 | ++it) 471 | { 472 | it->second->write(time); 473 | } 474 | 475 | if (!model_->joint_states_.empty()) 476 | mech_stats_publisher_->publish(time); 477 | } 478 | 479 | /// stop all actuators 480 | void shutdown() 481 | { 482 | for (ptr_vector::iterator it = model_->transmissions_.begin(); 483 | it != model_->transmissions_.end(); 484 | ++it) 485 | { 486 | it->actuator_->command_.enable_ = false; 487 | it->actuator_->command_.effort_ = 0; 488 | } 489 | 490 | for (ptr_vector::iterator eh = ethercat_hardware_.begin(); 491 | eh != ethercat_hardware_.end(); 492 | ++eh) 493 | { 494 | eh->update(false, true); 495 | } 496 | 497 | if (ethercat_hardware_.size() > 1) 498 | { 499 | for (uint8_t hardware_index = 0; hardware_index < ethercat_hardware_.size(); 500 | ++hardware_index) 501 | { 502 | delete hardware_update_thread_[hardware_index]; 503 | } 504 | } 505 | } 506 | 507 | string eth_; 508 | boost::shared_ptr model_; 509 | ptr_vector ethercat_hardware_; 510 | std::unique_ptr mech_stats_publisher_; 511 | 512 | // robot state interface 513 | ros_ethercat_model::RobotStateInterface robot_state_interface_; 514 | 515 | // joint state interface 516 | hardware_interface::JointStateInterface joint_state_interface_; 517 | 518 | // imu sensor interface 519 | hardware_interface::ImuSensorInterface imu_sensor_interface_; 520 | 521 | // joint command interface 522 | hardware_interface::PositionJointInterface joint_position_command_interface_; 523 | hardware_interface::VelocityJointInterface joint_velocity_command_interface_; 524 | hardware_interface::EffortJointInterface joint_effort_command_interface_; 525 | 526 | protected: 527 | static int lock_fd(int fd) 528 | { 529 | struct flock lock; 530 | 531 | lock.l_type = F_WRLCK; 532 | lock.l_whence = SEEK_SET; 533 | lock.l_start = 0; 534 | lock.l_len = 0; 535 | 536 | return fcntl(fd, F_SETLK, &lock); 537 | } 538 | 539 | static string generatePIDFilename(const char* interface) 540 | { 541 | string filename; 542 | filename = ethercat_pid_dir + "EtherCAT_" + string(interface) + ".pid"; 543 | return filename; 544 | } 545 | 546 | static int setupPidFile(const char* interface) 547 | { 548 | pid_t pid; 549 | int fd; 550 | FILE *fp = NULL; 551 | 552 | string filename = generatePIDFilename(interface); 553 | 554 | umask(0); 555 | mkdir(ethercat_pid_dir.c_str(), 0777); 556 | int PID_FLAGS = O_RDWR | O_CREAT | O_EXCL; 557 | int PID_MODE = S_IWUSR | S_IRUSR | S_IWGRP | S_IRGRP | S_IWOTH | S_IROTH; 558 | fd = open(filename.c_str(), PID_FLAGS, PID_MODE); 559 | if (fd == -1) 560 | { 561 | if (errno != EEXIST) 562 | { 563 | ROS_FATAL("Unable to create pid file '%s': %s", filename.c_str(), strerror(errno)); 564 | return -1; 565 | } 566 | 567 | if ((fd = open(filename.c_str(), O_RDWR)) < 0) 568 | { 569 | ROS_FATAL("Unable to open pid file '%s': %s", filename.c_str(), strerror(errno)); 570 | return -1; 571 | } 572 | 573 | if ((fp = fdopen(fd, "rw")) == NULL) 574 | { 575 | ROS_FATAL("Can't read from '%s': %s", filename.c_str(), strerror(errno)); 576 | return -1; 577 | } 578 | pid = -1; 579 | if ((fscanf(fp, "%d", &pid) != 1) || (pid == getpid()) || (lock_fd(fileno(fp)) == 0)) 580 | { 581 | int rc; 582 | 583 | if ((rc = unlink(filename.c_str())) == -1) 584 | { 585 | ROS_FATAL("Can't remove stale pid file '%s': %s", filename.c_str(), strerror(errno)); 586 | return -1; 587 | } 588 | } 589 | else 590 | { 591 | ROS_FATAL("Another instance of ros_ethercat is already running with pid: %d", pid); 592 | return -1; 593 | } 594 | } 595 | 596 | unlink(filename.c_str()); 597 | fd = open(filename.c_str(), PID_FLAGS, PID_MODE); 598 | 599 | if (fd == -1) 600 | { 601 | ROS_FATAL("Unable to open pid file '%s': %s", filename.c_str(), strerror(errno)); 602 | return -1; 603 | } 604 | 605 | if (lock_fd(fd) == -1) 606 | { 607 | ROS_FATAL("Unable to lock pid file '%s': %s", filename.c_str(), strerror(errno)); 608 | return -1; 609 | } 610 | 611 | if ((fp = fdopen(fd, "w")) == NULL) 612 | { 613 | ROS_FATAL("fdopen failed: %s", strerror(errno)); 614 | return -1; 615 | } 616 | 617 | fprintf(fp, "%d\n", getpid()); 618 | 619 | /* We do NOT close fd, since we want to keep the lock. */ 620 | fflush(fp); 621 | fcntl(fd, F_SETFD, (long) 1); // NOLINT(runtime/int) 622 | 623 | return 0; 624 | } 625 | 626 | static void cleanupPidFile(const char* interface) 627 | { 628 | string filename = generatePIDFilename(interface); 629 | unlink(filename.c_str()); 630 | } 631 | 632 | void collect_diagnostics_loop() 633 | { 634 | run_diagnostics_ = true; 635 | collect_diagnostics_running_ = true; 636 | ros::Rate diag_rate(1.0); // Send diagnostics at 1Hz 637 | while (run_diagnostics_) 638 | { 639 | for (ptr_vector::iterator eh = ethercat_hardware_.begin(); eh != ethercat_hardware_.end(); ++eh) 640 | { 641 | eh->collectDiagnostics(); 642 | } 643 | diag_rate.sleep(); 644 | } 645 | collect_diagnostics_running_ = false; 646 | } 647 | 648 | protected: 649 | void stop_collect_diagnostics() 650 | { 651 | run_diagnostics_ = false; 652 | } 653 | 654 | bool is_collect_diagnostics_running() 655 | { 656 | return collect_diagnostics_running_; 657 | } 658 | 659 | bool compatibility_mode_; 660 | bool run_diagnostics_; 661 | bool collect_diagnostics_running_; 662 | boost::thread collect_diagnostics_thread_; 663 | std::string robot_state_name_; 664 | std::vector hardware_update_thread_; 665 | }; 666 | 667 | #endif 668 | -------------------------------------------------------------------------------- /ros_ethercat_model/include/ros_ethercat_model/transmission.hpp: -------------------------------------------------------------------------------- 1 | /******************************************************************** 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008, Willow Garage, Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the Willow Garage nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | /* 35 | * Author: Stuart Glaser 36 | */ 37 | #ifndef ROS_ETHERCAT_MODEL_TRANSMISSION_H 38 | #define ROS_ETHERCAT_MODEL_TRANSMISSION_H 39 | 40 | #include 41 | #include 42 | #include "ros_ethercat_model/joint.hpp" 43 | #include "ros_ethercat_model/hardware_interface.hpp" 44 | #include 45 | 46 | namespace ros_ethercat_model 47 | { 48 | 49 | class RobotState; 50 | 51 | class Transmission 52 | { 53 | public: 54 | /// Destructor 55 | virtual ~Transmission() 56 | { 57 | delete actuator_; 58 | } 59 | 60 | /// Initializes the transmission from XML data 61 | virtual bool initXml(tinyxml2::XMLElement *config, RobotState *robot) 62 | { 63 | const char *name = config->Attribute("name"); 64 | name_ = name ? name : ""; 65 | return true; 66 | } 67 | 68 | /// Uses encoder data to fill out joint position and velocities 69 | virtual void propagatePosition() 70 | { 71 | }; 72 | 73 | /// Uses commanded joint efforts to fill out commanded motor currents 74 | virtual void propagateEffort() 75 | { 76 | }; 77 | 78 | /// the name of the transmission 79 | std::string name_; 80 | 81 | /// The state of the main joint that this transmission handles. 82 | /// Child classes may add more joints if they need them. 83 | /// JointState object owned by RobotState object. 84 | JointState *joint_; 85 | 86 | /// The actuator that this transmission handles. 87 | /// Child classes may add more actuators if they need them. 88 | Actuator *actuator_; 89 | }; 90 | 91 | } // namespace ros_ethercat_model 92 | 93 | #endif 94 | -------------------------------------------------------------------------------- /ros_ethercat_model/include/ros_ethercat_model/tree.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2011, PAL Robotics S.L. 3 | * All rights reserved. 4 | * 5 | * Copyright (c) 2008, Willow Garage, Inc. 6 | * All rights reserved. 7 | * 8 | * Author: Marcus Liebhardt 9 | * 10 | * This class has been derived from the chain class in the 11 | * ros_ethercat_model package in the pr2_mechanism stack for ROS 12 | * written by Stuart Glaser and Wim Meeussen. 13 | * 14 | * Redistribution and use in source and binary forms, with or without 15 | * modification, are permitted provided that the following conditions are met: 16 | * 17 | * * Redistributions of source code must retain the above copyright 18 | * notice, this list of conditions and the following disclaimer. 19 | * * Redistributions in binary form must reproduce the above copyright 20 | * notice, this list of conditions and the following disclaimer in the 21 | * documentation and/or other materials provided with the distribution. 22 | * * Neither the name of the Willow Garage, Inc. nor the names of its 23 | * contributors may be used to endorse or promote products derived from 24 | * this software without specific prior written permission. 25 | * 26 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 27 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 28 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 29 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 30 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 31 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 32 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 33 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 34 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 35 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 36 | * POSSIBILITY OF SUCH DAMAGE. 37 | */ 38 | 39 | #ifndef ROS_ETHERCAT_MODEL_TREE_H 40 | #define ROS_ETHERCAT_MODEL_TREE_H 41 | 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include "ros_ethercat_model/robot_state.hpp" 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | namespace ros_ethercat_model 54 | { 55 | 56 | class Tree 57 | { 58 | public: 59 | Tree() : 60 | kdl_tree_() 61 | { 62 | } 63 | /* 64 | * \brief initializes the tree object 65 | * The initializer's most important functionality is to create a vector of joints. 66 | * This vector is ordered according to the joint's number given by KDL's tree class, which is used by the 67 | * kdl_parser to create a KDL::Tree from the robot description. This structure is what a KDL tree solver expects. 68 | * The vector of joints can later be used to read the joints' positions or to send efforts to them. 69 | * 70 | * \param robot_state the robot state object containing the robot model and the state of each joint in the robot 71 | */ 72 | bool init(RobotState *robot_state) 73 | { 74 | KDL::SegmentMap segmentMap; 75 | 76 | // construct the kdl tree 77 | if (!kdl_parser::treeFromUrdfModel(robot_state->robot_model_, kdl_tree_)) 78 | { 79 | ROS_ERROR("Failed to construct KDL:Tree from robot_state's URDF model! Aborting ..."); 80 | } 81 | else 82 | { 83 | ROS_INFO("KDL::Tree successful created."); 84 | 85 | segmentMap = kdl_tree_.getSegments(); 86 | } 87 | 88 | // the first step of extracting the joints from the tree is to go through all tree_elements, check for a joint, 89 | // and check in case a joint is found, if it is not of not of type KDL::Joint::None 90 | 91 | // map for saving the temporary result of the joint extraction from the tree 92 | std::map jointMap; 93 | 94 | ROS_DEBUG("Extracting all joints from the tree, which are not of type KDL::Joint::None."); 95 | for (KDL::SegmentMap::const_iterator seg_it = segmentMap.begin(); seg_it != segmentMap.end(); 96 | ++seg_it) 97 | { 98 | if (seg_it->second.segment.getJoint().getType() != KDL::Joint::None) 99 | jointMap[seg_it->second.q_nr] = seg_it->second.segment.getJoint().getName().c_str(); 100 | } 101 | 102 | // in the second step the joints found get checked, if they appear in the JointState vector of the robot 103 | ROS_DEBUG("Checking, if extracted joints can be found in the JointState vector of the robot."); 104 | joints_.clear(); 105 | for (std::map::const_iterator jnt_it = jointMap.begin(); 106 | jnt_it != jointMap.end(); ++jnt_it) 107 | { 108 | JointState* jnt = robot_state->getJointState(jnt_it->second.c_str()); 109 | if (!jnt) 110 | { 111 | ROS_ERROR("Joint '%s' has not been found in the robot's joint state vector! Aborting ...", 112 | jnt_it->second.c_str()); 113 | return false; 114 | } 115 | joints_.push_back(jnt); 116 | } 117 | 118 | ROS_DEBUG("The result after joint extraction and checking:"); 119 | for (unsigned int i = 0; i < joints_.size(); ++i) 120 | { 121 | ROS_DEBUG("joints_[%d]: joint_.name = %s", i, joints_[i]->joint_->name.c_str()); 122 | } 123 | 124 | ROS_INFO("Added %i joints", static_cast(joints_.size())); 125 | 126 | return true; 127 | } 128 | 129 | /// get the position of the joints of the tree as a KDL::JntArray 130 | void getPositions(KDL::JntArray& positions) const 131 | { 132 | assert(positions.rows() == joints_.size()); 133 | for (unsigned int i = 0; i < joints_.size(); ++i) 134 | positions(i) = joints_[i]->position_; 135 | } 136 | 137 | /// get the position of the joints of the tree as any type with size() and [] lookup 138 | template 139 | void getPositions(Vec &v) const 140 | { 141 | assert((static_cast) v.size() == (static_cast) joints_.size()); 142 | for (size_t i = 0; i < joints_.size(); ++i) 143 | v[i] = joints_[i]->position_; 144 | } 145 | 146 | /// get the velocities of the joints of the tree as a KDL::JntArrayVel (fills in the positions, too) 147 | void getVelocities(KDL::JntArrayVel &velocities) const 148 | { 149 | assert(velocities.q.rows() == joints_.size()); 150 | assert(velocities.qdot.rows() == joints_.size()); 151 | for (unsigned int i = 0; i < joints_.size(); ++i) 152 | { 153 | velocities.q(i) = joints_[i]->position_; 154 | velocities.qdot(i) = joints_[i]->velocity_; 155 | } 156 | } 157 | 158 | /// get the velocities of the joints of the tree as any type with size() and [] lookup 159 | template 160 | void getVelocities(Vec &v) const 161 | { 162 | assert((static_cast) v.size() == (static_cast) joints_.size()); 163 | for (size_t i = 0; i < joints_.size(); ++i) 164 | v[i] = joints_[i]->velocity_; 165 | } 166 | 167 | /// get the measured joint efforts of the tree's joints as a KDL::JntArray 168 | void getEfforts(KDL::JntArray &efforts) const 169 | { 170 | assert(efforts.rows() == joints_.size()); 171 | for (unsigned int i = 0; i < joints_.size(); ++i) 172 | efforts(i) = joints_[i]->effort_; 173 | } 174 | 175 | /// get the measured joint efforts of the tree's joints as any type with size() and [] lookup 176 | template 177 | void getEfforts(Vec &v) const 178 | { 179 | assert((static_cast) v.size() == (static_cast) joints_.size()); 180 | for (size_t i = 0; i < joints_.size(); ++i) 181 | v[i] = joints_[i]->effort_; 182 | } 183 | 184 | /// set the commanded joint efforts of the tree's joints from a KDL::JntArray 185 | void setEfforts(const KDL::JntArray &efforts) 186 | { 187 | assert(efforts.rows() == joints_.size()); 188 | for (unsigned int i = 0; i < joints_.size(); ++i) 189 | joints_[i]->commanded_effort_ = efforts(i); 190 | } 191 | 192 | /// set the commanded joint efforts of the tree's joints from any type that implements size() and [] lookup 193 | template 194 | void setEfforts(const Vec &v) 195 | { 196 | assert((static_cast) v.size() == (static_cast) joints_.size()); 197 | for (size_t i = 0; i < joints_.size(); ++i) 198 | joints_[i]->commanded_effort_ = v[i]; 199 | } 200 | 201 | /// add to the commanded joint efforts of the tree's joints from a KDL::JntArray 202 | void addEfforts(const KDL::JntArray &efforts) 203 | { 204 | assert(efforts.rows() == joints_.size()); 205 | for (unsigned int i = 0; i < joints_.size(); ++i) 206 | joints_[i]->commanded_effort_ += efforts(i); 207 | } 208 | 209 | /// add to the commanded joint efforts of the tree's joints from any type that implements size() and [] lookup 210 | template 211 | void addEfforts(const Vec &v) 212 | { 213 | assert((static_cast) v.size() == (static_cast) joints_.size()); 214 | for (size_t i = 0; i < joints_.size(); ++i) 215 | joints_[i]->commanded_effort_ += v[i]; 216 | } 217 | 218 | /// get a KDL::Tree object that represents the tree 219 | void toKdl(KDL::Tree &tree) const 220 | { 221 | tree = kdl_tree_; 222 | } 223 | 224 | /// returns a pointer to the joint state of a joint in the list of the tree's actuated joints (index starts at 0) 225 | JointState* getJoint(unsigned int actuated_joint_i) const 226 | { 227 | if (actuated_joint_i >= joints_.size()) 228 | return NULL; 229 | else 230 | return joints_[actuated_joint_i]; 231 | } 232 | 233 | /// returns the number of actuated joints in the tree 234 | int size() const 235 | { 236 | return joints_.size(); 237 | } 238 | 239 | private: 240 | KDL::Tree kdl_tree_; 241 | /// a vector of pointers to joint states; includes only the ones that can be actuated (not fixed joints) 242 | std::vector joints_; 243 | }; 244 | 245 | } // namespace ros_ethercat_model 246 | 247 | #endif /* ros_ethercat_model_TREE_H */ 248 | -------------------------------------------------------------------------------- /ros_ethercat_model/launch/joint_state_controller.yaml: -------------------------------------------------------------------------------- 1 | # Software License Agreement (BSD License) 2 | # Copyright © 2022 belongs to Shadow Robot Company Ltd. 3 | # All rights reserved. 4 | # 5 | # Redistribution and use in source and binary forms, with or without modification, 6 | # are permitted provided that the following conditions are met: 7 | # 1. Redistributions of source code must retain the above copyright notice, 8 | # this list of conditions and the following disclaimer. 9 | # 2. Redistributions in binary form must reproduce the above copyright notice, 10 | # this list of conditions and the following disclaimer in the documentation 11 | # and/or other materials provided with the distribution. 12 | # 3. Neither the name of Shadow Robot Company Ltd nor the names of its contributors 13 | # may be used to endorse or promote products derived from this software without 14 | # specific prior written permission. 15 | # 16 | # This software is provided by Shadow Robot Company Ltd "as is" and any express 17 | # or implied warranties, including, but not limited to, the implied warranties of 18 | # merchantability and fitness for a particular purpose are disclaimed. In no event 19 | # shall the copyright holder be liable for any direct, indirect, incidental, special, 20 | # exemplary, or consequential damages (including, but not limited to, procurement of 21 | # substitute goods or services; loss of use, data, or profits; or business interruption) 22 | # however caused and on any theory of liability, whether in contract, strict liability, 23 | # or tort (including negligence or otherwise) arising in any way out of the use of this 24 | # software, even if advised of the possibility of such damage. 25 | 26 | joint_state_controller: 27 | type: joint_state_controller/JointStateController 28 | -------------------------------------------------------------------------------- /ros_ethercat_model/launch/joint_state_publisher.launch: -------------------------------------------------------------------------------- 1 | 2 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /ros_ethercat_model/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 26 | 27 | 28 | ros_ethercat_model 29 | 0.3.1 30 | The mechanism model 31 | Shadow Robot's software team 32 | 33 | BSD 34 | 35 | http://www.shadowrobot.com/ 36 | 37 | Manos Nikolaidis 38 | 39 | catkin 40 | 41 | roscpp 42 | hardware_interface 43 | pluginlib 44 | urdf 45 | kdl_parser 46 | sr_robot_msgs 47 | cmake_modules 48 | realtime_tools 49 | ros_ethercat_hardware 50 | tinyxml2 51 | 52 | controller_manager 53 | roscpp 54 | hardware_interface 55 | pluginlib 56 | urdf 57 | kdl_parser 58 | sr_robot_msgs 59 | joint_state_controller 60 | realtime_tools 61 | ros_ethercat_hardware 62 | tinyxml2 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /ros_ethercat_model/src/ros_ethercat.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * Copyright © 2015, 2022 belongs to Shadow Robot Company Ltd. 4 | * All rights reserved. 5 | * 6 | * Redistribution and use in source and binary forms, with or without modification, 7 | * are permitted provided that the following conditions are met: 8 | * 1. Redistributions of source code must retain the above copyright notice, 9 | * this list of conditions and the following disclaimer. 10 | * 2. Redistributions in binary form must reproduce the above copyright notice, 11 | * this list of conditions and the following disclaimer in the documentation 12 | * and/or other materials provided with the distribution. 13 | * 3. Neither the name of Shadow Robot Company Ltd nor the names of its contributors 14 | * may be used to endorse or promote products derived from this software without 15 | * specific prior written permission. 16 | * 17 | * This software is provided by Shadow Robot Company Ltd "as is" and any express 18 | * or implied warranties, including, but not limited to, the implied warranties of 19 | * merchantability and fitness for a particular purpose are disclaimed. In no event 20 | * shall the copyright holder be liable for any direct, indirect, incidental, special, 21 | * exemplary, or consequential damages (including, but not limited to, procurement of 22 | * substitute goods or services; loss of use, data, or profits; or business interruption) 23 | * however caused and on any theory of liability, whether in contract, strict liability, 24 | * or tort (including negligence or otherwise) arising in any way out of the use of this 25 | * software, even if advised of the possibility of such damage. 26 | */ 27 | 28 | #include "ros_ethercat_model/ros_ethercat.hpp" 29 | 30 | PLUGINLIB_EXPORT_CLASS(RosEthercat, hardware_interface::RobotHW) 31 | --------------------------------------------------------------------------------