├── docs ├── class_diagram.png └── class_diagram.uxf ├── ros2_robotiq_gripper.humble.repos ├── epick_description ├── meshes │ └── visual │ │ └── epick_body.stl ├── urdf │ ├── example.urdf.xacro │ ├── helper_macros.xacro │ ├── epick_body.xacro │ ├── epick_driver_ros2_control.xacro │ └── epick_single_suction_cup.xacro ├── config │ └── controllers.yaml ├── package.xml └── CMakeLists.txt ├── .gitignore ├── epick_moveit_studio ├── behavior_plugin_description.xml ├── test │ ├── CMakeLists.txt │ └── test_load_behaviors.cpp ├── package.xml ├── config │ └── tree_nodes_model.xml ├── CMakeLists.txt ├── src │ ├── get_epick_object_detection_status.cpp │ ├── behavior_loader.cpp │ └── compare_epick_object_detection_status.cpp └── include │ └── epick_moveit_studio │ ├── get_epick_object_detection_status.hpp │ └── compare_epick_object_detection_status.hpp ├── .env ├── epick_driver ├── hardware_interface_plugin.xml ├── package.xml ├── include │ └── epick_driver │ │ ├── visibility_control.hpp │ │ ├── crc_utils.hpp │ │ ├── default_serial_factory.hpp │ │ ├── driver_factory.hpp │ │ ├── default_driver_factory.hpp │ │ ├── driver_exception.hpp │ │ ├── serial_factory.hpp │ │ ├── default_serial.hpp │ │ ├── data_utils.hpp │ │ ├── fake │ │ └── fake_driver.hpp │ │ ├── default_driver.hpp │ │ ├── serial.hpp │ │ ├── hardware_interface_utils.hpp │ │ └── driver.hpp ├── tests │ ├── test_data_utils.cpp │ ├── CMakeLists.txt │ ├── mock │ │ ├── mock_serial.hpp │ │ └── mock_driver.hpp │ ├── test_default_serial_factory.cpp │ └── test_default_driver_factory.cpp ├── src │ ├── data_utils.cpp │ ├── default_serial_factory.cpp │ ├── default_serial.cpp │ ├── fake │ │ └── fake_driver.cpp │ ├── hardware_interface_utils.cpp │ └── crc_utils.cpp └── CMakeLists.txt ├── epick_msgs ├── msg │ └── ObjectDetectionStatus.msg ├── CMakeLists.txt └── package.xml ├── .flake8 ├── epick_moveit_plugin ├── moveit_ros_control_interface_plugins.xml ├── package.xml ├── CMakeLists.txt └── src │ └── epick_gripper_controller_allocator.cpp ├── .github └── workflows │ ├── industrial_ci.yml │ ├── ci-format.yml │ └── ci-ros-lint.yml ├── epick_controllers ├── controller_plugins.xml ├── package.xml ├── include │ └── epick_controllers │ │ ├── epick_status_publisher_controller.hpp │ │ ├── epick_controller.hpp │ │ └── epick_gripper_action_controller.hpp ├── CMakeLists.txt └── src │ └── epick_status_publisher_controller.cpp ├── epick_hardware_tests ├── package.xml ├── CMakeLists.txt └── src │ ├── command_line_utility.cpp │ ├── command_line_utility.hpp │ ├── break.cpp │ └── get_status.cpp ├── LICENSE ├── .pre-commit-config.yaml ├── .clang-format ├── .clang-tidy └── README.md /docs/class_diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/ros2_epick_gripper/HEAD/docs/class_diagram.png -------------------------------------------------------------------------------- /ros2_robotiq_gripper.humble.repos: -------------------------------------------------------------------------------- 1 | repositories: 2 | serial: 3 | type: git 4 | url: https://github.com/tylerjw/serial.git 5 | version: ros2 6 | -------------------------------------------------------------------------------- /epick_description/meshes/visual/epick_body.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/PickNikRobotics/ros2_epick_gripper/HEAD/epick_description/meshes/visual/epick_body.stl -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # VSCode 2 | .vscode 3 | 4 | # ROS 5 | build 6 | install 7 | log 8 | 9 | # QtCreator 10 | *.user 11 | 12 | # PyCharm 13 | venv 14 | .idea 15 | *.egg-info 16 | __pycache__ 17 | -------------------------------------------------------------------------------- /epick_moveit_studio/behavior_plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /epick_description/urdf/example.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /.env: -------------------------------------------------------------------------------- 1 | # This file is require by Nektos to run GitHub actions locally. 2 | # To install Nektos "act" command read the following: 3 | # https://github.com/nektos/act 4 | # To run it, move to the root folder of the GitHub repository and run: 5 | # ~/bin/act pull_request --workflows ./.github/workflows/industrial_ci_action.yml -s GITHUB_TOKEN 6 | 7 | ROS_DISTRO=humble 8 | DOCKER_IMAGE=ubuntu:22.04 9 | -------------------------------------------------------------------------------- /epick_driver/hardware_interface_plugin.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ROS2 hardware interface for the Robotiq EPick gripper. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /epick_msgs/msg/ObjectDetectionStatus.msg: -------------------------------------------------------------------------------- 1 | # Reports whether an object has been gripped or not. 2 | int8 status 3 | 4 | int8 UNKNOWN=0 # Unknown object detection. Regulating towards requested vacuum/pressure. 5 | int8 OBJECT_DETECTED_AT_MIN_PRESSURE=1 # Object detected. Minimum vacuum value reached. 6 | int8 OBJECT_DETECTED_AT_MAX_PRESSURE=2 # Object detected. Maximum vacuum value reached. 7 | int8 NO_OBJECT_DETECTED=3 # Object loss, dropped or gripping timeout reached. 8 | -------------------------------------------------------------------------------- /epick_description/config/controllers.yaml: -------------------------------------------------------------------------------- 1 | # Configuration file required by the Controller Manager to load the specified controls. 2 | # See https://github.com/ros-controls/ros2_controllers/blob/master/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml 3 | 4 | controller_manager: 5 | ros__parameters: 6 | update_rate: 30 # Hz 7 | 8 | epick_gripper_action_controller: 9 | type: epick_controllers/EpickGripperActionController 10 | epick_status_publisher_controller: 11 | type: epick_controllers/EpickStatusPublisherController 12 | -------------------------------------------------------------------------------- /epick_moveit_studio/test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_package(ament_cmake_gmock REQUIRED) 2 | find_package(ament_cmake_gtest REQUIRED) 3 | 4 | ament_add_gtest(test_load_behaviors 5 | test_load_behaviors.cpp 6 | ) 7 | ament_target_dependencies(test_load_behaviors moveit_studio_behavior_interface pluginlib) 8 | target_link_libraries(test_load_behaviors moveit_studio_behavior_interface::shared_resources_node) 9 | 10 | ament_add_gmock(test_compare_epick_object_detection_status test_compare_epick_object_detection_status.cpp) 11 | target_link_libraries(test_compare_epick_object_detection_status epick_behaviors) 12 | -------------------------------------------------------------------------------- /.flake8: -------------------------------------------------------------------------------- 1 | [flake8] 2 | max-line-length = 88 3 | 4 | # Report all errors starting with E, F, W or C - or Bugbear's B590 rule, which is a "pragmatic" version of E501 (line too long) 5 | select = E,F,W,C,B590 6 | 7 | # Ignore W503 - Line break occurred before a binary operator - because this error is introduced by Black formatter. 8 | # Ignore E203 - whitespace before ':' - because Black includes spaces in formatting slice expressions. 9 | # Ignore E501 - line too long - in favor of B590, which is more forgiving of long strings and comments that would be silly to break up. 10 | extend-ignore = W503, E203, E501 11 | -------------------------------------------------------------------------------- /epick_moveit_plugin/moveit_ros_control_interface_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /epick_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(epick_msgs) 3 | 4 | # This module provides installation directories as per the GNU coding standards. 5 | include(GNUInstallDirs) 6 | 7 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 8 | add_compile_options(-Wall -Wextra -Wpedantic) 9 | endif() 10 | 11 | find_package(ament_cmake REQUIRED) 12 | find_package(rosidl_default_generators REQUIRED) 13 | find_package(std_msgs REQUIRED) 14 | 15 | rosidl_generate_interfaces(epick_msgs 16 | "msg/ObjectDetectionStatus.msg" 17 | DEPENDENCIES 18 | std_msgs 19 | ) 20 | 21 | ament_export_dependencies(rosidl_default_runtime) 22 | 23 | ament_package() 24 | -------------------------------------------------------------------------------- /.github/workflows/industrial_ci.yml: -------------------------------------------------------------------------------- 1 | name: CI/CD 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | industrial_ci: 7 | strategy: 8 | matrix: 9 | env: 10 | - { ROS_DISTRO: humble, ROS_REPO: testing } 11 | - { ROS_DISTRO: humble, ROS_REPO: main } 12 | env: 13 | # Remove the epick_moveit_studio package from the workspace before running CI, since its dependencies are not available in CI for this repo. 14 | TARGET_WORKSPACE: '. -epick_moveit_studio' 15 | UPSTREAM_WORKSPACE: ros2_robotiq_gripper.humble.repos 16 | runs-on: ubuntu-latest 17 | steps: 18 | - uses: actions/checkout@v1 19 | - uses: "ros-industrial/industrial_ci@master" 20 | env: ${{matrix.env}} 21 | -------------------------------------------------------------------------------- /.github/workflows/ci-format.yml: -------------------------------------------------------------------------------- 1 | # This is a format job. Pre-commit has a first-party GitHub action, so we use 2 | # that: https://github.com/pre-commit/action 3 | 4 | name: Format 5 | 6 | on: 7 | workflow_dispatch: 8 | pull_request: 9 | push: 10 | branches: 11 | - main 12 | 13 | jobs: 14 | pre-commit: 15 | name: Format 16 | runs-on: ubuntu-22.04 17 | steps: 18 | - uses: actions/checkout@v3 19 | - uses: actions/setup-python@v4.4.0 20 | with: 21 | python-version: "3.10" 22 | - name: Install system hooks 23 | run: sudo apt install -qq clang-format-14 cppcheck 24 | - uses: pre-commit/action@v3.0.0 25 | with: 26 | extra_args: --all-files --hook-stage manual 27 | -------------------------------------------------------------------------------- /epick_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | epick_description 5 | 0.0.0 6 | URDF and xacro description package for the Robotiq EPick vacuum gripper 7 | giovanni 8 | Henning Kayser 9 | BSD 10 | 11 | ament_cmake 12 | 13 | ament_lint_auto 14 | ament_lint_common 15 | 16 | 17 | ament_cmake 18 | 19 | 20 | -------------------------------------------------------------------------------- /epick_moveit_studio/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | epick_moveit_studio 4 | 0.0.1 5 | 6 | epick_moveit_studio 7 | 8 | Joe Schornak 9 | Joe Schornak 10 | 11 | BSD-3-Clause 12 | 13 | https://github.com/PickNikRobotics/ 14 | 15 | ament_cmake 16 | builtin_interfaces 17 | 18 | ament_lint_auto 19 | 20 | epick_msgs 21 | moveit_studio_behavior_interface 22 | pluginlib 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /epick_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | epick_msgs 5 | 0.0.1 6 | Messages, services and actions for the Epick Controller. 7 | Giovanni Remigi 8 | BSD-3-Clause 9 | Giovanni Remigi 10 | 11 | ament_cmake 12 | 13 | rosidl_default_generators 14 | 15 | std_msgs 16 | 17 | rosidl_default_runtime 18 | 19 | rosidl_interface_packages 20 | 21 | 22 | ament_cmake 23 | 24 | 25 | -------------------------------------------------------------------------------- /epick_controllers/controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This controller provides an interface to for the Robotiq Epick gripper. 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /epick_moveit_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | epick_moveit_plugin 5 | 0.0.1 6 | Controllers for the Robotiq Epick gripper. 7 | Giovanni Remigi 8 | BSD-3-Clause 9 | Giovanni Remigi 10 | 11 | ament_cmake 12 | 13 | moveit_ros_control_interface 14 | moveit_simple_controller_manager 15 | pluginlib 16 | 17 | epick_controllers 18 | 19 | ament_lint_auto 20 | ament_lint_common 21 | 22 | 23 | ament_cmake 24 | 25 | 26 | -------------------------------------------------------------------------------- /epick_controllers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | epick_controllers 5 | 0.0.1 6 | Controllers for the Robotiq Epick gripper. 7 | Giovanni Remigi 8 | BSD-3-Clause 9 | Giovanni Remigi 10 | 11 | ament_cmake 12 | 13 | controller_interface 14 | epick_msgs 15 | rclcpp 16 | rclcpp_action 17 | realtime_tools 18 | 19 | std_srvs 20 | 21 | ament_lint_auto 22 | ament_lint_common 23 | 24 | 25 | ament_cmake 26 | 27 | 28 | -------------------------------------------------------------------------------- /epick_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(epick_description) 3 | 4 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 5 | add_compile_options(-Wall -Wextra -Wpedantic) 6 | endif() 7 | 8 | find_package(ament_cmake REQUIRED) 9 | 10 | install( 11 | DIRECTORY 12 | config 13 | launch 14 | meshes 15 | urdf 16 | DESTINATION 17 | share/${PROJECT_NAME} 18 | ) 19 | 20 | if(BUILD_TESTING) 21 | find_package(ament_lint_auto REQUIRED) 22 | # the following line skips the linter which checks for copyrights 23 | # comment the line when a copyright and license is added to all source files 24 | set(ament_cmake_copyright_FOUND TRUE) 25 | # the following line skips cpplint (only works in a git repo) 26 | # comment the line when this package is in a git repo and when 27 | # a copyright and license is added to all source files 28 | set(ament_cmake_cpplint_FOUND TRUE) 29 | ament_lint_auto_find_test_dependencies() 30 | endif() 31 | 32 | ament_package() 33 | -------------------------------------------------------------------------------- /.github/workflows/ci-ros-lint.yml: -------------------------------------------------------------------------------- 1 | name: ROS Lint 2 | on: 3 | pull_request: 4 | 5 | jobs: 6 | ament_lint: 7 | name: ament_${{ matrix.linter }} 8 | runs-on: ubuntu-22.04 9 | strategy: 10 | fail-fast: false 11 | matrix: 12 | linter: [copyright, lint_cmake] 13 | steps: 14 | - uses: actions/checkout@v3 15 | - uses: ros-tooling/setup-ros@0.6.2 16 | - uses: ros-tooling/action-ros-lint@v0.1 17 | with: 18 | distribution: humble 19 | linter: ${{ matrix.linter }} 20 | package-name: epick_driver 21 | epick_controllers 22 | epick_description 23 | epick_hardware_tests 24 | 25 | ament_lint_121: 26 | name: ament_${{ matrix.linter }} 27 | runs-on: ubuntu-22.04 28 | strategy: 29 | fail-fast: false 30 | matrix: 31 | linter: [cpplint] 32 | steps: 33 | - uses: actions/checkout@v3 34 | - uses: ros-tooling/setup-ros@0.6.2 35 | - uses: ros-tooling/action-ros-lint@v0.1 36 | with: 37 | distribution: humble 38 | linter: cpplint 39 | arguments: "--linelength=121 --filter=-whitespace/newline" 40 | package-name: epick_driver 41 | epick_controllers 42 | epick_description 43 | epick_hardware_tests 44 | -------------------------------------------------------------------------------- /epick_moveit_studio/config/tree_nodes_model.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 |

7 | Compare two ObjectDetectionStatus messages. 8 |

9 | 10 | First ObjectDetectionStatus message to compare. 11 | Second ObjectDetectionStatus message to compare. 12 | 13 | 14 | 15 | 16 | 17 | 18 |

19 | Captures an ObjectDetectionStatus message and makes it available on an output port. 20 |

21 | 22 | ObjectDetectionStatus topic the behavior subscribes to. 23 | Contains the message that was received on the topic. 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /epick_hardware_tests/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | epick_hardware_tests 5 | 0.0.1 6 | ROS2 driver for the Robotiq EPick gripper. 7 | Giovanni Remigi 8 | BSD 9 | Giovanni Remigi 10 | 11 | ament_cmake 12 | 13 | launch 14 | launch_ros 15 | epick_driver 16 | 17 | 18 | ament_clang_format 19 | 20 | ament_clang_tidy 21 | 22 | ament_cmake_copyright 23 | 24 | ament_cmake_lint_cmake 25 | 26 | ament_lint_auto 27 | 28 | ament_lint_common 29 | 30 | 31 | ament_cmake 32 | 33 | 34 | -------------------------------------------------------------------------------- /epick_moveit_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # See https://docs.ros.org/en/foxy/How-To-Guides/Ament-CMake-Documentation.html 2 | 3 | cmake_minimum_required(VERSION 3.8) 4 | project(epick_moveit_plugin VERSION 0.0.1 LANGUAGES CXX) 5 | 6 | # This module provides installation directories as per the GNU coding standards. 7 | include(GNUInstallDirs) 8 | 9 | set(CMAKE_EXPORT_COMPILE_COMMANDS true) 10 | 11 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 12 | add_compile_options(-Wall -Wextra -Wpedantic) 13 | endif() 14 | 15 | find_package(ament_cmake REQUIRED) 16 | find_package(moveit_ros_control_interface REQUIRED) 17 | find_package(moveit_simple_controller_manager REQUIRED) 18 | find_package(pluginlib REQUIRED) 19 | 20 | add_library(epick_gripper_controller_allocator SHARED 21 | src/epick_gripper_controller_allocator.cpp 22 | ) 23 | ament_target_dependencies(epick_gripper_controller_allocator PUBLIC 24 | pluginlib 25 | moveit_ros_control_interface 26 | moveit_simple_controller_manager 27 | ) 28 | 29 | # Register the Epick gripper controller allocator plugin with MoveIt's ros_control interface 30 | pluginlib_export_plugin_description_file(moveit_ros_control_interface moveit_ros_control_interface_plugins.xml) 31 | 32 | # Install our library. 33 | install( 34 | TARGETS epick_gripper_controller_allocator 35 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} # lib 36 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} # lib 37 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} # bin 38 | INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # include 39 | ) 40 | 41 | ament_package() 42 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | BSD 3-Clause License 2 | 3 | Copyright (c) 2022, PickNik Robotics 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 | 1. Redistributions of source code must retain the above copyright notice, this 10 | list of conditions and the following disclaimer. 11 | 12 | 2. 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 | 3. Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived from 18 | this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | -------------------------------------------------------------------------------- /epick_driver/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | epick_driver 5 | 0.0.1 6 | ROS2 driver for the Robotiq EPick gripper. 7 | Giovanni Remigi 8 | BSD 9 | Giovanni Remigi 10 | 11 | ament_cmake 12 | 13 | hardware_interface 14 | lifecycle_msgs 15 | pluginlib 16 | rclcpp_lifecycle 17 | serial 18 | 19 | 20 | ament_clang_format 21 | 22 | ament_clang_tidy 23 | 24 | ament_cmake_copyright 25 | 26 | ament_cmake_lint_cmake 27 | 28 | ament_lint_auto 29 | 30 | ament_lint_common 31 | 32 | 33 | ament_cmake_gmock 34 | ros2_control_test_assets 35 | 36 | 37 | ament_cmake 38 | 39 | 40 | -------------------------------------------------------------------------------- /epick_moveit_studio/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.8) 2 | project(epick_moveit_studio LANGUAGES CXX) 3 | 4 | set(CMAKE_EXPORT_COMPILE_COMMANDS true) 5 | 6 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 7 | add_compile_options(-Wall -Wextra -Wpedantic) 8 | endif() 9 | 10 | find_package(ament_cmake REQUIRED) 11 | find_package(epick_msgs REQUIRED) 12 | find_package(moveit_studio_behavior_interface REQUIRED) 13 | find_package(pluginlib REQUIRED) 14 | 15 | add_library(epick_behaviors SHARED 16 | src/compare_epick_object_detection_status.cpp 17 | src/get_epick_object_detection_status.cpp 18 | src/behavior_loader.cpp 19 | ) 20 | target_include_directories(epick_behaviors 21 | PUBLIC $ 22 | PUBLIC $ 23 | ) 24 | target_link_libraries(epick_behaviors PUBLIC moveit_studio_behavior_interface::async_behavior_base) 25 | ament_target_dependencies(epick_behaviors PUBLIC 26 | epick_msgs 27 | moveit_studio_behavior_interface 28 | pluginlib 29 | ) 30 | 31 | install( 32 | TARGETS 33 | epick_behaviors 34 | EXPORT export_${PROJECT_NAME} 35 | ARCHIVE DESTINATION lib 36 | LIBRARY DESTINATION lib 37 | RUNTIME DESTINATION bin 38 | INCLUDES DESTINATION include 39 | ) 40 | 41 | install( 42 | DIRECTORY 43 | config 44 | DESTINATION share/${PROJECT_NAME} 45 | ) 46 | 47 | 48 | if(BUILD_TESTING) 49 | add_subdirectory(test) 50 | endif() 51 | 52 | pluginlib_export_plugin_description_file(moveit_studio_behavior_interface behavior_plugin_description.xml) 53 | 54 | ament_export_include_directories(include) 55 | ament_export_libraries(epick_behaviors) 56 | ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) 57 | ament_export_dependencies(epick_msgs moveit_studio_behavior_interface pluginlib) 58 | ament_package() 59 | -------------------------------------------------------------------------------- /epick_description/urdf/helper_macros.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 57 | 58 | 59 | 60 | -------------------------------------------------------------------------------- /.pre-commit-config.yaml: -------------------------------------------------------------------------------- 1 | # To use: 2 | # 3 | # pre-commit run -a 4 | # 5 | # Or: 6 | # 7 | # pre-commit install # (runs every time you commit in git) 8 | # 9 | # To update this file: 10 | # 11 | # pre-commit autoupdate 12 | # 13 | # See https://github.com/pre-commit/pre-commit 14 | repos: 15 | # Standard hooks 16 | - repo: https://github.com/pre-commit/pre-commit-hooks 17 | rev: v4.4.0 18 | hooks: 19 | - id: check-added-large-files 20 | exclude: \.(stl|dae)$ 21 | - id: check-ast 22 | - id: check-case-conflict 23 | - id: check-docstring-first 24 | - id: check-merge-conflict 25 | - id: check-symlinks 26 | - id: check-yaml 27 | args: ['--unsafe'] # Fixes errors parsing custom YAML constructors like ur_description's !degrees 28 | - id: debug-statements 29 | - id: end-of-file-fixer 30 | - id: mixed-line-ending 31 | - id: trailing-whitespace 32 | - id: fix-byte-order-marker 33 | 34 | - repo: https://github.com/psf/black 35 | rev: 23.3.0 36 | hooks: 37 | - id: black 38 | 39 | - repo: https://github.com/pycqa/flake8 40 | rev: 5.0.4 41 | hooks: 42 | - id: flake8 43 | # configured in .flake8 file 44 | 45 | - repo: https://github.com/codespell-project/codespell 46 | rev: v2.2.5 47 | hooks: 48 | - id: codespell 49 | args: ['--write-changes', '-L', 'atleast,inout'] # Provide a comma-separated list of misspelled words that codespell should ignore (for example: '-L', 'word1,word2,word3'). 50 | exclude: \.(svg|pyc|stl|dae|lock)$ 51 | 52 | - repo: https://github.com/pre-commit/mirrors-clang-format 53 | rev: v16.0.6 54 | hooks: 55 | - id: clang-format 56 | name: clang-format 57 | description: Format files with ClangFormat. 58 | entry: clang-format-14 59 | language: system 60 | files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|m|proto|vert)$ 61 | args: ['-fallback-style=none', '-i'] 62 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/visibility_control.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #define EPICK_DRIVER_EXPORT __attribute__((visibility("default"))) 32 | #define EPICK_DRIVER_IMPORT 33 | #define EPICK_DRIVER_PUBLIC __attribute__((visibility("default"))) 34 | #define EPICK_DRIVER_LOCAL __attribute__((visibility("hidden"))) 35 | #define EPICK_DRIVER_PUBLIC_TYPE 36 | -------------------------------------------------------------------------------- /epick_hardware_tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # See https://docs.ros.org/en/foxy/How-To-Guides/Ament-CMake-Documentation.html 2 | 3 | cmake_minimum_required(VERSION 3.8) 4 | project(epick_hardware_tests VERSION 0.0.1 LANGUAGES CXX) 5 | 6 | # This module provides installation directories as per the GNU coding standards. 7 | include(GNUInstallDirs) 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(hardware_interface REQUIRED) 15 | find_package(pluginlib REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(serial REQUIRED) 18 | find_package(epick_driver REQUIRED) 19 | 20 | set(THIS_PACKAGE_INCLUDE_DEPENDS 21 | ament_cmake 22 | hardware_interface 23 | pluginlib 24 | rclcpp 25 | rclcpp_lifecycle 26 | serial 27 | ) 28 | 29 | add_executable(activate 30 | src/activate.cpp 31 | src/command_line_utility.hpp 32 | src/command_line_utility.cpp 33 | ) 34 | ament_target_dependencies(activate epick_driver) 35 | 36 | add_executable(deactivate 37 | src/deactivate.cpp 38 | src/command_line_utility.hpp 39 | src/command_line_utility.cpp 40 | ) 41 | ament_target_dependencies(deactivate epick_driver) 42 | 43 | add_executable(get_status 44 | src/get_status.cpp 45 | src/command_line_utility.hpp 46 | src/command_line_utility.cpp 47 | ) 48 | ament_target_dependencies(get_status epick_driver) 49 | 50 | add_executable(grip 51 | src/grip.cpp 52 | src/command_line_utility.hpp 53 | src/command_line_utility.cpp 54 | ) 55 | ament_target_dependencies(grip epick_driver) 56 | 57 | add_executable(release 58 | src/release.cpp 59 | src/command_line_utility.hpp 60 | src/command_line_utility.cpp 61 | ) 62 | ament_target_dependencies(release epick_driver) 63 | 64 | add_executable(break 65 | src/break.cpp 66 | src/command_line_utility.hpp 67 | src/command_line_utility.cpp 68 | ) 69 | ament_target_dependencies(break epick_driver) 70 | 71 | ament_package() 72 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/crc_utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | #include 33 | 34 | namespace epick_driver::crc_utils 35 | { 36 | /** 37 | * @brief Compute the CRC for the CRC-16 MODBUS protocol. 38 | * @param data The data to compute the CRC for. 39 | * @return A 16-bits CRC. 40 | */ 41 | uint16_t compute_crc(const std::vector& data); 42 | } // namespace epick_driver::crc_utils 43 | -------------------------------------------------------------------------------- /epick_driver/tests/test_data_utils.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | 31 | #include 32 | 33 | namespace epick_driver::test 34 | { 35 | TEST(TestDataUtils, to_lower) 36 | { 37 | const char* str1 = "HeLlO"; 38 | ASSERT_EQ(data_utils::to_lower(str1), "hello"); 39 | 40 | std::string str2{ "HeLlO" }; 41 | ASSERT_EQ(data_utils::to_lower(str2), "hello"); 42 | } 43 | } // namespace epick_driver::test 44 | -------------------------------------------------------------------------------- /.clang-format: -------------------------------------------------------------------------------- 1 | --- 2 | BasedOnStyle: Google 3 | ColumnLimit: 120 4 | MaxEmptyLinesToKeep: 1 5 | SortIncludes: false 6 | 7 | Standard: Auto 8 | IndentWidth: 2 9 | TabWidth: 2 10 | UseTab: Never 11 | AccessModifierOffset: -2 12 | ConstructorInitializerIndentWidth: 2 13 | NamespaceIndentation: None 14 | ContinuationIndentWidth: 4 15 | IndentCaseLabels: true 16 | IndentFunctionDeclarationAfterType: false 17 | 18 | AlignEscapedNewlinesLeft: false 19 | AlignTrailingComments: true 20 | 21 | AllowAllParametersOfDeclarationOnNextLine: false 22 | ExperimentalAutoDetectBinPacking: false 23 | ObjCSpaceBeforeProtocolList: true 24 | Cpp11BracedListStyle: false 25 | 26 | AllowShortBlocksOnASingleLine: true 27 | AllowShortIfStatementsOnASingleLine: false 28 | AllowShortLoopsOnASingleLine: false 29 | AllowShortFunctionsOnASingleLine: None 30 | AllowShortCaseLabelsOnASingleLine: false 31 | 32 | AlwaysBreakTemplateDeclarations: true 33 | AlwaysBreakBeforeMultilineStrings: false 34 | BreakBeforeBinaryOperators: false 35 | BreakBeforeTernaryOperators: false 36 | BreakConstructorInitializersBeforeComma: true 37 | 38 | BinPackParameters: true 39 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 40 | DerivePointerBinding: false 41 | PointerBindsToType: true 42 | 43 | PenaltyExcessCharacter: 50 44 | PenaltyBreakBeforeFirstCallParameter: 30 45 | PenaltyBreakComment: 1000 46 | PenaltyBreakFirstLessLess: 10 47 | PenaltyBreakString: 100 48 | PenaltyReturnTypeOnItsOwnLine: 50 49 | 50 | SpacesBeforeTrailingComments: 2 51 | SpacesInParentheses: false 52 | SpacesInAngles: false 53 | SpaceInEmptyParentheses: false 54 | SpacesInCStyleCastParentheses: false 55 | SpaceAfterCStyleCast: false 56 | SpaceAfterControlStatementKeyword: true 57 | SpaceBeforeAssignmentOperators: true 58 | 59 | # Configure each individual brace in BraceWrapping 60 | BreakBeforeBraces: Custom 61 | 62 | # Control of individual brace wrapping cases 63 | BraceWrapping: 64 | AfterCaseLabel: true 65 | AfterClass: true 66 | AfterControlStatement: true 67 | AfterEnum: true 68 | AfterFunction: true 69 | AfterNamespace: true 70 | AfterStruct: true 71 | AfterUnion: true 72 | BeforeCatch: true 73 | BeforeElse: true 74 | IndentBraces: false 75 | ... 76 | -------------------------------------------------------------------------------- /.clang-tidy: -------------------------------------------------------------------------------- 1 | --- 2 | # TODO(henningkayser): Re-enable performance-unnecessary-value-param once #214 is resolved 3 | Checks: '-*, 4 | performance-*, 5 | -performance-unnecessary-value-param, 6 | llvm-namespace-comment, 7 | modernize-redundant-void-arg, 8 | modernize-use-nullptr, 9 | modernize-use-default, 10 | modernize-use-override, 11 | modernize-loop-convert, 12 | readability-named-parameter, 13 | readability-redundant-smartptr-get, 14 | readability-redundant-string-cstr, 15 | readability-simplify-boolean-expr, 16 | readability-container-size-empty, 17 | readability-identifier-naming, 18 | ' 19 | HeaderFilterRegex: '' 20 | AnalyzeTemporaryDtors: false 21 | CheckOptions: 22 | - key: llvm-namespace-comment.ShortNamespaceLines 23 | value: '10' 24 | - key: llvm-namespace-comment.SpacesBeforeComments 25 | value: '2' 26 | - key: readability-braces-around-statements.ShortStatementLines 27 | value: '2' 28 | # type names 29 | - key: readability-identifier-naming.ClassCase 30 | value: CamelCase 31 | - key: readability-identifier-naming.EnumCase 32 | value: CamelCase 33 | - key: readability-identifier-naming.UnionCase 34 | value: CamelCase 35 | # method names 36 | - key: readability-identifier-naming.MethodCase 37 | value: camelBack 38 | # variable names 39 | - key: readability-identifier-naming.VariableCase 40 | value: lower_case 41 | - key: readability-identifier-naming.ClassMemberSuffix 42 | value: '_' 43 | # const static or global variables are UPPER_CASE 44 | - key: readability-identifier-naming.EnumConstantCase 45 | value: UPPER_CASE 46 | - key: readability-identifier-naming.StaticConstantCase 47 | value: UPPER_CASE 48 | - key: readability-identifier-naming.ClassConstantCase 49 | value: UPPER_CASE 50 | - key: readability-identifier-naming.GlobalVariableCase 51 | value: UPPER_CASE 52 | ... 53 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/default_serial_factory.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace epick_driver 38 | { 39 | /** 40 | * This class is used to create a default driver to interact with the hardware. 41 | */ 42 | class DefaultSerialFactory : public SerialFactory 43 | { 44 | public: 45 | DefaultSerialFactory() = default; 46 | 47 | /** 48 | * @brief Create a serial interface. 49 | * @param info The hardware information. 50 | * @return A sarial interface to communicate with the hardware. 51 | */ 52 | std::unique_ptr create(const hardware_interface::HardwareInfo& info) const; 53 | 54 | protected: 55 | // Seam for testing. 56 | virtual std::unique_ptr create_serial() const; 57 | }; 58 | } // namespace epick_driver 59 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/driver_factory.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | 37 | namespace epick_driver 38 | { 39 | /** 40 | * The hardware interface internally uses a factory to create and configure a 41 | * driver to interact with the Epick Gripper. 42 | * A factory is used to keep the code cleaner but also to simplify testing. 43 | * With a factory, we can test that the parameters read by the hardware 44 | * interface are correctly parsed and passed down to the driver. 45 | * A factory can also be mocked to return different implementation of the 46 | * Driver (or mocks) that do not require interaction with the real hardware. 47 | */ 48 | class DriverFactory 49 | { 50 | public: 51 | virtual std::unique_ptr create(const hardware_interface::HardwareInfo& info) const = 0; 52 | }; 53 | } // namespace epick_driver 54 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/default_driver_factory.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | namespace epick_driver 38 | { 39 | /** 40 | * This class is used to create a default driver to interact with the hardware. 41 | */ 42 | class DefaultDriverFactory : public DriverFactory 43 | { 44 | public: 45 | DefaultDriverFactory() = default; 46 | 47 | /** 48 | * @brief Create a driver. 49 | * @param info The hardware information. 50 | * @return A driver to interact with the hardware. 51 | */ 52 | std::unique_ptr create(const hardware_interface::HardwareInfo& info) const; 53 | 54 | protected: 55 | // Seam for testing. 56 | virtual std::unique_ptr create_driver(const hardware_interface::HardwareInfo& info) const; 57 | }; 58 | } // namespace epick_driver 59 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/driver_exception.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | namespace epick_driver 36 | { 37 | /** 38 | * This is a custom exception thrown by the Driver. 39 | */ 40 | class DriverException : public std::exception 41 | { 42 | std::string what_; 43 | 44 | public: 45 | explicit DriverException(const std::string& description) 46 | { 47 | std::stringstream ss; 48 | ss << "DriverException: " << description << "."; 49 | what_ = ss.str(); 50 | } 51 | 52 | DriverException(const DriverException& other) : what_(other.what_) 53 | { 54 | } 55 | 56 | ~DriverException() override = default; 57 | 58 | // Disable copy constructors 59 | DriverException& operator=(const DriverException&) = delete; 60 | 61 | [[nodiscard]] const char* what() const throw() override 62 | { 63 | return what_.c_str(); 64 | } 65 | }; 66 | } // namespace epick_driver 67 | -------------------------------------------------------------------------------- /epick_driver/tests/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Add support for GTest. 2 | find_package(ament_cmake_gtest REQUIRED) 3 | find_package(ament_cmake_gmock REQUIRED) 4 | find_package(ament_lint_auto REQUIRED) 5 | find_package(ros2_control_test_assets REQUIRED) 6 | 7 | ament_lint_auto_find_test_dependencies() 8 | 9 | # GMock throws an error if we don't switch off this option in tests. 10 | if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") 11 | add_compile_options(-Wno-sign-conversion) 12 | endif() 13 | 14 | ############################################################################### 15 | # test_default_driver 16 | 17 | ament_add_gmock(test_default_driver 18 | test_default_driver.cpp 19 | mock/mock_serial.hpp 20 | ) 21 | target_include_directories(test_default_driver 22 | PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} 23 | ) 24 | target_link_libraries(test_default_driver epick_driver) 25 | 26 | ############################################################################### 27 | # test_epick_gripper_hardware_interface 28 | 29 | ament_add_gmock(test_epick_gripper_hardware_interface 30 | test_epick_gripper_hardware_interface.cpp 31 | ) 32 | target_include_directories(test_epick_gripper_hardware_interface 33 | PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} 34 | ) 35 | target_link_libraries(test_epick_gripper_hardware_interface epick_driver) 36 | ament_target_dependencies(test_epick_gripper_hardware_interface 37 | ros2_control_test_assets 38 | ) 39 | 40 | ############################################################################### 41 | # test_default_serial_factory 42 | 43 | ament_add_gmock(test_default_serial_factory 44 | test_default_serial_factory.cpp 45 | mock/mock_serial.hpp 46 | ) 47 | target_include_directories(test_default_serial_factory 48 | PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} 49 | ) 50 | target_link_libraries(test_default_serial_factory epick_driver) 51 | 52 | ############################################################################### 53 | # test_default_driver_factory 54 | 55 | ament_add_gmock(test_default_driver_factory 56 | test_default_driver_factory.cpp 57 | mock/mock_driver.hpp 58 | ) 59 | target_include_directories(test_default_driver_factory 60 | PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} 61 | ) 62 | target_link_libraries(test_default_driver_factory epick_driver) 63 | 64 | ############################################################################### 65 | # test_data_utils 66 | 67 | ament_add_gmock(test_data_utils 68 | test_data_utils.cpp 69 | ) 70 | target_include_directories(test_data_utils 71 | PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} 72 | ) 73 | target_link_libraries(test_data_utils epick_driver) 74 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/serial_factory.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | 37 | namespace epick_driver 38 | { 39 | /** 40 | * This factory is used to create and configure a Serial interface 41 | * implementation that is used by the driver to interact with the hardware. 42 | * A factory is used to keep the code cleaner but also to simplify testing. 43 | * With a factory, we can test that the parameters read by the hardware 44 | * interface are correctly parsed and passed down to the serial interface. 45 | * A factory can also be mocked to return different implementation of the 46 | * serial interface (or mocks) that do not require interaction with the real 47 | * hardware. 48 | */ 49 | class SerialFactory 50 | { 51 | public: 52 | virtual std::unique_ptr create(const hardware_interface::HardwareInfo& info) const = 0; 53 | }; 54 | 55 | } // namespace epick_driver 56 | -------------------------------------------------------------------------------- /epick_description/urdf/epick_body.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 22 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 67 | 68 | 69 | 70 | -------------------------------------------------------------------------------- /epick_driver/tests/mock/mock_serial.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | namespace epick_driver::test 39 | { 40 | class MockSerial : public epick_driver::Serial 41 | { 42 | public: 43 | MOCK_METHOD(void, open, (), (override)); 44 | MOCK_METHOD(bool, is_open, (), (override, const)); 45 | MOCK_METHOD(void, close, (), (override)); 46 | MOCK_METHOD(std::vector, read, (size_t size), (override)); 47 | MOCK_METHOD(void, write, (const std::vector& buffer), (override)); 48 | MOCK_METHOD(void, set_port, (const std::string& port), (override)); 49 | MOCK_METHOD(std::string, get_port, (), (override, const)); 50 | MOCK_METHOD(void, set_timeout, (std::chrono::milliseconds timeout), (override)); 51 | MOCK_METHOD(std::chrono::milliseconds, get_timeout, (), (override, const)); 52 | MOCK_METHOD(void, set_baudrate, (uint32_t baudrate), (override)); 53 | MOCK_METHOD(uint32_t, get_baudrate, (), (override, const)); 54 | }; 55 | } // namespace epick_driver::test 56 | -------------------------------------------------------------------------------- /epick_controllers/include/epick_controllers/epick_status_publisher_controller.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | namespace epick_controllers 36 | { 37 | class EpickStatusPublisherController : public controller_interface::ControllerInterface 38 | { 39 | public: 40 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 41 | 42 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 43 | 44 | controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; 45 | 46 | CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; 47 | 48 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; 49 | 50 | CallbackReturn on_init() override; 51 | 52 | private: 53 | rclcpp::Publisher::SharedPtr object_detection_status_pub_; 54 | }; 55 | } // namespace epick_controllers 56 | -------------------------------------------------------------------------------- /epick_driver/tests/mock/mock_driver.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | 37 | namespace epick_driver::test 38 | { 39 | class MockDriver : public epick_driver::Driver 40 | { 41 | public: 42 | MOCK_METHOD(void, set_slave_address, (uint8_t slave_address), (override)); 43 | MOCK_METHOD(void, set_mode, (GripperMode mode), (override)); 44 | MOCK_METHOD(void, set_grip_max_vacuum_pressure, (float vacuum_pressure), (override)); 45 | MOCK_METHOD(void, set_grip_min_vacuum_pressure, (float vacuum_pressure), (override)); 46 | MOCK_METHOD(void, set_grip_timeout, (std::chrono::milliseconds grip_timeout), (override)); 47 | MOCK_METHOD(void, set_release_timeout, (std::chrono::milliseconds release_timeout), (override)); 48 | MOCK_METHOD(bool, connect, (), (override)); 49 | MOCK_METHOD(void, disconnect, (), (override)); 50 | MOCK_METHOD(void, activate, (), (override)); 51 | MOCK_METHOD(void, deactivate, (), (override)); 52 | MOCK_METHOD(void, grip, (), (override)); 53 | MOCK_METHOD(void, release, (), (override)); 54 | MOCK_METHOD(GripperStatus, get_status, (), (override)); 55 | }; 56 | } // namespace epick_driver::test 57 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/default_serial.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | namespace serial 40 | { 41 | class Serial; 42 | } 43 | 44 | namespace epick_driver 45 | { 46 | class DefaultSerial : public Serial 47 | { 48 | public: 49 | /** 50 | * Creates a Serial object to send and receive bytes to and from the serial 51 | * port. 52 | */ 53 | DefaultSerial(); 54 | 55 | void open() override; 56 | 57 | [[nodiscard]] bool is_open() const override; 58 | 59 | void close() override; 60 | 61 | [[nodiscard]] std::vector read(size_t size = 1) override; 62 | void write(const std::vector& data) override; 63 | 64 | void set_port(const std::string& port) override; 65 | [[nodiscard]] std::string get_port() const override; 66 | 67 | void set_timeout(std::chrono::milliseconds timeout_ms) override; 68 | [[nodiscard]] std::chrono::milliseconds get_timeout() const override; 69 | 70 | void set_baudrate(uint32_t baudrate) override; 71 | [[nodiscard]] uint32_t get_baudrate() const override; 72 | 73 | private: 74 | std::unique_ptr serial_ = nullptr; 75 | }; 76 | } // namespace epick_driver 77 | -------------------------------------------------------------------------------- /epick_moveit_studio/src/get_epick_object_detection_status.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | 31 | #include 32 | #include 33 | 34 | namespace 35 | { 36 | /** @brief Maximum duration to wait for a message to be published before failing. */ 37 | constexpr auto kWaitDuration = std::chrono::seconds{ 1 }; 38 | } // namespace 39 | 40 | namespace epick_moveit_studio 41 | { 42 | GetEpickObjectDetectionStatus::GetEpickObjectDetectionStatus( 43 | const std::string& name, const BT::NodeConfiguration& config, 44 | const std::shared_ptr& shared_resources) 45 | : moveit_studio::behaviors::GetMessageFromTopicBehaviorBase(name, config, 46 | shared_resources) 47 | { 48 | } 49 | 50 | tl::expected, std::string> GetEpickObjectDetectionStatus::getWaitForMessageTimeout() 51 | { 52 | return kWaitDuration; 53 | } 54 | 55 | } // namespace epick_moveit_studio 56 | 57 | template class moveit_studio::behaviors::GetMessageFromTopicBehaviorBase; 58 | -------------------------------------------------------------------------------- /epick_controllers/include/epick_controllers/epick_controller.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | namespace epick_controllers 36 | { 37 | class EpickController : public controller_interface::ControllerInterface 38 | { 39 | public: 40 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 41 | 42 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 43 | 44 | controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; 45 | 46 | CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; 47 | 48 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; 49 | 50 | CallbackReturn on_init() override; 51 | 52 | private: 53 | // When we send a true, the gripper will begin to grip, when false the gripper will release. 54 | rclcpp::Service::SharedPtr grip_srv_; 55 | 56 | // The logic of the server to control the gripper. 57 | bool grip_cmd(std_srvs::srv::SetBool::Request::SharedPtr request, 58 | std_srvs::srv::SetBool::Response::SharedPtr response); 59 | }; 60 | } // namespace epick_controllers 61 | -------------------------------------------------------------------------------- /epick_moveit_studio/src/behavior_loader.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | // Include headers for your custom Behaviors 36 | #include 37 | #include 38 | 39 | namespace epick_moveit_studio 40 | { 41 | class BehaviorLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase 42 | { 43 | public: 44 | void registerBehaviors(BT::BehaviorTreeFactory& factory, 45 | const std::shared_ptr& shared_resources) override 46 | { 47 | moveit_studio::behaviors::registerBehavior(factory, 48 | "CompareEpickObjectDetectionStatus"); 49 | moveit_studio::behaviors::registerBehavior(factory, "GetEpickObjectDetectionStatus", 50 | shared_resources); 51 | } 52 | }; 53 | 54 | } // namespace epick_moveit_studio 55 | 56 | PLUGINLIB_EXPORT_CLASS(epick_moveit_studio::BehaviorLoader, moveit_studio::behaviors::SharedResourcesNodeLoaderBase); 57 | -------------------------------------------------------------------------------- /epick_moveit_studio/src/compare_epick_object_detection_status.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | namespace 35 | { 36 | constexpr auto kPortIDValue1 = "value1"; 37 | constexpr auto kPortIDValue2 = "value2"; 38 | } // namespace 39 | 40 | namespace epick_moveit_studio 41 | { 42 | CompareEpickObjectDetectionStatus::CompareEpickObjectDetectionStatus(const std::string& name, 43 | const BT::NodeConfiguration& config) 44 | : BT::SyncActionNode(name, config) 45 | { 46 | } 47 | 48 | BT::PortsList CompareEpickObjectDetectionStatus::providedPorts() 49 | { 50 | return BT::PortsList{ 51 | BT::InputPort(kPortIDValue1), 52 | BT::InputPort(kPortIDValue2), 53 | }; 54 | } 55 | 56 | BT::NodeStatus CompareEpickObjectDetectionStatus::tick() 57 | { 58 | const auto value1 = getInput(kPortIDValue1); 59 | const auto value2 = getInput(kPortIDValue2); 60 | 61 | if (!value1.has_value() || !value2.has_value()) 62 | { 63 | return BT::NodeStatus::FAILURE; 64 | } 65 | 66 | if (value1.value().status == value2.value().status) 67 | { 68 | return BT::NodeStatus::SUCCESS; 69 | } 70 | else 71 | { 72 | return BT::NodeStatus::FAILURE; 73 | } 74 | } 75 | } // namespace epick_moveit_studio 76 | -------------------------------------------------------------------------------- /epick_hardware_tests/src/command_line_utility.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include "command_line_utility.hpp" 30 | 31 | #include 32 | 33 | void CommandLineUtility::registerHandler(const std::string& parameter, ParameterHandler handler, bool isMandatory) 34 | { 35 | handlers[parameter] = handler; 36 | if (isMandatory) 37 | { 38 | mandatoryParams.insert(parameter); 39 | } 40 | } 41 | 42 | bool CommandLineUtility::parse(int argc, char* argv[]) 43 | { 44 | for (int i = 1; i < argc; i++) 45 | { 46 | auto it = handlers.find(argv[i]); 47 | if (it != handlers.end()) 48 | { 49 | receivedParams.insert(it->first); 50 | 51 | if (std::holds_alternative(it->second)) 52 | { 53 | auto& handler = std::get(it->second); 54 | i++; 55 | if (i < argc) 56 | { 57 | handler(argv[i]); 58 | } 59 | else 60 | { 61 | std::cerr << it->first << " requires a value.\n"; 62 | } 63 | } 64 | else if (std::holds_alternative(it->second)) 65 | { 66 | auto& handler = std::get(it->second); 67 | handler(); 68 | } 69 | } 70 | else 71 | { 72 | std::cerr << "Unknown argument: " << argv[i] << "\n"; 73 | return false; 74 | } 75 | } 76 | 77 | for (const auto& param : mandatoryParams) 78 | { 79 | if (receivedParams.find(param) == receivedParams.end()) 80 | { 81 | std::cerr << "Missing mandatory argument: " << param << "\n"; 82 | return false; 83 | } 84 | } 85 | 86 | return true; 87 | } 88 | -------------------------------------------------------------------------------- /epick_hardware_tests/src/command_line_utility.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | /** 38 | * Class to parse the command line. 39 | */ 40 | class CommandLineUtility 41 | { 42 | using LambdaWithValue = std::function; 43 | using LambdaWithoutValue = std::function; 44 | using ParameterHandler = std::variant; 45 | 46 | public: 47 | /** 48 | * Assign to each command parameter a lambda function to handle it. 49 | * @param parameter The parameter to handle. 50 | * @param handler The lambda function to handle the parameter value. 51 | * @param isMandatory True if the parameter is mandatory, else otherwise. 52 | */ 53 | void registerHandler(const std::string& parameter, ParameterHandler handler, bool isMandatory = false); 54 | 55 | /** 56 | * Parse the command line and read all parameters. 57 | * @param argc The number of tokens in the command line. 58 | * @param argv The list of tokens. 59 | * @return True if the parsing is successful. 60 | */ 61 | bool parse(int argc, char* argv[]); 62 | 63 | private: 64 | // Map that associates a lambda function to each parameter to process the expected value. 65 | std::map handlers; 66 | 67 | // Store all mandatory parameters. 68 | std::set mandatoryParams; 69 | 70 | // Store the parameters which are parsed in the command line. If a parameter is mandatory, 71 | // but cannot be found in the received parameters, then print an error. 72 | std::set receivedParams; 73 | }; 74 | -------------------------------------------------------------------------------- /epick_description/urdf/epick_driver_ros2_control.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 14 | 15 | 16 | 17 | epick_driver/EpickGripperHardwareInterface 18 | 19 | 20 | 21 | ${usb_port} 22 | ${baud_rate} 23 | ${timeout} 24 | 25 | 26 | 27 | 28 | ${use_fake_hardware} 29 | 30 | 31 | ${slave_address} 32 | 33 | 34 | ${mode} 35 | 36 | 37 | ${grip_max_vacuum_pressure} 38 | ${grip_min_vacuum_pressure} 39 | ${grip_timeout} 40 | ${release_timeout} 41 | 42 | 43 | 44 | 45 | 50 | 51 | 52 | 58 | 59 | 60 | 67 | 68 | 69 | 70 | 74 | 75 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/data_utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | /** 37 | * Utility class to convert between commonly used data types. 38 | */ 39 | namespace epick_driver::data_utils 40 | { 41 | /** 42 | * Convert a sequence of uint8_t into a sequence of hex numbers. 43 | * @param bytes The sequence of bytes. 44 | * @return A string containing the sequence of hex numbers. 45 | */ 46 | std::string to_hex(const std::vector& bytes); 47 | 48 | /** 49 | * Convert a sequence of uint16_t into a sequence of hex numbers. 50 | * @param bytes The sequence of bytes. 51 | * @return A string containing the sequence of hex numbers. 52 | */ 53 | std::string to_hex(const std::vector& bytes); 54 | 55 | /** 56 | * Convert a byte to a binary representation for testing purposes. 57 | * @param byte The byte to decode. 58 | * @return The binary representation of the given byte. 59 | */ 60 | std::string to_binary_string(const uint8_t byte); 61 | 62 | /** 63 | * Get the Most Significant Byte (MSB) of the given value. 64 | * @param value A 16-bits value. 65 | * @return The Most Significant Byte (MSB) of the given value. 66 | */ 67 | uint8_t get_msb(uint16_t value); 68 | 69 | /** 70 | * Get the Least Significant Byte (LSB) of the given value. 71 | * @param value A 16-bits value. 72 | * @return The Least Significant Byte (LSB) of the given value. 73 | */ 74 | uint8_t get_lsb(uint16_t value); 75 | 76 | /** 77 | * Convert a string to lowercase. 78 | * @param str The string to convert. 79 | * @return The lowercase string. 80 | */ 81 | std::string to_lower(const std::string& str); 82 | 83 | } // namespace epick_driver::data_utils 84 | -------------------------------------------------------------------------------- /epick_moveit_plugin/src/epick_gripper_controller_allocator.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2023, PickNik 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 Fraunhofer IPA 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 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | namespace 42 | { 43 | constexpr auto kGripperCommandAction = "gripper_cmd"; 44 | } // namespace 45 | 46 | namespace epick_moveit_plugin 47 | { 48 | /** 49 | * \brief Controller allocator plugin to allow the EpickGripperActionController to execute trajectories using MoveIt in 50 | * the same way as the ros2_controllers GripperActionController 51 | */ 52 | class EpickGripperControllerAllocator : public moveit_ros_control_interface::ControllerHandleAllocator 53 | { 54 | public: 55 | moveit_controller_manager::MoveItControllerHandlePtr alloc(const rclcpp::Node::SharedPtr& node, 56 | const std::string& name, 57 | const std::vector& /* resources */) override 58 | { 59 | return std::make_shared(node, name, 60 | kGripperCommandAction); 61 | } 62 | }; 63 | 64 | } // namespace epick_moveit_plugin 65 | 66 | PLUGINLIB_EXPORT_CLASS(epick_moveit_plugin::EpickGripperControllerAllocator, 67 | moveit_ros_control_interface::ControllerHandleAllocator); 68 | -------------------------------------------------------------------------------- /epick_moveit_studio/include/epick_moveit_studio/get_epick_object_detection_status.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | #include 33 | 34 | namespace epick_moveit_studio 35 | { 36 | /** 37 | * @brief Capture a epick_msgs::msg::ObjectDetectionStatus message. 38 | * @details The topic to monitor is set through the "topic_name" parameter, and the resulting message is available on 39 | * the "message_out" output port. 40 | * 41 | * @details 42 | * | Data Port Name | Port Type | Object Type | 43 | * | -------------- | --------- | -------------------------------------- | 44 | * | topic_name | input | std::string | 45 | * | message_out | output | epick_msgs::msg::ObjectDetectionStatus | 46 | */ 47 | class GetEpickObjectDetectionStatus final 48 | : public moveit_studio::behaviors::GetMessageFromTopicBehaviorBase 49 | { 50 | public: 51 | GetEpickObjectDetectionStatus(const std::string& name, const BT::NodeConfiguration& config, 52 | const std::shared_ptr& shared_resources); 53 | 54 | private: 55 | tl::expected, std::string> getWaitForMessageTimeout() override; 56 | 57 | /** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */ 58 | std::shared_future>& getFuture() override 59 | { 60 | return future_; 61 | } 62 | 63 | /** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */ 64 | std::shared_future> future_; 65 | }; 66 | } // namespace epick_moveit_studio 67 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/fake/fake_driver.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | 33 | namespace epick_driver 34 | { 35 | /** 36 | * This is a fake driver that can be used for testing interactions with the 37 | * hardware interface or the controller without being connected to the real 38 | * hardware. At the moment the fake driver is very basic but it can be 39 | * improved to behave as close as possible to the real hardware. 40 | * To use this driver you have to enable the following parameter in your 41 | * hardware interface configuration in the robot URDF. 42 | * 43 | * 44 | * true 45 | */ 46 | class FakeDriver : public Driver 47 | { 48 | public: 49 | void set_slave_address(uint8_t slave_address) override; 50 | void set_mode(GripperMode gripper_mode) override; 51 | void set_grip_max_vacuum_pressure(float vacuum_pressure) override; 52 | void set_grip_min_vacuum_pressure(float vacuum_pressure) override; 53 | void set_grip_timeout(std::chrono::milliseconds timeout) override; 54 | void set_release_timeout(std::chrono::milliseconds timeout) override; 55 | bool connect() override; 56 | void disconnect() override; 57 | void activate() override; 58 | void deactivate() override; 59 | void grip() override; 60 | void release() override; 61 | GripperStatus get_status() override; 62 | 63 | private: 64 | uint8_t slave_address_ = 0x00; 65 | GripperMode gripper_mode_ = GripperMode::Unknown; 66 | 67 | float grip_max_vacuum_pressure_ = 0.0f; 68 | float grip_min_vacuum_pressure_ = 0.0f; 69 | 70 | std::chrono::milliseconds grip_timeout_ = std::chrono::milliseconds(100); 71 | std::chrono::milliseconds release_timeout_ = std::chrono::milliseconds(100); 72 | 73 | bool connected_ = false; 74 | bool activated_ = false; 75 | bool regulate_ = false; 76 | }; 77 | 78 | } // namespace epick_driver 79 | -------------------------------------------------------------------------------- /epick_moveit_studio/test/test_load_behaviors.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | namespace 37 | { 38 | constexpr auto kBehaviorInterfacePackageName = "moveit_studio_behavior_interface"; 39 | constexpr auto kBehaviorBaseClassName = "moveit_studio::behaviors::SharedResourcesNodeLoaderBase"; 40 | constexpr auto kTestBehaviorName = "test_behavior_name"; 41 | } // namespace 42 | 43 | /** 44 | * @brief This test makes sure that the Behaviors provided in this package can be successfully registered and 45 | * instantiated by the behavior tree factory. 46 | */ 47 | TEST(EpickBehaviors, LoadBehaviorPlugins) 48 | { 49 | pluginlib::ClassLoader class_loader( 50 | kBehaviorInterfacePackageName, kBehaviorBaseClassName); 51 | 52 | auto node = std::make_shared("test_node"); 53 | auto shared_resources = std::make_shared(node); 54 | 55 | BT::BehaviorTreeFactory factory; 56 | { 57 | auto plugin_instance = class_loader.createUniqueInstance("epick_moveit_studio::BehaviorLoader"); 58 | ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources)); 59 | } 60 | 61 | // Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info. 62 | EXPECT_NO_THROW((void)factory.instantiateTreeNode(kTestBehaviorName, "CompareEpickObjectDetectionStatus", 63 | BT::NodeConfiguration())); 64 | EXPECT_NO_THROW( 65 | (void)factory.instantiateTreeNode(kTestBehaviorName, "GetEpickObjectDetectionStatus", BT::NodeConfiguration())); 66 | } 67 | 68 | int main(int argc, char** argv) 69 | { 70 | rclcpp::init(argc, argv); 71 | 72 | testing::InitGoogleTest(&argc, argv); 73 | return RUN_ALL_TESTS(); 74 | } 75 | -------------------------------------------------------------------------------- /epick_driver/src/data_utils.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | namespace epick_driver::data_utils 37 | { 38 | constexpr std::array vChars = { 39 | '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' 40 | }; 41 | 42 | std::string to_hex(const std::vector& bytes) 43 | { 44 | std::string hex; 45 | for (auto it = std::begin(bytes); it != std::end(bytes); ++it) 46 | { 47 | if (it != bytes.begin()) 48 | { 49 | hex += " "; 50 | } 51 | hex += ""; 52 | uint8_t ch = *it; 53 | hex += vChars[((ch >> 4) & 0xF)]; 54 | hex += vChars[(ch & 0xF)]; 55 | } 56 | 57 | return hex; 58 | } 59 | 60 | std::string to_hex(const std::vector& bytes) 61 | { 62 | std::string hex; 63 | for (auto it = std::begin(bytes); it != std::end(bytes); ++it) 64 | { 65 | if (it != bytes.begin()) 66 | { 67 | hex += " "; 68 | } 69 | hex += ""; 70 | uint16_t ch = *it; 71 | hex += vChars[((ch >> 12) & 0xF)]; 72 | hex += vChars[((ch >> 8) & 0xF)]; 73 | hex += vChars[((ch >> 4) & 0xF)]; 74 | hex += vChars[(ch & 0xF)]; 75 | } 76 | 77 | return hex; 78 | } 79 | 80 | std::string to_binary_string(const uint8_t byte) 81 | { 82 | std::string result = ""; 83 | for (int i = 7; i >= 0; --i) 84 | { 85 | result += ((byte >> i) & 1) ? '1' : '0'; 86 | } 87 | return result; 88 | } 89 | 90 | uint8_t get_msb(uint16_t value) 91 | { 92 | return static_cast(value >> 8) & 0xFF; 93 | } 94 | 95 | uint8_t get_lsb(uint16_t value) 96 | { 97 | return static_cast(value & 0xFF); 98 | } 99 | 100 | std::string to_lower(const std::string& str) 101 | { 102 | std::string result = str; 103 | std::transform(result.begin(), result.end(), result.begin(), [](unsigned char c) { return std::tolower(c); }); 104 | return result; 105 | } 106 | 107 | } // namespace epick_driver::data_utils 108 | -------------------------------------------------------------------------------- /docs/class_diagram.uxf: -------------------------------------------------------------------------------- 1 | 11Space for diagram notesUMLClass53948417644valign=center 2 | transparency=0 3 | bg=#FFFFFF 4 | <<interface>> 5 | SerialUMLClass28648417644valign=center 6 | transparency=0 7 | bg=#FFFFFF 8 | DefaultSerialRelation45149511033lt=<<-80;10;10;10UMLClass28637417644valign=center 9 | transparency=0 10 | bg=#FFFFFF 11 | DefaultDriverRelation45140711099lt=<-80;70;10;10UMLClass28626417644valign=center 12 | transparency=0 13 | bg=#FFFFFF 14 | EpicGripper 15 | HardwareInterfaceRelation45129711099lt=<-80;70;10;10UMLClass53937417644valign=center 16 | transparency=0 17 | bg=#FFFFFF 18 | <<interface>> 19 | DriverRelation45138511033lt=<<-80;10;10;10UMLClass79248417644valign=center 20 | transparency=0 21 | bg=#FFFFFF 22 | MockSerialRelation70449511033lt=<<-10;10;80;10UMLClass79237417644valign=center 23 | transparency=0 24 | bg=#FFFFFF 25 | MockDriverRelation70438511033lt=<<-10;10;80;10UMLClass53926417644valign=center 26 | transparency=0 27 | bg=#FFFFFF 28 | FakeDriverRelation6162977799lt=<<- 29 | {default}10;70;10;10UMLPackage781341198198Test 30 | -------------------------------------------------------------------------------- /epick_controllers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # See https://docs.ros.org/en/foxy/How-To-Guides/Ament-CMake-Documentation.html 2 | 3 | cmake_minimum_required(VERSION 3.8) 4 | project(epick_controllers VERSION 0.0.1 LANGUAGES CXX) 5 | 6 | # This module provides installation directories as per the GNU coding standards. 7 | include(GNUInstallDirs) 8 | 9 | set(CMAKE_EXPORT_COMPILE_COMMANDS true) 10 | 11 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 12 | add_compile_options(-Wall -Wextra -Wpedantic) 13 | endif() 14 | 15 | find_package(ament_cmake REQUIRED) 16 | find_package(controller_interface REQUIRED) 17 | find_package(epick_msgs REQUIRED) 18 | find_package(rclcpp REQUIRED) 19 | find_package(rclcpp_action REQUIRED) 20 | find_package(realtime_tools REQUIRED) 21 | find_package(std_srvs REQUIRED) 22 | 23 | set(THIS_PACKAGE_INCLUDE_DEPENDS 24 | controller_interface 25 | epick_msgs 26 | rclcpp 27 | rclcpp_action 28 | realtime_tools 29 | std_srvs 30 | ) 31 | 32 | # Epick controller library. 33 | 34 | add_library( 35 | epick_controllers 36 | SHARED 37 | include/epick_controllers/epick_controller.hpp 38 | include/epick_controllers/epick_gripper_action_controller.hpp 39 | include/epick_controllers/epick_status_publisher_controller.hpp 40 | src/epick_controller.cpp 41 | src/epick_gripper_action_controller.cpp 42 | src/epick_status_publisher_controller.cpp 43 | ) 44 | target_include_directories( 45 | epick_controllers 46 | PUBLIC 47 | $ 48 | $ 49 | ) 50 | ament_target_dependencies( 51 | epick_controllers 52 | ${THIS_PACKAGE_INCLUDE_DEPENDS} 53 | ) 54 | 55 | ############################################################################### 56 | # PLUGINS 57 | 58 | pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml) 59 | 60 | ############################################################################### 61 | # EXPORTS 62 | 63 | # This is necessary to allow this library’s clients to use the syntax 64 | # target_link_libraries(client ::) 65 | # target_link_libraries(client stepit_hardware::stepit_hardware) 66 | # Without this a client cannot find this library. 67 | # It can take an arbitrary list of targets named as EXPORT in an install call. 68 | ament_export_targets( 69 | epick_controllers_targets # Must match the EXPORT label below in the install section. 70 | ) 71 | # Help downstream packages to find transitive dependencies i.e. export all 72 | # dependencies required by a package to use this library. 73 | # When a package calls find_package(epick_driver), CMake looks for a file 74 | # called epick_driverConfig.cmake which sets up everything another project 75 | # would need to depend on this one. 76 | ament_export_dependencies( 77 | ${THIS_PACKAGE_INCLUDE_DEPENDS} 78 | ) 79 | # Tell downstream packages where to find our headers. 80 | ament_export_include_directories( 81 | include 82 | ) 83 | # Tell downstream packages our libraries to link against. 84 | ament_export_libraries( 85 | epick_controllers 86 | ) 87 | 88 | ############################################################################### 89 | # INSTALL 90 | 91 | # Install all files of the include folder into the give destination. 92 | install( 93 | DIRECTORY include/ 94 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 95 | ) 96 | 97 | # Install our library. 98 | install( 99 | TARGETS epick_controllers 100 | EXPORT epick_controllers_targets 101 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} # lib 102 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} # lib 103 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} # bin 104 | INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # include 105 | ) 106 | 107 | ament_package() 108 | -------------------------------------------------------------------------------- /epick_driver/src/default_serial_factory.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | #include 31 | 32 | #include 33 | 34 | namespace epick_driver 35 | { 36 | 37 | const auto kLogger = rclcpp::get_logger("DefaultSerialFactory"); 38 | 39 | constexpr auto kUsbPortParamName = "usb_port"; 40 | constexpr auto kUsbPortParamDefault = "/dev/ttyUSB0"; 41 | 42 | constexpr auto kBaudrateParamName = "baudrate"; 43 | constexpr auto kBaudrateAddressParamDefault = 115200; 44 | 45 | constexpr auto kTimeoutParamName = "timeout"; 46 | constexpr auto kTimeoutParamDefault = 0.5; 47 | 48 | std::unique_ptr DefaultSerialFactory::create(const hardware_interface::HardwareInfo& info) const 49 | { 50 | RCLCPP_INFO(kLogger, "Reading usb_port..."); 51 | std::string usb_port = info.hardware_parameters.count(kUsbPortParamName) ? 52 | info.hardware_parameters.at(kUsbPortParamName) : 53 | kUsbPortParamDefault; 54 | RCLCPP_INFO(kLogger, "usb_port: %s", usb_port.c_str()); 55 | 56 | RCLCPP_INFO(kLogger, "Reading baudrate..."); 57 | uint32_t baudrate = info.hardware_parameters.count(kBaudrateParamName) ? 58 | static_cast(std::stoul(info.hardware_parameters.at(kBaudrateParamName))) : 59 | kBaudrateAddressParamDefault; 60 | RCLCPP_INFO(kLogger, "baudrate: %dbps", baudrate); 61 | 62 | RCLCPP_INFO(kLogger, "Reading timeout..."); 63 | double timeout = info.hardware_parameters.count(kTimeoutParamName) ? 64 | std::stod(info.hardware_parameters.at(kTimeoutParamName)) : 65 | kTimeoutParamDefault; 66 | RCLCPP_INFO(kLogger, "timeout: %fs", timeout); 67 | 68 | auto serial = create_serial(); 69 | serial->set_port(usb_port); 70 | serial->set_baudrate(baudrate); 71 | serial->set_timeout(std::chrono::duration_cast(std::chrono::duration(timeout))); 72 | return serial; 73 | } 74 | 75 | std::unique_ptr DefaultSerialFactory::create_serial() const 76 | { 77 | return std::make_unique(); 78 | } 79 | 80 | } // namespace epick_driver 81 | -------------------------------------------------------------------------------- /epick_driver/src/default_serial.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | 31 | #include 32 | 33 | namespace epick_driver 34 | { 35 | 36 | DefaultSerial::DefaultSerial() : serial_{ std::make_unique() } 37 | { 38 | } 39 | 40 | void DefaultSerial::open() 41 | { 42 | serial_->open(); 43 | } 44 | 45 | bool DefaultSerial::is_open() const 46 | { 47 | return serial_->isOpen(); 48 | } 49 | 50 | void DefaultSerial::close() 51 | { 52 | serial_->close(); 53 | } 54 | 55 | std::vector DefaultSerial::read(size_t size) 56 | { 57 | std::vector data; 58 | size_t bytes_read = serial_->read(data, size); 59 | if (bytes_read != size) 60 | { 61 | const auto error_msg = "Requested " + std::to_string(size) + " bytes, but got " + std::to_string(bytes_read); 62 | THROW(serial::IOException, error_msg.c_str()); 63 | } 64 | return data; 65 | } 66 | 67 | void DefaultSerial::write(const std::vector& data) 68 | { 69 | std::size_t num_bytes_written = serial_->write(data); 70 | serial_->flush(); 71 | if (num_bytes_written != data.size()) 72 | { 73 | const auto error_msg = 74 | "Attempted to write " + std::to_string(data.size()) + " bytes, but wrote " + std::to_string(num_bytes_written); 75 | THROW(serial::IOException, error_msg.c_str()); 76 | } 77 | } 78 | 79 | void DefaultSerial::set_port(const std::string& port) 80 | { 81 | serial_->setPort(port); 82 | } 83 | 84 | std::string DefaultSerial::get_port() const 85 | { 86 | return serial_->getPort(); 87 | } 88 | 89 | void DefaultSerial::set_timeout(std::chrono::milliseconds timeout) 90 | { 91 | serial::Timeout simple_timeout = serial::Timeout::simpleTimeout(static_cast(timeout.count())); 92 | serial_->setTimeout(simple_timeout); 93 | } 94 | 95 | std::chrono::milliseconds DefaultSerial::get_timeout() const 96 | { 97 | uint32_t timeout = serial_->getTimeout().read_timeout_constant; 98 | return std::chrono::milliseconds{ timeout }; 99 | } 100 | 101 | void DefaultSerial::set_baudrate(uint32_t baudrate) 102 | { 103 | serial_->setBaudrate(baudrate); 104 | } 105 | 106 | uint32_t DefaultSerial::get_baudrate() const 107 | { 108 | return serial_->getBaudrate(); 109 | } 110 | 111 | } // namespace epick_driver 112 | -------------------------------------------------------------------------------- /epick_driver/tests/test_default_serial_factory.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | 37 | namespace epick_driver::test 38 | { 39 | // This factory will populate the injected mock with data read form the HardwareInfo. 40 | class StubSerialFactory : public DefaultSerialFactory 41 | { 42 | public: 43 | explicit StubSerialFactory(std::unique_ptr serial) : serial_{ std::move(serial) } 44 | { 45 | } 46 | 47 | protected: 48 | std::unique_ptr create_serial() const override 49 | { 50 | return std::move(serial_); 51 | } 52 | 53 | private: 54 | mutable std::unique_ptr serial_; 55 | }; 56 | 57 | /** 58 | * Here we test the serial factory with default parameters. 59 | */ 60 | TEST(TestDefaultSerialFactory, create_with_default_parameters) 61 | { 62 | hardware_interface::HardwareInfo info; 63 | 64 | auto serial = std::make_unique(); 65 | 66 | // This line is only required when running the test inside the IDE. 67 | testing::Mock::AllowLeak(serial.get()); 68 | 69 | EXPECT_CALL(*serial, set_port("/dev/ttyUSB0")); 70 | EXPECT_CALL(*serial, set_baudrate(115200)); 71 | EXPECT_CALL(*serial, set_timeout(std::chrono::milliseconds{ 500 })); 72 | 73 | StubSerialFactory serial_factory(std::move(serial)); 74 | auto created_serial = serial_factory.create(info); 75 | } 76 | 77 | /** 78 | * Here we test the serial factory with default parameters. 79 | */ 80 | TEST(TestDefaultSerialFactory, create_with_given_parameters) 81 | { 82 | hardware_interface::HardwareInfo info; 83 | info.hardware_parameters.emplace("usb_port", "/dev/ttyUSB1"); 84 | info.hardware_parameters.emplace("baudrate", "9600"); 85 | info.hardware_parameters.emplace("timeout", "0.1"); 86 | 87 | auto serial = std::make_unique(); 88 | 89 | // This line is only required when running the test inside the IDE. 90 | testing::Mock::AllowLeak(serial.get()); 91 | 92 | EXPECT_CALL(*serial, set_port("/dev/ttyUSB1")); 93 | EXPECT_CALL(*serial, set_baudrate(9600)); 94 | EXPECT_CALL(*serial, set_timeout(std::chrono::milliseconds(100))); 95 | 96 | StubSerialFactory serial_factory(std::move(serial)); 97 | auto created_serial = serial_factory.create(info); 98 | } 99 | } // namespace epick_driver::test 100 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/default_driver.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | namespace epick_driver 40 | { 41 | /** 42 | * This is the default implementation of the Driver to control the Epick Gripper. 43 | */ 44 | class DefaultDriver : public Driver 45 | { 46 | public: 47 | /** 48 | * Initialize the interface to control the Robotiq EPick gripper. 49 | * @param serial_interface The serial connection. 50 | * @param slave_address The slave address as specified in the MODBUS RTU protocol. 51 | */ 52 | explicit DefaultDriver(std::unique_ptr serial); 53 | 54 | bool connect() override; 55 | void disconnect() override; 56 | 57 | void set_slave_address(uint8_t slave_address) override; 58 | void set_mode(GripperMode gripper_mode) override; 59 | void set_grip_max_vacuum_pressure(float vacuum_pressure) override; 60 | void set_grip_min_vacuum_pressure(float vacuum_pressure) override; 61 | void set_grip_timeout(std::chrono::milliseconds grip_timeout) override; 62 | void set_release_timeout(std::chrono::milliseconds release_timeout) override; 63 | 64 | GripperStatus get_status() override; 65 | 66 | /** Activate the gripper with the specified operation mode and parameters. */ 67 | void activate() override; 68 | 69 | /** Deactivate the gripper. */ 70 | void deactivate() override; 71 | 72 | void grip() override; 73 | 74 | void release() override; 75 | 76 | private: 77 | /** 78 | * With this command we send a request and wait for a response of given size. 79 | * Behind the scene, if the response is not received, the software makes an attempt 80 | * to resend the command up to 5 times before returning an empty response. 81 | * @param request The command request. 82 | * @param response_size The response expected size. 83 | * @return The response or an empty vector if an en error occurred. 84 | */ 85 | std::vector send(const std::vector& request, size_t response_size) const; 86 | 87 | std::unique_ptr serial_ = nullptr; 88 | uint8_t slave_address_ = 0x00; 89 | GripperMode gripper_mode_ = GripperMode::AutomaticMode; 90 | 91 | float grip_max_vacuum_pressure_ = 0.0; 92 | float grip_min_vacuum_pressure_ = 0.0; 93 | 94 | std::chrono::milliseconds grip_timeout_ = std::chrono::milliseconds(0); 95 | std::chrono::milliseconds release_timeout_ = std::chrono::milliseconds(0); 96 | }; 97 | } // namespace epick_driver 98 | -------------------------------------------------------------------------------- /epick_moveit_studio/include/epick_moveit_studio/compare_epick_object_detection_status.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | 35 | namespace BT 36 | { 37 | /** 38 | * @brief Template specialization of convertFromString to parse a string as an epick_msgs::msg::ObjectDetectionStatus. 39 | * @details This allows setting one of the input ports of CompareEpickObjectDetectionStatus by typing a string. 40 | * @param str Input string to convert. 41 | * @return epick_msgs::msg::ObjectDetectionStatus 42 | */ 43 | template <> 44 | inline epick_msgs::msg::ObjectDetectionStatus 45 | convertFromString(BT::StringView str) 46 | { 47 | using Status = epick_msgs::msg::ObjectDetectionStatus; 48 | if (str == "OBJECT_DETECTED_AT_MIN_PRESSURE") 49 | { 50 | return epick_msgs::build().status(Status::OBJECT_DETECTED_AT_MIN_PRESSURE); 51 | } 52 | else if (str == "OBJECT_DETECTED_AT_MAX_PRESSURE") 53 | { 54 | return epick_msgs::build().status(Status::OBJECT_DETECTED_AT_MAX_PRESSURE); 55 | } 56 | else if (str == "NO_OBJECT_DETECTED") 57 | { 58 | return epick_msgs::build().status(Status::NO_OBJECT_DETECTED); 59 | } 60 | else if (str == "UNKNOWN") 61 | { 62 | return epick_msgs::build().status(Status::UNKNOWN); 63 | } 64 | else 65 | { 66 | throw BT::RuntimeError(std::string("To convert into a ObjectDetectionStatus message, the input string must be one " 67 | "of [OBJECT_DETECTED_AT_MIN_PRESSURE, OBJECT_DETECTED_AT_MAX_PRESSURE, " 68 | "NO_OBJECT_DETECTED, UNKNOWN]. Cannot parse the provided string: ") 69 | .append(str)); 70 | } 71 | } 72 | } // namespace BT 73 | 74 | namespace epick_moveit_studio 75 | { 76 | /** 77 | * @brief Behavior to compare ObjectDetectionStatus messages. 78 | * @details When ticked, gets two epick_msgs::msg::ObjectDetectionStatus messages from the "value1" and "value2" input 79 | * data ports. The behavior succeeds if they are identical and fails if they are different. 80 | * 81 | * | Data Port Name | Port Type | Object Type | 82 | * | -------------- | --------- | -------------------------------------- | 83 | * | value1 | input | epick_msgs::msg::ObjectDetectionStatus | 84 | * | value2 | input | epick_msgs::msg::ObjectDetectionStatus | 85 | */ 86 | class CompareEpickObjectDetectionStatus final : public BT::SyncActionNode 87 | { 88 | public: 89 | CompareEpickObjectDetectionStatus(const std::string& name, const BT::NodeConfiguration& config); 90 | 91 | static BT::PortsList providedPorts(); 92 | 93 | BT::NodeStatus tick() override; 94 | }; 95 | } // namespace epick_moveit_studio 96 | -------------------------------------------------------------------------------- /epick_description/urdf/epick_single_suction_cup.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 28 | 40 | 41 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 63 | 64 | 65 | 66 | 72 | 73 | 74 | 75 | 76 | 79 | 80 | 81 | 82 | 83 | 84 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/serial.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | namespace epick_driver 37 | { 38 | /** 39 | * The driver talks to the hardware through an implementation of the 40 | * serial interface. 41 | * We can mock the serial connection to check that a high level command 42 | * (e.g activate) is converted into the correct sequence of bytes 43 | * including CRC. 44 | */ 45 | class Serial 46 | { 47 | public: 48 | virtual ~Serial() = default; 49 | 50 | /** 51 | * Opens the serial port as long as the port is set and the port isn't 52 | * already open. 53 | */ 54 | virtual void open() = 0; 55 | 56 | /** 57 | * Gets the open status of the serial port. 58 | * @return Returns true if the port is open, false otherwise. 59 | */ 60 | [[nodiscard]] virtual bool is_open() const = 0; 61 | 62 | /** Closes the serial port. */ 63 | virtual void close() = 0; 64 | 65 | /** 66 | * Read a given amount of bytes from the serial port. 67 | * @param size The number of bytes to read. 68 | * @return The sequence of bytes. 69 | * @throw serial::PortNotOpenedException 70 | * @throw serial::SerialException 71 | */ 72 | [[nodiscard]] virtual std::vector read(size_t size) = 0; 73 | 74 | /** 75 | * Write a sequence of bytes to the serial port. 76 | * @param data A vector containing data to be written to the serial port. 77 | * @throw serial::PortNotOpenedException 78 | * @throw serial::SerialException 79 | * @throw serial::IOException 80 | */ 81 | virtual void write(const std::vector& data) = 0; 82 | 83 | /** 84 | * Sets the serial port identifier. 85 | * @param port A const std::string reference containing the address of the 86 | * serial port, which would be something like "COM1" on Windows and 87 | * "/dev/ttyS0" on Linux. 88 | * @throw std::invalid_argument 89 | */ 90 | virtual void set_port(const std::string& port) = 0; 91 | 92 | /** 93 | * Gets the serial port identifier. 94 | * @see SerialInterface::setPort 95 | * @throw std::invalid_argument 96 | */ 97 | [[nodiscard]] virtual std::string get_port() const = 0; 98 | 99 | /** 100 | * Set read timeout in milliseconds. 101 | * @param timeout Read timeout in milliseconds. 102 | */ 103 | virtual void set_timeout(const std::chrono::milliseconds timeout) = 0; 104 | 105 | /** 106 | * Get read timeout in milliseconds. 107 | * @return Read timeout in milliseconds. 108 | */ 109 | [[nodiscard]] virtual std::chrono::milliseconds get_timeout() const = 0; 110 | 111 | /** 112 | * Sets the baudrate for the serial port. 113 | * @param baudrate An integer that sets the baud rate for the serial port. 114 | */ 115 | virtual void set_baudrate(uint32_t baudrate) = 0; 116 | 117 | /** 118 | * Gets the baudrate for the serial port. 119 | * @return An integer that sets the baud rate for the serial port. 120 | */ 121 | [[nodiscard]] virtual uint32_t get_baudrate() const = 0; 122 | }; 123 | } // namespace epick_driver 124 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/hardware_interface_utils.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | namespace epick_driver::hardware_interface_utils 37 | { 38 | /** 39 | * Look for a GPIO command interface in the hardware info. 40 | * @param gpio_name The component name. 41 | * @param interface_name The interface name. 42 | * @return the interface info of the requested interface, if any. 43 | */ 44 | std::optional 45 | get_gpios_command_interface(std::string gpio_name, std::string interface_name, 46 | const hardware_interface::HardwareInfo& info); 47 | 48 | /** 49 | * Look for a GPIO state interface in the hardware info. 50 | * @param gpio_name The component name. 51 | * @param interface_name The interface name. 52 | * @return the interface info of the requested interface, if any. 53 | */ 54 | std::optional 55 | get_gpios_state_interface(std::string gpio_name, std::string interface_name, 56 | const hardware_interface::HardwareInfo& info); 57 | 58 | /** 59 | * Look for a joint command interface in the hardware info. 60 | * @param joint_name The component name. 61 | * @param interface_name The interface name. 62 | * @return the interface info of the requested interface, if any. 63 | */ 64 | std::optional 65 | get_joints_command_interface(std::string joint_name, std::string interface_name, 66 | const hardware_interface::HardwareInfo& info); 67 | 68 | /** 69 | * Look for a joint state interface in the hardware info. 70 | * @param joint_name The component name. 71 | * @param interface_name The interface name. 72 | * @return the interface info of the requested interface, if any. 73 | */ 74 | std::optional 75 | get_joints_state_interface(std::string joint_name, std::string interface_name, 76 | const hardware_interface::HardwareInfo& info); 77 | 78 | /** 79 | * All command and state interfaces work with double values. We can 80 | * use double value to represent boolean values: 81 | * 0.0 = true 82 | * 1.0 = false 83 | * To avoid directly comparing double, to determine if a double represents 84 | * a false or a true, we use a simple formula: 85 | * bool is_true = value >= 0.5 86 | * @param value The value to be converted into a boolean. 87 | * @return The boolean linked to the given double value. 88 | */ 89 | bool is_true(double value); 90 | 91 | /** 92 | * All command and state interfaces work with double values. We can 93 | * use double value to represent boolean values: 94 | * 0.0 = true 95 | * 1.0 = false 96 | * To avoid directly comparing double, to determine if a double represents 97 | * a false or a true, we use a simple formula: 98 | * bool is_true = value >= 0.5 99 | * @param value The value to be converted into a boolean. 100 | * @return The boolean linked to the given double value. 101 | */ 102 | bool is_false(double value); 103 | } // namespace epick_driver::hardware_interface_utils 104 | -------------------------------------------------------------------------------- /epick_controllers/include/epick_controllers/epick_gripper_action_controller.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | namespace epick_controllers 44 | { 45 | class EpickGripperActionController : public controller_interface::ControllerInterface 46 | { 47 | public: 48 | using GripperCommandAction = control_msgs::action::GripperCommand; 49 | using GoalHandle = rclcpp_action::ServerGoalHandle; 50 | using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; 51 | using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer>; 52 | 53 | struct Commands 54 | { 55 | /** 56 | * @brief Represents the binary flag send to the gripper hardware interface to actuate the vacuum gripper. 57 | * @details If grip_cmd == 0.0, the gripper releases any held object. If grip_cmd == 1.0, the gripper activates its 58 | * vacuum pump and attempts to grasp an object. 59 | */ 60 | double grip_cmd; 61 | }; 62 | 63 | controller_interface::InterfaceConfiguration command_interface_configuration() const override; 64 | 65 | controller_interface::InterfaceConfiguration state_interface_configuration() const override; 66 | 67 | controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; 68 | 69 | CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; 70 | 71 | CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; 72 | 73 | CallbackReturn on_init() override; 74 | 75 | private: 76 | rclcpp_action::GoalResponse goal_callback(const rclcpp_action::GoalUUID& uuid, 77 | std::shared_ptr goal); 78 | 79 | rclcpp_action::CancelResponse cancel_callback(const std::shared_ptr goal_handle); 80 | 81 | void accepted_callback(std::shared_ptr goal_handle); 82 | 83 | void preempt_active_goal(); 84 | 85 | void set_hold_position(); 86 | 87 | void check_for_success(const double current_regulate_state, const double current_regulate_command); 88 | 89 | std::optional> regulate_command_interface_; 90 | 91 | std::optional> is_regulating_state_interface_; 92 | 93 | std::shared_ptr> action_server_; 94 | 95 | std::shared_ptr goal_handle_timer_; 96 | 97 | RealtimeGoalHandleBuffer rt_active_goal_; 98 | 99 | GripperCommandAction::Result::SharedPtr pre_alloc_result_; 100 | 101 | realtime_tools::RealtimeBuffer commands_buffer_; 102 | 103 | Commands commands_; 104 | Commands commands_rt_; 105 | }; 106 | } // namespace epick_controllers 107 | -------------------------------------------------------------------------------- /epick_driver/src/fake/fake_driver.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | #include 31 | 32 | #include 33 | 34 | namespace epick_driver 35 | { 36 | const auto kLogger = rclcpp::get_logger("FakeDriver"); 37 | 38 | void FakeDriver::set_slave_address(uint8_t slave_address) 39 | { 40 | slave_address_ = slave_address; 41 | RCLCPP_INFO(kLogger, "slave_address set to: %d", slave_address); 42 | } 43 | 44 | void FakeDriver::set_mode(const GripperMode gripper_mode) 45 | { 46 | gripper_mode_ = gripper_mode; 47 | RCLCPP_INFO(kLogger, "mode set to: %s", default_driver_utils::gripper_mode_to_string(gripper_mode).c_str()); 48 | } 49 | 50 | void FakeDriver::set_grip_max_vacuum_pressure(float vacuum_pressure) 51 | { 52 | grip_max_vacuum_pressure_ = vacuum_pressure; 53 | RCLCPP_INFO(kLogger, "grip max vacuum pressure set to: %fkPa", vacuum_pressure); 54 | } 55 | 56 | void FakeDriver::set_grip_min_vacuum_pressure(float vacuum_pressure) 57 | { 58 | grip_min_vacuum_pressure_ = vacuum_pressure; 59 | RCLCPP_INFO(kLogger, "grip min vacuum pressure set to: %fkPa", vacuum_pressure); 60 | } 61 | 62 | void FakeDriver::set_grip_timeout(std::chrono::milliseconds timeout) 63 | { 64 | grip_timeout_ = timeout; 65 | RCLCPP_INFO(kLogger, "grip timeout set to: %ldms", timeout.count()); 66 | } 67 | 68 | void FakeDriver::set_release_timeout(std::chrono::milliseconds timeout) 69 | { 70 | release_timeout_ = timeout; 71 | RCLCPP_INFO(kLogger, "release timeout set to: %ldms", timeout.count()); 72 | } 73 | 74 | bool FakeDriver::connect() 75 | { 76 | connected_ = true; 77 | RCLCPP_INFO(kLogger, "Gripper connected."); 78 | return true; 79 | } 80 | 81 | void FakeDriver::disconnect() 82 | { 83 | RCLCPP_INFO(kLogger, "Gripper disconnected."); 84 | connected_ = false; 85 | } 86 | 87 | void FakeDriver::activate() 88 | { 89 | RCLCPP_INFO(kLogger, "Gripper activated."); 90 | activated_ = true; 91 | } 92 | 93 | void FakeDriver::deactivate() 94 | { 95 | RCLCPP_INFO(kLogger, "Gripper deactivated."); 96 | activated_ = false; 97 | } 98 | 99 | void FakeDriver::grip() 100 | { 101 | regulate_ = true; 102 | RCLCPP_INFO(kLogger, "Grip enable."); 103 | } 104 | 105 | void FakeDriver::release() 106 | { 107 | regulate_ = false; 108 | RCLCPP_INFO(kLogger, "Grip released."); 109 | } 110 | 111 | GripperStatus FakeDriver::get_status() 112 | { 113 | GripperStatus status; 114 | status.gripper_activation_action = 115 | activated_ ? GripperActivationAction::Activate : GripperActivationAction::ClearGripperFaultStatus; 116 | status.gripper_mode = gripper_mode_; 117 | status.gripper_regulate_action = 118 | regulate_ ? GripperRegulateAction::FollowRequestedVacuumParameters : GripperRegulateAction::StopVacuumGenerator; 119 | status.gripper_activation_status = 120 | activated_ ? GripperActivationStatus::GripperOperational : GripperActivationStatus::GripperNotActivated; 121 | status.gripper_fault_status = GripperFaultStatus::NoFault; 122 | status.actuator_status = regulate_ ? ActuatorStatus::Gripping : ActuatorStatus::PassiveReleasing; 123 | status.object_detection_status = 124 | regulate_ ? ObjectDetectionStatus::ObjectDetectedAtMaxPressure : ObjectDetectionStatus::NoObjectDetected; 125 | status.max_vacuum_pressure = grip_max_vacuum_pressure_; 126 | status.actual_vacuum_pressure = (grip_max_vacuum_pressure_ + grip_min_vacuum_pressure_) / 2; 127 | return status; 128 | } 129 | } // namespace epick_driver 130 | -------------------------------------------------------------------------------- /epick_driver/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # See https://docs.ros.org/en/foxy/How-To-Guides/Ament-CMake-Documentation.html 2 | 3 | cmake_minimum_required(VERSION 3.8) 4 | project(epick_driver VERSION 0.0.1 LANGUAGES CXX) 5 | 6 | # This module provides installation directories as per the GNU coding standards. 7 | include(GNUInstallDirs) 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic) 11 | endif() 12 | 13 | find_package(ament_cmake REQUIRED) 14 | find_package(hardware_interface REQUIRED) 15 | find_package(pluginlib REQUIRED) 16 | find_package(rclcpp REQUIRED) 17 | find_package(rclcpp_lifecycle REQUIRED) 18 | find_package(serial REQUIRED) 19 | 20 | set(THIS_PACKAGE_INCLUDE_DEPENDS 21 | ament_cmake 22 | hardware_interface 23 | pluginlib 24 | rclcpp 25 | rclcpp_lifecycle 26 | serial 27 | ) 28 | 29 | # Epick driver library. 30 | 31 | add_library( 32 | epick_driver 33 | SHARED 34 | include/epick_driver/fake/fake_driver.hpp 35 | include/epick_driver/crc_utils.hpp 36 | include/epick_driver/data_utils.hpp 37 | include/epick_driver/default_driver.hpp 38 | include/epick_driver/default_driver_factory.hpp 39 | include/epick_driver/default_driver_utils.hpp 40 | include/epick_driver/default_serial.hpp 41 | include/epick_driver/default_serial_factory.hpp 42 | include/epick_driver/driver.hpp 43 | include/epick_driver/driver_exception.hpp 44 | include/epick_driver/driver_factory.hpp 45 | include/epick_driver/epick_gripper_hardware_interface.hpp 46 | include/epick_driver/hardware_interface_utils.hpp 47 | include/epick_driver/serial.hpp 48 | include/epick_driver/serial_factory.hpp 49 | include/epick_driver/visibility_control.hpp 50 | src/epick_gripper_hardware_interface.cpp 51 | src/crc_utils.cpp 52 | src/data_utils.cpp 53 | src/default_driver.cpp 54 | src/default_driver_factory.cpp 55 | src/default_driver_utils.cpp 56 | src/fake/fake_driver.cpp 57 | src/default_serial.cpp 58 | src/default_serial_factory.cpp 59 | src/hardware_interface_utils.cpp 60 | ) 61 | target_link_libraries(epick_driver atomic) 62 | target_include_directories( 63 | epick_driver 64 | PUBLIC 65 | $ 66 | $ 67 | ) 68 | ament_target_dependencies( 69 | epick_driver 70 | ${THIS_PACKAGE_INCLUDE_DEPENDS} 71 | ) 72 | 73 | ############################################################################### 74 | # PLUGINS 75 | 76 | pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml) 77 | 78 | ############################################################################### 79 | # EXPORTS 80 | 81 | # This is necessary to allow this library’s clients to use the syntax 82 | # target_link_libraries(client ::) 83 | # target_link_libraries(client stepit_hardware::stepit_hardware) 84 | # Without this a client cannot find this library. 85 | # It can take an arbitrary list of targets named as EXPORT in an install call. 86 | ament_export_targets( 87 | epick_driver_targets # Must match the EXPORT label below in the install section. 88 | ) 89 | # Help downstream packages to find transitive dependencies i.e. export all 90 | # dependencies required by a package to use this library. 91 | # When a package calls find_package(epick_driver), CMake looks for a file 92 | # called epick_driverConfig.cmake which sets up everything another project 93 | # would need to depend on this one. 94 | ament_export_dependencies( 95 | ${THIS_PACKAGE_INCLUDE_DEPENDS} 96 | ) 97 | # Tell downstream packages where to find our headers. 98 | ament_export_include_directories( 99 | include 100 | ) 101 | # Tell downstream packages our libraries to link against. 102 | ament_export_libraries( 103 | epick_driver 104 | ) 105 | 106 | ############################################################################### 107 | # INSTALL 108 | 109 | # Install all files of the include folder into the give destination. 110 | install( 111 | DIRECTORY include/ 112 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # include 113 | ) 114 | 115 | # Install our library. 116 | install( 117 | TARGETS epick_driver 118 | EXPORT epick_driver_targets 119 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} # lib 120 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} # lib 121 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} # bin 122 | INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # include 123 | ) 124 | 125 | ############################################################################### 126 | # TESTS 127 | 128 | # CTest module automatically creates a BUILD_TESTING option that selects 129 | # whether to enable testing support (ON by default). 130 | include(CTest) 131 | if(BUILD_TESTING) 132 | add_subdirectory(tests) 133 | endif() 134 | 135 | ############################################################################### 136 | # LINTERS 137 | 138 | add_custom_target(format 139 | COMMAND clang-format -i `git ls-files *.hpp *.cpp` 140 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) 141 | add_custom_target(tidy 142 | COMMAND clang-tidy -p ${CMAKE_BINARY_DIR} `git ls-files *.cpp` 143 | WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) 144 | 145 | ament_package() 146 | -------------------------------------------------------------------------------- /epick_driver/tests/test_default_driver_factory.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | 31 | #include 32 | 33 | #include 34 | 35 | #include 36 | 37 | namespace epick_driver::test 38 | { 39 | // This factory will populate the injected mock with data read form the HardwareInfo. 40 | class TestDriverFactory : public DefaultDriverFactory 41 | { 42 | public: 43 | explicit TestDriverFactory(std::unique_ptr driver) : driver_{ std::move(driver) } 44 | { 45 | } 46 | 47 | protected: 48 | std::unique_ptr create_driver([[maybe_unused]] const hardware_interface::HardwareInfo& info) const override 49 | { 50 | return std::move(driver_); 51 | } 52 | 53 | private: 54 | mutable std::unique_ptr driver_; 55 | }; 56 | 57 | /** 58 | * Here we test the driver factory with default parameters. 59 | */ 60 | TEST(TestDefaultDriverFactory, create_with_default_parameters) 61 | { 62 | hardware_interface::HardwareInfo info; 63 | 64 | auto driver = std::make_unique(); 65 | 66 | // This line is only required when running the test inside the IDE. 67 | testing::Mock::AllowLeak(driver.get()); 68 | 69 | EXPECT_CALL(*driver, set_slave_address(0x9)); 70 | EXPECT_CALL(*driver, set_mode(GripperMode::AutomaticMode)); 71 | EXPECT_CALL(*driver, set_grip_max_vacuum_pressure(-100)); 72 | EXPECT_CALL(*driver, set_grip_min_vacuum_pressure(-10)); 73 | EXPECT_CALL(*driver, set_grip_timeout(std::chrono::milliseconds(500))); 74 | EXPECT_CALL(*driver, set_release_timeout(std::chrono::milliseconds(500))); 75 | EXPECT_CALL(*driver, connect()).Times(0); 76 | EXPECT_CALL(*driver, disconnect()).Times(0); 77 | EXPECT_CALL(*driver, activate()).Times(0); 78 | EXPECT_CALL(*driver, deactivate()).Times(0); 79 | EXPECT_CALL(*driver, grip()).Times(0); 80 | EXPECT_CALL(*driver, release()).Times(0); 81 | EXPECT_CALL(*driver, get_status()).Times(0); 82 | 83 | TestDriverFactory driver_factory{ std::move(driver) }; 84 | auto created_driver = driver_factory.create(info); 85 | } 86 | 87 | /** 88 | * Here we test the driver factory with given parameters. 89 | */ 90 | TEST(TestDefaultDriverFactory, create_with_given_parameters) 91 | { 92 | hardware_interface::HardwareInfo info; 93 | 94 | info.hardware_parameters.emplace("slave_address", "1"); 95 | info.hardware_parameters.emplace("mode", "AdvancedMode"); 96 | info.hardware_parameters.emplace("grip_max_vacuum_pressure", "-50"); 97 | info.hardware_parameters.emplace("grip_min_vacuum_pressure", "-10"); 98 | info.hardware_parameters.emplace("grip_timeout", "1.0"); 99 | info.hardware_parameters.emplace("release_timeout", "0.2"); 100 | 101 | auto driver = std::make_unique(); 102 | 103 | // This line is only required when running the test inside the IDE. 104 | testing::Mock::AllowLeak(driver.get()); 105 | 106 | EXPECT_CALL(*driver, set_slave_address(0x1)); 107 | EXPECT_CALL(*driver, set_mode(GripperMode::AdvancedMode)); 108 | EXPECT_CALL(*driver, set_grip_max_vacuum_pressure(-50)); 109 | EXPECT_CALL(*driver, set_grip_min_vacuum_pressure(-10)); 110 | EXPECT_CALL(*driver, set_grip_timeout(std::chrono::milliseconds(1000))); 111 | EXPECT_CALL(*driver, set_release_timeout(std::chrono::milliseconds(200))); 112 | EXPECT_CALL(*driver, connect()).Times(0); 113 | EXPECT_CALL(*driver, disconnect()).Times(0); 114 | EXPECT_CALL(*driver, activate()).Times(0); 115 | EXPECT_CALL(*driver, deactivate()).Times(0); 116 | EXPECT_CALL(*driver, grip()).Times(0); 117 | EXPECT_CALL(*driver, release()).Times(0); 118 | EXPECT_CALL(*driver, get_status()).Times(0); 119 | 120 | TestDriverFactory driver_factory{ std::move(driver) }; 121 | auto created_driver = driver_factory.create(info); 122 | } 123 | } // namespace epick_driver::test 124 | -------------------------------------------------------------------------------- /epick_controllers/src/epick_status_publisher_controller.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | 31 | namespace epick_controllers 32 | { 33 | namespace state 34 | { 35 | enum StateInterfaces : size_t 36 | { 37 | OBJECT_DETECTION_STATUS = 0 38 | }; 39 | } 40 | 41 | constexpr auto kObjectDetectionStateInterface = "gripper/object_detection_status"; 42 | 43 | constexpr auto kObjectDetectionStatusTopic = "/object_detection_status"; 44 | 45 | controller_interface::InterfaceConfiguration EpickStatusPublisherController::command_interface_configuration() const 46 | { 47 | controller_interface::InterfaceConfiguration config; 48 | config.type = controller_interface::interface_configuration_type::NONE; 49 | return config; 50 | } 51 | 52 | controller_interface::InterfaceConfiguration EpickStatusPublisherController::state_interface_configuration() const 53 | { 54 | controller_interface::InterfaceConfiguration config; 55 | config.type = controller_interface::interface_configuration_type::INDIVIDUAL; 56 | config.names.emplace_back(kObjectDetectionStateInterface); 57 | return config; 58 | } 59 | 60 | controller_interface::return_type EpickStatusPublisherController::update([[maybe_unused]] const rclcpp::Time& time, 61 | [[maybe_unused]] const rclcpp::Duration& period) 62 | { 63 | double object_detection_status = state_interfaces_[state::OBJECT_DETECTION_STATUS].get_value(); 64 | auto object_detection_status_msg = std::make_shared(); 65 | 66 | if (object_detection_status < 0.5) 67 | { 68 | object_detection_status_msg->status = object_detection_status_msg->UNKNOWN; 69 | } 70 | else if (object_detection_status < 1.5) 71 | { 72 | object_detection_status_msg->status = object_detection_status_msg->OBJECT_DETECTED_AT_MIN_PRESSURE; 73 | } 74 | else if (object_detection_status < 2.5) 75 | { 76 | object_detection_status_msg->status = object_detection_status_msg->OBJECT_DETECTED_AT_MAX_PRESSURE; 77 | } 78 | else if (object_detection_status < 3.5) 79 | { 80 | object_detection_status_msg->status = object_detection_status_msg->NO_OBJECT_DETECTED; 81 | } 82 | else 83 | { 84 | object_detection_status_msg->status = object_detection_status_msg->UNKNOWN; 85 | } 86 | 87 | object_detection_status_pub_->publish(*object_detection_status_msg); 88 | 89 | return controller_interface::return_type::OK; 90 | } 91 | 92 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 93 | EpickStatusPublisherController::on_activate([[maybe_unused]] const rclcpp_lifecycle::State& previous_state) 94 | { 95 | try 96 | { 97 | object_detection_status_pub_ = 98 | get_node()->create_publisher(kObjectDetectionStatusTopic, 10); 99 | } 100 | catch (...) 101 | { 102 | return LifecycleNodeInterface::CallbackReturn::ERROR; 103 | } 104 | return LifecycleNodeInterface::CallbackReturn::SUCCESS; 105 | } 106 | 107 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn 108 | EpickStatusPublisherController::on_deactivate([[maybe_unused]] const rclcpp_lifecycle::State& previous_state) 109 | { 110 | try 111 | { 112 | object_detection_status_pub_.reset(); 113 | } 114 | catch (...) 115 | { 116 | return LifecycleNodeInterface::CallbackReturn::ERROR; 117 | } 118 | return LifecycleNodeInterface::CallbackReturn::SUCCESS; 119 | } 120 | 121 | rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn EpickStatusPublisherController::on_init() 122 | { 123 | return LifecycleNodeInterface::CallbackReturn::SUCCESS; 124 | } 125 | 126 | } // namespace epick_controllers 127 | 128 | #include "pluginlib/class_list_macros.hpp" 129 | 130 | PLUGINLIB_EXPORT_CLASS(epick_controllers::EpickStatusPublisherController, controller_interface::ControllerInterface) 131 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ROS2 Robotiq EPick Gripper 2 | 3 | [![CI](https://github.com/PickNikRobotics/ros2_epick_gripper/actions/workflows/industrial_ci.yml/badge.svg)](https://github.com/PickNikRobotics/ros2_epick_gripper/actions/workflows/industrial_ci.yml) 4 | [![Format](https://github.com/PickNikRobotics/ros2_epick_gripper/actions/workflows/ci-format.yml/badge.svg)](https://github.com/PickNikRobotics/ros2_epick_gripper/actions/workflows/ci-format.yml) 5 | [![Linters](https://github.com/PickNikRobotics/ros2_epick_gripper/actions/workflows/ci-ros-lint.yml/badge.svg)](https://github.com/PickNikRobotics/ros2_epick_gripper/actions/workflows/ci-ros-lint.yml) 6 | 7 | This repository contains the ROS 2 driver, controller, and description packages for working with a Robotiq EPick Gripper. 8 | 9 | ## Hardware Interface 10 | 11 | The `epick_driver` package serves as a ROS 2 Hardware Interface for the Robotiq EPick Gripper, enabling direct interaction with the gripper hardware. 12 | 13 | Below is a sample configuration that outlines various parameters for this interface. 14 | 15 | ``` 16 | 17 | 18 | 19 | 20 | 21 | 22 | epick_driver/EpickGripperHardwareInterface 23 | 24 | 25 | 26 | /dev/ttyUSB0 27 | 115200 28 | 0.2 29 | 30 | 31 | 32 | 33 | false 34 | 35 | 36 | 0x9 37 | 38 | 39 | AdvancedMode 40 | 41 | 42 | -60 43 | -10 44 | 25.0 45 | 2.0 46 | 47 | 48 | 49 | 50 | 55 | 56 | 57 | 62 | 63 | 64 | 71 | 72 | 73 | 74 | 78 | 79 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | ``` 93 | 94 | The hardware interface exposes a command interface labeled `grip_cmd` for gripping and releasing operations. Assigning the value of 1.0 to this command interface initiates the gripping action, while a value of 0.0 triggers the release mechanism. 95 | 96 | A parallel state interface named `grip_cmd` reflects the status of the performed command. 97 | 98 | The interface also offers an `object_detection_status` state interface to provide real-time object detection feedback during the grip operation. 99 | 100 | ## Controller 101 | 102 | The `epick_controllers` package establishes a ROS 2 Controller that communicates with the hardware interface through ROS 2 messages. 103 | 104 | The controller exposes a ROS 2 service `/grip_cmd` that accepts a boolean request to control the gripper. A true request initiates gripping, whereas a false request triggers release. 105 | 106 | For monitoring purposes, the controller also provides a `/object_detection_status` topic that returns one of the following object detection states: 107 | 108 | - UNKNOWN; 109 | - OBJECT_DETECTED_AT_MIN_PRESSURE; 110 | - OBJECT_DETECTED_AT_MAX_PRESSURE; 111 | - NO_OBJECT_DETECTED. 112 | 113 | No specialized configuration is needed for the controller. Here is a sample `controllers.yaml` file for reference: 114 | 115 | ``` 116 | controller_manager: 117 | ros__parameters: 118 | update_rate: 30 # Hz 119 | 120 | epick_controller: 121 | type: epick_controllers/EpickController 122 | ``` 123 | 124 | ## Test commands 125 | 126 | The `epick_hardware_tests` package includes a set of terminal-based commands designed for hardware validation. The available commands are as follows: 127 | 128 | - `activate`: Establishes a connection to the gripper and activates it. 129 | - `deactivate`: Connects to the gripper and deactivates it. 130 | - `get_status`: Obtains the current status of the gripper after activation. 131 | - `grip`: Initiates the gripping action. 132 | - `release`: Executes the release operation. 133 | -------------------------------------------------------------------------------- /epick_driver/src/hardware_interface_utils.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | 31 | #include 32 | 33 | namespace epick_driver::hardware_interface_utils 34 | { 35 | /** 36 | * Look for a GPIO interface in the hardware info. 37 | * @param gpio_name The component name. 38 | * @param interface_name The interface name. 39 | * @return the interface info of the requested interface, if any. 40 | */ 41 | std::optional 42 | get_gpios_command_interface(std::string gpio_name, std::string interface_name, 43 | const hardware_interface::HardwareInfo& info) 44 | { 45 | auto gpio_component = std::find_if(info.gpios.begin(), info.gpios.end(), 46 | [&gpio_name](const auto& gpio) { return gpio.name == gpio_name; }); 47 | 48 | if (gpio_component != info.gpios.end()) 49 | { 50 | auto hardware_info = 51 | std::find_if(gpio_component->command_interfaces.begin(), gpio_component->command_interfaces.end(), 52 | [&interface_name](const auto& cmd) { return cmd.name == interface_name; }); 53 | 54 | if (hardware_info != gpio_component->command_interfaces.end()) 55 | { 56 | return *hardware_info; 57 | } 58 | } 59 | return std::nullopt; 60 | } 61 | 62 | std::optional get_gpios_state_interface(std::string gpio_name, 63 | std::string interface_name, 64 | const hardware_interface::HardwareInfo& info) 65 | { 66 | auto gpio_component = std::find_if(info.gpios.begin(), info.gpios.end(), 67 | [&gpio_name](const auto& gpio) { return gpio.name == gpio_name; }); 68 | 69 | if (gpio_component != info.gpios.end()) 70 | { 71 | auto hardware_info = std::find_if(gpio_component->state_interfaces.begin(), gpio_component->state_interfaces.end(), 72 | [&interface_name](const auto& cmd) { return cmd.name == interface_name; }); 73 | 74 | if (hardware_info != gpio_component->state_interfaces.end()) 75 | { 76 | return *hardware_info; 77 | } 78 | } 79 | return std::nullopt; 80 | } 81 | 82 | bool is_true(double value) 83 | { 84 | return value >= 0.5; 85 | } 86 | 87 | bool is_false(double value) 88 | { 89 | return value < 0.5; 90 | } 91 | 92 | std::optional 93 | get_joints_command_interface(std::string joint_name, std::string interface_name, 94 | const hardware_interface::HardwareInfo& info) 95 | { 96 | auto joint_component = std::find_if(info.joints.begin(), info.joints.end(), 97 | [&joint_name](const auto& gpio) { return gpio.name == joint_name; }); 98 | 99 | if (joint_component != info.joints.end()) 100 | { 101 | auto hardware_info = 102 | std::find_if(joint_component->command_interfaces.begin(), joint_component->command_interfaces.end(), 103 | [&interface_name](const auto& cmd) { return cmd.name == interface_name; }); 104 | 105 | if (hardware_info != joint_component->command_interfaces.end()) 106 | { 107 | return *hardware_info; 108 | } 109 | } 110 | return std::nullopt; 111 | } 112 | 113 | std::optional 114 | get_joints_state_interface(std::string joint_name, std::string interface_name, 115 | const hardware_interface::HardwareInfo& info) 116 | { 117 | auto joint_component = std::find_if(info.joints.begin(), info.joints.end(), 118 | [&joint_name](const auto& gpio) { return gpio.name == joint_name; }); 119 | 120 | if (joint_component != info.joints.end()) 121 | { 122 | auto hardware_info = 123 | std::find_if(joint_component->state_interfaces.begin(), joint_component->state_interfaces.end(), 124 | [&interface_name](const auto& cmd) { return cmd.name == interface_name; }); 125 | 126 | if (hardware_info != joint_component->state_interfaces.end()) 127 | { 128 | return *hardware_info; 129 | } 130 | } 131 | return std::nullopt; 132 | } 133 | 134 | } // namespace epick_driver::hardware_interface_utils 135 | -------------------------------------------------------------------------------- /epick_driver/src/crc_utils.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | 31 | #include 32 | 33 | namespace epick_driver::crc_utils 34 | { 35 | /** 36 | * Following are the tables for the CRC-16 MODBUS protocol. This particular CRC 37 | * is commonly used in the Modbus RTU serial communications protocol, which is 38 | * a de facto standard communication protocol and an ISO standard used by many 39 | * industrial devices. 40 | */ 41 | 42 | /* Table of CRC values for high–order byte */ 43 | 44 | // clang-format off 45 | constexpr std::array kCRCHiTable = { 46 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 47 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 48 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 49 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 50 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 51 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 52 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 53 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 54 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 55 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 56 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 57 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 58 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 59 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 60 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 61 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 62 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 63 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 64 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 65 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 66 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 67 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 68 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 69 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 70 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 71 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 72 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 73 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 74 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 75 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 76 | 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 77 | 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40 78 | }; 79 | 80 | /* Table of CRC values for low–order byte */ 81 | constexpr std::array kCRCLoTable = { 82 | 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 83 | 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 84 | 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 85 | 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, 86 | 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 87 | 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 88 | 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 89 | 0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, 90 | 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 91 | 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, 92 | 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 93 | 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 94 | 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 95 | 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 96 | 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 97 | 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 98 | 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 99 | 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 100 | 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 101 | 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 102 | 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 103 | 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 104 | 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, 105 | 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 106 | 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 107 | 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 108 | 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 109 | 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 110 | 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 111 | 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, 112 | 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 113 | 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40 114 | }; 115 | // clang-format on 116 | 117 | uint16_t compute_crc(const std::vector& cmd) 118 | { 119 | uint16_t crc_hi = 0x00FF; 120 | uint16_t crc_lo = 0x00FF; 121 | 122 | for (uint8_t byte : cmd) 123 | { 124 | uint8_t index = crc_lo ^ byte; 125 | crc_lo = crc_hi ^ kCRCHiTable[index]; 126 | crc_hi = kCRCLoTable[index]; 127 | } 128 | 129 | return (crc_lo << 8) + crc_hi; 130 | } 131 | } // namespace epick_driver::crc_utils 132 | -------------------------------------------------------------------------------- /epick_driver/include/epick_driver/driver.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2023 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #pragma once 30 | 31 | #include 32 | #include 33 | 34 | namespace epick_driver 35 | { 36 | 37 | // Indicates if the user has requested the gripper to be activated. 38 | enum class GripperActivationAction 39 | { 40 | ClearGripperFaultStatus, 41 | Activate, 42 | }; 43 | 44 | // Indicates the gripper mode. 45 | enum class GripperMode 46 | { 47 | AutomaticMode, // Automatic. 48 | AdvancedMode, // Advanced. 49 | Unknown 50 | }; 51 | 52 | enum class GripperRegulateAction 53 | { 54 | StopVacuumGenerator, // Stop the vacuum generator. 55 | FollowRequestedVacuumParameters // Follow the requested vacuum parameters in real time. 56 | }; 57 | 58 | enum class GripperReleaseAction 59 | { 60 | NormalRelease, // Normal operation. 61 | ReleaseWithoutTimeout // Open the valves without any timeout. 62 | }; 63 | 64 | // Indicates the status of the gripper activation sequence. 65 | enum class GripperActivationStatus 66 | { 67 | GripperNotActivated, // Gripper is not activated. 68 | GripperOperational, // Gripper is operational. 69 | Unknown 70 | }; 71 | 72 | // Indicates the status of the object detection. 73 | enum class ObjectDetectionStatus 74 | { 75 | ObjectDetectedAtMinPressure, // Object detected. Minimum vacuum value reached. 76 | ObjectDetectedAtMaxPressure, // Object detected. Maximum vacuum value reached. 77 | NoObjectDetected, // Object loss, dropped or gripping timeout reached. 78 | Unknown // Unknown object detection. Regulating towards requested vacuum/pressure. 79 | }; 80 | 81 | enum class GripperFaultStatus 82 | { 83 | NoFault, 84 | ActionDelayed, 85 | PorousMaterialDetected, 86 | GrippingTimeout, 87 | ActivationBitNotSet, 88 | MaximumTemperatureExceeded, 89 | NoCommunicationForAtLeastOneSecond, 90 | UnderMinimumOperatingVoltage, 91 | AutomaticReleaseInProgress, 92 | InternalFault, 93 | AutomaticReleaseCompleted, 94 | Unknown 95 | }; 96 | 97 | enum class ActuatorStatus 98 | { 99 | Standby, 100 | Gripping, 101 | PassiveReleasing, 102 | ActiveReleasing 103 | }; 104 | 105 | struct GripperStatus 106 | { 107 | GripperActivationAction gripper_activation_action; 108 | GripperMode gripper_mode; 109 | GripperRegulateAction gripper_regulate_action; 110 | GripperActivationStatus gripper_activation_status; 111 | GripperFaultStatus gripper_fault_status; 112 | ActuatorStatus actuator_status; 113 | ObjectDetectionStatus object_detection_status; 114 | float max_vacuum_pressure; 115 | float actual_vacuum_pressure; 116 | }; 117 | 118 | /** 119 | * This is the interface of the driver to control the Epick Gripper. 120 | * The Driver interface can be easily mocked for testing or implemented to 121 | * fake the behavior of the real hardware. 122 | */ 123 | class Driver 124 | { 125 | public: 126 | Driver() = default; 127 | 128 | virtual void set_slave_address(uint8_t slave_address) = 0; 129 | 130 | virtual void set_mode(GripperMode gripper_mode) = 0; 131 | 132 | /** 133 | * Set the gripper maximum vacuum pressure to hold an object relative to the 134 | * atmospheric pressure. 135 | * The vacuum pressure is measured in kPa below the atmospheric pressure 136 | * (100KpA) and must fall between -100kPa (perfect vacuum) and 0kPa. 137 | * @param vacuum_pressure The maximum grip vacuum pressure between 138 | * -100kPa and 0kPa. 139 | */ 140 | virtual void set_grip_max_vacuum_pressure(float vacuum_pressure) = 0; 141 | 142 | /** 143 | * Set the gripper minimum acceptable vacuum pressure to hold an object 144 | * relative to the atmospheric pressure. 145 | * The vacuum pressure is measured in kPa below the atmospheric pressure 146 | * (100KpA) and must fall between -100kPa (perfect vacuum) and 0kPa. 147 | * @param vacuum_pressure The minimum acceptable vacuum pressure between 148 | * -100kPa and 0kPa. 149 | */ 150 | virtual void set_grip_min_vacuum_pressure(float vacuum_pressure) = 0; 151 | 152 | virtual void set_grip_timeout(std::chrono::milliseconds timeout) = 0; 153 | 154 | virtual void set_release_timeout(std::chrono::milliseconds timeout) = 0; 155 | 156 | /** Connect to the gripper serial connection. */ 157 | virtual bool connect() = 0; 158 | 159 | /** Disconnect from the gripper serial connection. */ 160 | virtual void disconnect() = 0; 161 | 162 | virtual void activate() = 0; 163 | virtual void deactivate() = 0; 164 | 165 | virtual void grip() = 0; 166 | virtual void release() = 0; 167 | 168 | virtual GripperStatus get_status() = 0; 169 | }; 170 | } // namespace epick_driver 171 | -------------------------------------------------------------------------------- /epick_hardware_tests/src/break.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | 37 | #include "command_line_utility.hpp" 38 | 39 | // This command is used to reproduce an issue we have with writing and reading 40 | // on the serial port. It runs an infinite loop of write/read instructions and 41 | // stops if/when an error is encountered. It usually fails between 1 and 6 42 | // minutes. 43 | 44 | using epick_driver::DefaultDriver; 45 | using epick_driver::DefaultSerial; 46 | using epick_driver::GripperMode; 47 | 48 | constexpr auto kComPort = "/dev/ttyUSB0"; 49 | constexpr auto kBaudRate = 115200; 50 | constexpr auto kTimeout = 0.5; 51 | constexpr auto kSlaveAddress = 0x09; 52 | 53 | int main(int argc, char* argv[]) 54 | { 55 | CommandLineUtility cli; 56 | 57 | std::string port = kComPort; 58 | cli.registerHandler( 59 | "--port", [&port](const char* value) { port = value; }, false); 60 | 61 | int baudrate = kBaudRate; 62 | cli.registerHandler( 63 | "--baudrate", [&baudrate](const char* value) { baudrate = std::stoi(value); }, false); 64 | 65 | double timeout = kTimeout; 66 | cli.registerHandler( 67 | "--timeout", [&timeout](const char* value) { timeout = std::stod(value); }, false); 68 | 69 | int slave_address = kSlaveAddress; 70 | cli.registerHandler( 71 | "--slave-address", [&slave_address](const char* value) { slave_address = std::stoi(value); }, false); 72 | 73 | cli.registerHandler("-h", [&]() { 74 | std::cout << "Usage: ./set_relative_pressure [OPTIONS]\n" 75 | << "Options:\n" 76 | << " --port VALUE Set the com port (default " << kComPort << ")\n" 77 | << " --baudrate VALUE Set the baudrate (default " << kBaudRate << "bps)\n" 78 | << " --timeout VALUE Set the read/write timeout (default " << kTimeout << "s)\n" 79 | << " --slave-address VALUE Set the slave address (default " << kSlaveAddress << ")\n" 80 | << " -h Show this help message\n"; 81 | exit(0); 82 | }); 83 | 84 | if (!cli.parse(argc, argv)) 85 | { 86 | return 1; 87 | } 88 | 89 | try 90 | { 91 | auto serial = std::make_unique(); 92 | serial->set_port(port); 93 | serial->set_baudrate(baudrate); 94 | serial->set_timeout(std::chrono::duration_cast(std::chrono::duration(timeout))); 95 | 96 | auto driver = std::make_unique(std::move(serial)); 97 | driver->set_slave_address(slave_address); 98 | 99 | std::cout << "Using the following parameters: " << std::endl; 100 | std::cout << " - port: " << port << std::endl; 101 | std::cout << " - baudrate: " << baudrate << "bps" << std::endl; 102 | std::cout << " - read/write timeout: " << timeout << "s" << std::endl; 103 | std::cout << " - slave address: " << slave_address << std::endl; 104 | 105 | std::cout << "Checking if the gripper is connected..." << std::endl; 106 | 107 | const bool connected = driver->connect(); 108 | if (!connected) 109 | { 110 | std::cout << "The gripper is not connected" << std::endl; 111 | return 1; 112 | } 113 | 114 | std::cout << "The gripper is connected." << std::endl; 115 | std::cout << "Activating the gripper..." << std::endl; 116 | 117 | driver->activate(); 118 | 119 | std::cout << "The gripper is activated." << std::endl; 120 | std::cout << "Reading multiple times the gripper status..." << std::endl; 121 | 122 | const auto start = std::chrono::steady_clock::now(); 123 | 124 | uint32_t counter = 0; 125 | bool ok = true; 126 | while (ok) 127 | { 128 | try 129 | { 130 | counter++; 131 | driver->get_status(); 132 | } 133 | catch (serial::IOException& e) 134 | { 135 | ok = false; 136 | std::cout << "Error at " << counter << " iteration: " << e.what() << std::endl; 137 | } 138 | } 139 | 140 | const auto end = std::chrono::steady_clock::now(); 141 | const auto duration = std::chrono::duration_cast(end - start); 142 | const int minutes = duration.count() / 60; 143 | const int seconds = duration.count() % 60; 144 | 145 | std::cout << "Execution time: " << minutes << "m:" << seconds << "s" << std::endl; 146 | } 147 | catch (const serial::IOException& e) 148 | { 149 | std::cout << "Failed to communicating with the gripper:" << e.what(); 150 | return 1; 151 | } 152 | 153 | return 0; 154 | } 155 | -------------------------------------------------------------------------------- /epick_hardware_tests/src/get_status.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2022 PickNik, Inc. 2 | // 3 | // Redistribution and use in source and binary forms, with or without 4 | // modification, are permitted provided that the following conditions are met: 5 | // 6 | // * Redistributions of source code must retain the above copyright 7 | // notice, this list of conditions and the following disclaimer. 8 | // 9 | // * Redistributions in binary form must reproduce the above copyright 10 | // notice, this list of conditions and the following disclaimer in the 11 | // documentation and/or other materials provided with the distribution. 12 | // 13 | // * Neither the name of the {copyright_holder} 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 HOLDER 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 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | #include 36 | #include 37 | 38 | #include "command_line_utility.hpp" 39 | 40 | // This is a command to read the status of the gripper. 41 | 42 | using epick_driver::DefaultDriver; 43 | using epick_driver::DefaultSerial; 44 | using epick_driver::GripperMode; 45 | using epick_driver::default_driver_utils::actuator_status_to_string; 46 | using epick_driver::default_driver_utils::fault_status_to_string; 47 | using epick_driver::default_driver_utils::gripper_activation_action_to_string; 48 | using epick_driver::default_driver_utils::gripper_mode_to_string; 49 | using epick_driver::default_driver_utils::gripper_regulate_action_to_string; 50 | using epick_driver::default_driver_utils::object_detection_to_string; 51 | 52 | constexpr auto kComPort = "/dev/ttyUSB0"; 53 | constexpr auto kBaudRate = 115200; 54 | constexpr auto kTimeout = 0.5; 55 | constexpr auto kSlaveAddress = 0x09; 56 | 57 | int main(int argc, char* argv[]) 58 | { 59 | CommandLineUtility cli; 60 | 61 | std::string port = kComPort; 62 | cli.registerHandler( 63 | "--port", [&port](const char* value) { port = value; }, false); 64 | 65 | int baudrate = kBaudRate; 66 | cli.registerHandler( 67 | "--baudrate", [&baudrate](const char* value) { baudrate = std::stoi(value); }, false); 68 | 69 | double timeout = kTimeout; 70 | cli.registerHandler( 71 | "--timeout", [&timeout](const char* value) { timeout = std::stod(value); }, false); 72 | 73 | int slave_address = kSlaveAddress; 74 | cli.registerHandler( 75 | "--slave-address", [&slave_address](const char* value) { slave_address = std::stoi(value); }, false); 76 | 77 | cli.registerHandler("-h", [&]() { 78 | std::cout << "Usage: ./set_relative_pressure [OPTIONS]\n" 79 | << "Options:\n" 80 | << " --port VALUE Set the com port (default " << kComPort << ")\n" 81 | << " --baudrate VALUE Set the baudrate (default " << kBaudRate << "bps)\n" 82 | << " --timeout VALUE Set the read/write timeout (default " << kTimeout << "s)\n" 83 | << " --slave-address VALUE Set the slave address (default " << kSlaveAddress << ")\n" 84 | << " -h Show this help message\n"; 85 | exit(0); 86 | }); 87 | 88 | if (!cli.parse(argc, argv)) 89 | { 90 | return 1; 91 | } 92 | 93 | try 94 | { 95 | auto serial = std::make_unique(); 96 | serial->set_port(port); 97 | serial->set_baudrate(baudrate); 98 | serial->set_timeout(std::chrono::duration_cast(std::chrono::duration(timeout))); 99 | 100 | auto driver = std::make_unique(std::move(serial)); 101 | driver->set_slave_address(slave_address); 102 | 103 | std::cout << "Using the following parameters: " << std::endl; 104 | std::cout << " - port: " << port << std::endl; 105 | std::cout << " - baudrate: " << baudrate << "bps" << std::endl; 106 | std::cout << " - read/write timeut: " << timeout << "ms" << std::endl; 107 | std::cout << " - slave address: " << slave_address << std::endl; 108 | 109 | std::cout << "Checking if the gripper is connected..." << std::endl; 110 | 111 | const bool connected = driver->connect(); 112 | if (!connected) 113 | { 114 | std::cout << "The gripper is not connected" << std::endl; 115 | return 1; 116 | } 117 | 118 | std::cout << "The gripper is connected." << std::endl; 119 | std::cout << "Reading the gripper status..." << std::endl; 120 | 121 | const auto status = driver->get_status(); 122 | 123 | std::cout << "Status retrieved:" << std::endl; 124 | 125 | std::cout << " - gripper activation action: " 126 | << gripper_activation_action_to_string(status.gripper_activation_action) << std::endl; 127 | std::cout << " - gripper regulate action: " << gripper_regulate_action_to_string(status.gripper_regulate_action) 128 | << std::endl; 129 | std::cout << " - gripper mode: " << gripper_mode_to_string(status.gripper_mode) << std::endl; 130 | std::cout << " - object detection status: " << object_detection_to_string(status.object_detection_status) 131 | << std::endl; 132 | std::cout << " - gripper fault status: " << fault_status_to_string(status.gripper_fault_status) << std::endl; 133 | std::cout << " - actuator status: " << actuator_status_to_string(status.actuator_status) << std::endl; 134 | std::cout << " - max vacuum pressure: " << status.max_vacuum_pressure << "kPa" << std::endl; 135 | std::cout << " - actual vacuum pressure: " << status.actual_vacuum_pressure << "kPa" << std::endl; 136 | } 137 | catch (const serial::IOException& e) 138 | { 139 | std::cout << "Failed to communicating with the gripper:" << e.what(); 140 | return 1; 141 | } 142 | 143 | return 0; 144 | } 145 | --------------------------------------------------------------------------------