diff --git a/.clang-format b/.clang-format index 4a79087..3437820 100644 --- a/.clang-format +++ b/.clang-format @@ -1,20 +1,74 @@ --- -Language: Cpp BasedOnStyle: Google +ColumnLimit: 120 +MaxEmptyLinesToKeep: 1 +SortIncludes: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never AccessModifierOffset: -2 -AlignAfterOpenBracket: AlwaysBreak +ConstructorInitializerIndentWidth: 2 +NamespaceIndentation: None +ContinuationIndentWidth: 4 +IndentCaseLabels: true +IndentFunctionDeclarationAfterType: false + +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true + +AllowAllParametersOfDeclarationOnNextLine: false +ExperimentalAutoDetectBinPacking: false +ObjCSpaceBeforeProtocolList: true +Cpp11BracedListStyle: false + +AllowShortBlocksOnASingleLine: true +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AllowShortCaseLabelsOnASingleLine: false + +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true + +BinPackParameters: true +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true + +PenaltyExcessCharacter: 50 +PenaltyBreakBeforeFirstCallParameter: 30 +PenaltyBreakComment: 1000 +PenaltyBreakFirstLessLess: 10 +PenaltyBreakString: 100 +PenaltyReturnTypeOnItsOwnLine: 50 + +SpacesBeforeTrailingComments: 2 +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterCStyleCast: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases BraceWrapping: + AfterCaseLabel: true AfterClass: true + AfterControlStatement: true + AfterEnum: true AfterFunction: true AfterNamespace: true AfterStruct: true - AfterEnum: true -BreakBeforeBraces: Custom -ColumnLimit: 100 -ConstructorInitializerIndentWidth: 0 -ContinuationIndentWidth: 2 -DerivePointerAlignment: false -PointerAlignment: Middle -ReflowComments: false -... + AfterUnion: true + BeforeCatch: true + BeforeElse: true + IndentBraces: false diff --git a/.github/workflows/ci-format.yml b/.github/workflows/ci-format.yml index 3d60336..797a757 100644 --- a/.github/workflows/ci-format.yml +++ b/.github/workflows/ci-format.yml @@ -13,14 +13,14 @@ on: jobs: pre-commit: name: Format - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - - uses: actions/checkout@v3 - - uses: actions/setup-python@v4.4.0 - with: - python-version: '3.10' - - name: Install system hooks - run: sudo apt install -qq clang-format-14 cppcheck - - uses: pre-commit/action@v3.0.0 - with: - extra_args: --all-files --hook-stage manual + - uses: actions/checkout@v3 + - uses: actions/setup-python@v4.4.0 + with: + python-version: "3.10" + - name: Install system hooks + run: sudo apt install -qq clang-format-14 cppcheck + - uses: pre-commit/action@v3.0.0 + with: + extra_args: --all-files --hook-stage manual diff --git a/.github/workflows/ci-ros-lint.yml b/.github/workflows/ci-ros-lint.yml index be06560..d038c28 100644 --- a/.github/workflows/ci-ros-lint.yml +++ b/.github/workflows/ci-ros-lint.yml @@ -5,41 +5,37 @@ on: jobs: ament_lint: name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 strategy: fail-fast: false matrix: - linter: [cppcheck, copyright, lint_cmake] + linter: [copyright, lint_cmake] steps: - - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.2 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - distribution: rolling - linter: ${{ matrix.linter }} - package-name: - robotiq_driver - robotiq_controllers - robotiq_description + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@0.6.2 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + distribution: rolling + linter: ${{ matrix.linter }} + package-name: robotiq_driver + robotiq_controllers + robotiq_description - - ament_lint_100: + ament_lint_121: name: ament_${{ matrix.linter }} - runs-on: ubuntu-20.04 + runs-on: ubuntu-22.04 strategy: fail-fast: false matrix: - linter: [cpplint] + linter: [cpplint] steps: - - uses: actions/checkout@v3 - - uses: ros-tooling/setup-ros@0.6.2 - - uses: ros-tooling/action-ros-lint@v0.1 - with: - distribution: rolling - linter: cpplint - arguments: "--linelength=100 --filter=-whitespace/newline" - package-name: - robotiq_driver - robotiq_controllers - robotiq_description - ros2_robotiq_gripper + - uses: actions/checkout@v3 + - uses: ros-tooling/setup-ros@0.6.2 + - uses: ros-tooling/action-ros-lint@v0.1 + with: + distribution: rolling + linter: cpplint + arguments: "--linelength=121 --filter=-whitespace/newline" + package-name: robotiq_driver + robotiq_controllers + robotiq_description diff --git a/.github/workflows/prerelease-check.yml b/.github/workflows/prerelease-check.yml index 9052985..343abeb 100644 --- a/.github/workflows/prerelease-check.yml +++ b/.github/workflows/prerelease-check.yml @@ -4,31 +4,27 @@ on: workflow_dispatch: inputs: ros_distro: - description: 'Chose ROS distribution' + description: "Chose ROS distribution" required: true - default: 'rolling' + default: "rolling" type: choice options: - - foxy - - galactic - - humble - - iron - - rolling + - humble + - iron + - rolling branch: - description: 'Chose branch for distro' + description: "Chose branch for distro" required: true - default: 'master' + default: "main" type: choice options: - - foxy - - galactic - - humble - - iron - - master + - humble + - iron + - master jobs: pre_release: - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 steps: - uses: actions/checkout@v3 with: diff --git a/.github/workflows/reusable-industrial-ci-with-cache.yml b/.github/workflows/reusable-industrial-ci-with-cache.yml index 490b680..905a4bf 100644 --- a/.github/workflows/reusable-industrial-ci-with-cache.yml +++ b/.github/workflows/reusable-industrial-ci-with-cache.yml @@ -6,51 +6,50 @@ on: workflow_call: inputs: ref_for_scheduled_build: - description: 'Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag.' - default: '' + description: "Reference on which the repo should be checkout for scheduled build. Usually is this name of a branch or a tag." + default: "" required: false type: string upstream_workspace: - description: 'UPSTREAM_WORKSPACE variable for industrial_ci. Usually path to local .repos file.' + description: "UPSTREAM_WORKSPACE variable for industrial_ci. Usually path to local .repos file." required: true type: string ros_distro: - description: 'ROS_DISTRO variable for industrial_ci' + description: "ROS_DISTRO variable for industrial_ci" required: true type: string ros_repo: description: 'ROS_REPO to run for industrial_ci. Possible values: "main", "testing"' - default: 'main' + default: "main" required: false type: string os_code_name: - description: 'OS_CODE_NAME variable for industrial_ci' - default: '' + description: "OS_CODE_NAME variable for industrial_ci" + default: "" required: false type: string before_install_upstream_dependencies: - description: 'BEFORE_INSTALL_UPSTREAM_DEPENDENCIES variable for industrial_ci' - default: '' + description: "BEFORE_INSTALL_UPSTREAM_DEPENDENCIES variable for industrial_ci" + default: "" required: false type: string ccache_dir: description: 'Local path to store cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' - default: '.ccache' + default: ".ccache" required: false type: string basedir: description: 'Local path to workspace base directory to cache (from "github.workspace"). For standard industrial_ci configuration do not have to be changed' - default: '.work' + default: ".work" required: false type: string - jobs: reusable_industrial_ci_with_cache: name: ${{ inputs.ros_distro }} ${{ inputs.ros_repo }} ${{ inputs.os_code_name }} - runs-on: ubuntu-latest + runs-on: ubuntu-22.04 env: CCACHE_DIR: ${{ github.workspace }}/${{ inputs.ccache_dir }} BASEDIR: ${{ github.workspace }}/${{ inputs.basedir }} @@ -80,7 +79,7 @@ jobs: restore-keys: | ccache-${{ env.CACHE_PREFIX }}-${{ github.sha }} ccache-${{ env.CACHE_PREFIX }} - - uses: 'ros-industrial/industrial_ci@master' + - uses: "ros-industrial/industrial_ci@master" env: UPSTREAM_WORKSPACE: ${{ inputs.upstream_workspace }} ROS_DISTRO: ${{ inputs.ros_distro }} diff --git a/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp b/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp index f922400..d0dac29 100644 --- a/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp +++ b/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp @@ -40,19 +40,17 @@ class RobotiqActivationController : public controller_interface::ControllerInter controller_interface::InterfaceConfiguration state_interface_configuration() const override; - controller_interface::return_type update( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; CallbackReturn on_init() override; private: - bool reactivateGripper( - std_srvs::srv::Trigger::Request::SharedPtr req, - std_srvs::srv::Trigger::Response::SharedPtr resp); + bool reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr req, + std_srvs::srv::Trigger::Response::SharedPtr resp); static constexpr double ASYNC_WAITING = 2.0; enum CommandInterfaces diff --git a/robotiq_controllers/src/robotiq_activation_controller.cpp b/robotiq_controllers/src/robotiq_activation_controller.cpp index 03c4fa8..aa63d21 100644 --- a/robotiq_controllers/src/robotiq_activation_controller.cpp +++ b/robotiq_controllers/src/robotiq_activation_controller.cpp @@ -30,8 +30,7 @@ namespace robotiq_controllers { -controller_interface::InterfaceConfiguration -RobotiqActivationController::command_interface_configuration() const +controller_interface::InterfaceConfiguration RobotiqActivationController::command_interface_configuration() const { controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -42,8 +41,7 @@ RobotiqActivationController::command_interface_configuration() const return config; } -controller_interface::InterfaceConfiguration -RobotiqActivationController::state_interface_configuration() const +controller_interface::InterfaceConfiguration RobotiqActivationController::state_interface_configuration() const { controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; @@ -51,62 +49,67 @@ RobotiqActivationController::state_interface_configuration() const return config; } -controller_interface::return_type RobotiqActivationController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +controller_interface::return_type RobotiqActivationController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { return controller_interface::return_type::OK; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -RobotiqActivationController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) +RobotiqActivationController::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { // Check command interfaces. - if (command_interfaces_.size() != 2) { - RCLCPP_ERROR( - get_node()->get_logger(), "Expected %d command interfaces, but got %zu.", 2, - command_interfaces_.size()); + if (command_interfaces_.size() != 2) + { + RCLCPP_ERROR(get_node()->get_logger(), "Expected %d command interfaces, but got %zu.", 2, + command_interfaces_.size()); return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; } - try { + try + { // Create service for re-activating the gripper. reactivate_gripper_srv_ = get_node()->create_service( - "~/reactivate_gripper", - [this]( - std_srvs::srv::Trigger::Request::SharedPtr req, - std_srvs::srv::Trigger::Response::SharedPtr resp) { this->reactivateGripper(req, resp); }); - } catch (...) { + "~/reactivate_gripper", + [this](std_srvs::srv::Trigger::Request::SharedPtr req, std_srvs::srv::Trigger::Response::SharedPtr resp) { + this->reactivateGripper(req, resp); + }); + } + catch (...) + { return LifecycleNodeInterface::CallbackReturn::ERROR; } return LifecycleNodeInterface::CallbackReturn::SUCCESS; } rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -RobotiqActivationController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) +RobotiqActivationController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) { - try { + try + { reactivate_gripper_srv_.reset(); - } catch (...) { + } + catch (...) + { return LifecycleNodeInterface::CallbackReturn::ERROR; } return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn -RobotiqActivationController::on_init() +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn RobotiqActivationController::on_init() { return LifecycleNodeInterface::CallbackReturn::SUCCESS; } -bool RobotiqActivationController::reactivateGripper( - std_srvs::srv::Trigger::Request::SharedPtr /*req*/, - std_srvs::srv::Trigger::Response::SharedPtr resp) +bool RobotiqActivationController::reactivateGripper(std_srvs::srv::Trigger::Request::SharedPtr /*req*/, + std_srvs::srv::Trigger::Response::SharedPtr resp) { command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].set_value(ASYNC_WAITING); command_interfaces_[REACTIVATE_GRIPPER_CMD].set_value(1.0); - while (command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value() == ASYNC_WAITING) { + while (command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value() == ASYNC_WAITING) + { std::this_thread::sleep_for(std::chrono::milliseconds(50)); } resp->success = command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value(); @@ -117,5 +120,4 @@ bool RobotiqActivationController::reactivateGripper( #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - robotiq_controllers::RobotiqActivationController, controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS(robotiq_controllers::RobotiqActivationController, controller_interface::ControllerInterface) diff --git a/robotiq_description/CMakeLists.txt b/robotiq_description/CMakeLists.txt index 6ba826b..e908b35 100644 --- a/robotiq_description/CMakeLists.txt +++ b/robotiq_description/CMakeLists.txt @@ -10,10 +10,11 @@ find_package(ament_cmake REQUIRED) install( DIRECTORY - urdf + config launch - rviz meshes + rviz + urdf DESTINATION share/${PROJECT_NAME} ) diff --git a/robotiq_driver/config/robotiq_controllers.yaml b/robotiq_description/config/robotiq_controllers.yaml similarity index 100% rename from robotiq_driver/config/robotiq_controllers.yaml rename to robotiq_description/config/robotiq_controllers.yaml diff --git a/robotiq_driver/launch/robotiq_control.launch.py b/robotiq_description/launch/robotiq_control.launch.py similarity index 93% rename from robotiq_driver/launch/robotiq_control.launch.py rename to robotiq_description/launch/robotiq_control.launch.py index 2e0d196..d9430ee 100644 --- a/robotiq_driver/launch/robotiq_control.launch.py +++ b/robotiq_description/launch/robotiq_control.launch.py @@ -33,6 +33,7 @@ LaunchConfiguration, PathJoinSubstitution, ) +from launch.conditions import IfCondition import launch_ros import os @@ -48,10 +49,6 @@ def generate_launch_description(): description_pkg_share, "rviz", "view_urdf.rviz" ) - pkg_share = launch_ros.substitutions.FindPackageShare( - package="robotiq_driver" - ).find("robotiq_driver") - args = [] args.append( launch.actions.DeclareLaunchArgument( @@ -67,6 +64,11 @@ def generate_launch_description(): description="Absolute path to rviz config file", ) ) + args.append( + launch.actions.DeclareLaunchArgument( + name="launch_rviz", default_value="false", description="Launch RViz?" + ) + ) robot_description_content = Command( [ @@ -85,7 +87,7 @@ def generate_launch_description(): update_rate_config_file = PathJoinSubstitution( [ - pkg_share, + description_pkg_share, "config", "robotiq_update_rate.yaml", ] @@ -93,7 +95,7 @@ def generate_launch_description(): controllers_file = "robotiq_controllers.yaml" initial_joint_controllers = PathJoinSubstitution( - [pkg_share, "config", controllers_file] + [description_pkg_share, "config", controllers_file] ) control_node = launch_ros.actions.Node( @@ -118,6 +120,7 @@ def generate_launch_description(): name="rviz2", output="log", arguments=["-d", LaunchConfiguration("rvizconfig")], + condition=IfCondition(LaunchConfiguration("launch_rviz")), ) joint_state_broadcaster_spawner = launch_ros.actions.Node( diff --git a/robotiq_description/package.xml b/robotiq_description/package.xml index 18611bc..2cb93b0 100644 --- a/robotiq_description/package.xml +++ b/robotiq_description/package.xml @@ -19,6 +19,12 @@ urdf xacro + ros2_control + ros2_controllers + + gripper_controllers + joint_state_broadcaster + ament_lint_auto ament_lint_common diff --git a/robotiq_description/urdf/2f_85.ros2_control.xacro b/robotiq_description/urdf/2f_85.ros2_control.xacro index df721b1..8ca33f3 100644 --- a/robotiq_description/urdf/2f_85.ros2_control.xacro +++ b/robotiq_description/urdf/2f_85.ros2_control.xacro @@ -13,6 +13,10 @@ + + + false + topic_based_ros2_control/TopicBasedSystem /isaac_joint_commands diff --git a/robotiq_driver/CMakeLists.txt b/robotiq_driver/CMakeLists.txt index bda81ea..df9f8a6 100644 --- a/robotiq_driver/CMakeLists.txt +++ b/robotiq_driver/CMakeLists.txt @@ -1,12 +1,15 @@ +# See https://docs.ros.org/en/humble/How-To-Guides/Ament-CMake-Documentation.html + cmake_minimum_required(VERSION 3.8) -project(robotiq_driver) +project(robotiq_driver LANGUAGES CXX) + +# This module provides installation directories as per the GNU coding standards. +include(GNUInstallDirs) if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() - -# find dependencies find_package(ament_cmake REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) @@ -23,88 +26,115 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS serial ) +# Robotiq driver library. + add_library( - ${PROJECT_NAME} + robotiq_driver SHARED - src/crc.cpp + include/robotiq_driver/fake/fake_driver.hpp + include/robotiq_driver/crc_utils.hpp + include/robotiq_driver/data_utils.hpp + include/robotiq_driver/default_driver.hpp + include/robotiq_driver/default_driver_factory.hpp + include/robotiq_driver/default_serial.hpp + include/robotiq_driver/default_serial_factory.hpp + include/robotiq_driver/driver.hpp + include/robotiq_driver/driver_exception.hpp + include/robotiq_driver/driver_factory.hpp + include/robotiq_driver/hardware_interface.hpp + include/robotiq_driver/serial.hpp + include/robotiq_driver/serial_factory.hpp + include/robotiq_driver/visibility_control.hpp + src/crc_utils.cpp + src/data_utils.cpp src/hardware_interface.cpp - src/robotiq_gripper_interface.cpp + src/default_driver.cpp + src/default_driver_factory.cpp + src/fake/fake_driver.cpp + src/default_serial.cpp + src/default_serial_factory.cpp ) +target_link_libraries(robotiq_driver atomic) target_include_directories( - ${PROJECT_NAME} + robotiq_driver PUBLIC $ $ ) -target_link_libraries(${PROJECT_NAME} atomic) ament_target_dependencies( - ${PROJECT_NAME} + robotiq_driver ${THIS_PACKAGE_INCLUDE_DEPENDS} ) +############################################################################### +# PLUGINS + pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml) -add_executable(gripper_interface_test src/gripper_interface_test.cpp) -target_include_directories(gripper_interface_test PRIVATE include) -ament_target_dependencies(gripper_interface_test serial) -target_link_libraries(gripper_interface_test ${PROJECT_NAME}) +############################################################################### +# EXPORTS -# INSTALL -install( - TARGETS ${PROJECT_NAME} - EXPORT export_${PROJECT_NAME} - ARCHIVE DESTINATION lib/${PROJECT_NAME} - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin +# This is necessary to allow this library’s clients to use the syntax +# target_link_libraries(client ::) +# Without this a client cannot find this library. +# It can take an arbitrary list of targets named as EXPORT in an install call. +ament_export_targets( + robotiq_driver_targets # Must match the EXPORT label below in the install section. ) -install( - TARGETS gripper_interface_test - DESTINATION lib/${PROJECT_NAME} +# Help downstream packages to find transitive dependencies i.e. export all +# dependencies required by a package to use this library. +# When a package calls find_package(epick_driver), CMake looks for a file +# called epick_driverConfig.cmake which sets up everything another project +# would need to depend on this one. +ament_export_dependencies( + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -install( - DIRECTORY include/ - DESTINATION include +# Tell downstream packages where to find our headers. +ament_export_include_directories( + include +) +# Tell downstream packages our libraries to link against. +ament_export_libraries( + robotiq_driver ) + +############################################################################### +# INSTALL + +# Install all files of the include folder into the give destination. install( - DIRECTORY config - DESTINATION share/${PROJECT_NAME} + DIRECTORY include/ + DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # include ) + +# Install our library. install( - DIRECTORY launch - DESTINATION share/${PROJECT_NAME} + TARGETS robotiq_driver + EXPORT robotiq_driver_targets + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} # lib + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} # lib + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} # bin + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} # include ) +############################################################################### +# TESTS + +# CTest module automatically creates a BUILD_TESTING option that selects +# whether to enable testing support (ON by default). +include(CTest) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - # the following line skips the linter which checks for copyrights - # comment the line when a copyright and license is added to all source files - set(ament_cmake_copyright_FOUND TRUE) - # the following line skips cpplint (only works in a git repo) - # comment the line when this package is in a git repo and when - # a copyright and license is added to all source files - set(ament_cmake_cpplint_FOUND TRUE) - # the following skips uncrustify - # ament_uncrustify and ament_clang_format cannot both be satisfied - set(ament_cmake_uncrustify_FOUND TRUE) - # the following skips ament_flake8 - # ament_flake8 and black fight over double or single quotes - # flake8 is run via pre-commit with a .flake8 configuration file - set(ament_cmake_flake8_FOUND TRUE) - ament_lint_auto_find_test_dependencies() + add_subdirectory(tests) endif() -## EXPORTS -ament_export_include_directories( - include -) -ament_export_libraries( - ${PROJECT_NAME} -) -ament_export_targets( - export_${PROJECT_NAME} -) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) +############################################################################### +# LINTERS + +add_custom_target(format + COMMAND clang-format -i `git ls-files *.hpp *.cpp` + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) +add_custom_target(tidy + COMMAND clang-tidy -p ${CMAKE_BINARY_DIR} `git ls-files *.cpp` + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR}) ament_package() diff --git a/robotiq_driver/include/robotiq_driver/crc.hpp b/robotiq_driver/include/robotiq_driver/crc_utils.hpp similarity index 83% rename from robotiq_driver/include/robotiq_driver/crc.hpp rename to robotiq_driver/include/robotiq_driver/crc_utils.hpp index ca19d21..2d7f54c 100644 --- a/robotiq_driver/include/robotiq_driver/crc.hpp +++ b/robotiq_driver/include/robotiq_driver/crc_utils.hpp @@ -1,4 +1,4 @@ -// Copyright (c) 2022 PickNik, Inc. +// Copyright (c) 2023 PickNik, Inc. // // Redistribution and use in source and binary forms, with or without // modification, are permitted provided that the following conditions are met: @@ -31,4 +31,12 @@ #include #include -uint16_t computeCRC(const std::vector & cmd); +namespace robotiq_driver::crc_utils +{ +/** + * @brief Compute the CRC for the CRC-16 MODBUS protocol. + * @param data The data to compute the CRC for. + * @return A 16-bits CRC. + */ +uint16_t compute_crc(const std::vector& data); +} // namespace robotiq_driver::crc_utils diff --git a/robotiq_driver/include/robotiq_driver/data_utils.hpp b/robotiq_driver/include/robotiq_driver/data_utils.hpp new file mode 100644 index 0000000..0d68269 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/data_utils.hpp @@ -0,0 +1,76 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include +#include +#include +#include + +/** + * Utility class to convert between commonly used data types. + */ +namespace robotiq_driver::data_utils +{ +/** + * Convert a sequence of uint8_t into a sequence of hex numbers. + * @param bytes The sequence of bytes. + * @return A string containing the sequence of hex numbers. + */ +std::string to_hex(const std::vector& bytes); + +/** + * Convert a sequence of uint16_t into a sequence of hex numbers. + * @param bytes The sequence of bytes. + * @return A string containing the sequence of hex numbers. + */ +std::string to_hex(const std::vector& bytes); + +/** + * Convert a byte to a binary representation for testing purposes. + * @param byte The byte to decode. + * @return The binary representation of the given byte. + */ +std::string to_binary_string(const uint8_t byte); + +/** + * Get the Most Significant Byte (MSB) of the given value. + * @param value A 16-bits value. + * @return The Most Significant Byte (MSB) of the given value. + */ +uint8_t get_msb(uint16_t value); + +/** + * Get the Least Significant Byte (LSB) of the given value. + * @param value A 16-bits value. + * @return The Least Significant Byte (LSB) of the given value. + */ +uint8_t get_lsb(uint16_t value); + +} // namespace robotiq_driver::data_utils diff --git a/robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp b/robotiq_driver/include/robotiq_driver/default_driver.hpp similarity index 62% rename from robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp rename to robotiq_driver/include/robotiq_driver/default_driver.hpp index c6203e2..8465dc8 100644 --- a/robotiq_driver/include/robotiq_driver/robotiq_gripper_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/default_driver.hpp @@ -28,130 +28,90 @@ #pragma once -#include - +#include #include #include +#include +#include + /** * @brief This class is responsible for communicating with the gripper via a serial port, and maintaining a record of * the gripper's current state. * */ -class RobotiqGripperInterface +namespace robotiq_driver +{ +class DefaultDriver : public Driver { public: - explicit RobotiqGripperInterface( - const std::string & com_port = "/dev/ttyUSB0", uint8_t slave_id = 0x09); + explicit DefaultDriver(std::unique_ptr serial); - /** - * @brief Activates the gripper. - * - * @throw serial::IOException on failure to successfully communicate with gripper port - */ - void activateGripper(); + bool connect() override; + void disconnect() override; - /** - * @brief Deactivates the gripper. - * - * @throw serial::IOException on failure to successfully communicate with gripper port - */ - void deactivateGripper(); + void set_slave_address(uint8_t slave_address) override; + + /** Activate the gripper with the specified operation mode and parameters. */ + void activate() override; + + /** Deactivate the gripper. */ + void deactivate() override; /** * @brief Commands the gripper to move to the desired position. - * * @param pos A value between 0x00 (fully open) and 0xFF (fully closed). */ - void setGripperPosition(uint8_t pos); + void set_gripper_position(uint8_t pos) override; /** * @brief Return the current position of the gripper. - * * @throw serial::IOException on failure to successfully communicate with gripper port - * * @return uint8_t A value between 0x00 (fully open) and 0xFF (fully closed). */ - uint8_t getGripperPosition(); + uint8_t get_gripper_position() override; /** * @brief Returns true if the gripper is currently moving, false otherwise. * */ - bool gripperIsMoving(); + bool gripper_is_moving() override; /** * @brief Set the speed of the gripper. - * * @param speed A value between 0x00 (stopped) and 0xFF (full speed). */ - void setSpeed(uint8_t speed); + void set_speed(uint8_t speed) override; /** * @brief Set how forcefully the gripper opens or closes. - * * @param force A value between 0x00 (no force) or 0xFF (maximum force). */ - void setForce(uint8_t force); - - enum class ActivationStatus - { - RESET, - ACTIVE - }; - - enum class ActionStatus - { - STOPPED, - MOVING - }; - - enum class GripperStatus - { - RESET, - IN_PROGRESS, - COMPLETED, - }; - - enum class ObjectDetectionStatus - { - MOVING, - OBJECT_DETECTED_OPENING, - OBJECT_DETECTED_CLOSING, - AT_REQUESTED_POSITION - }; + void set_force(uint8_t force) override; private: - std::vector createReadCommand(uint16_t first_register, uint8_t num_registers); - std::vector createWriteCommand( - uint16_t first_register, const std::vector & data); - /** - * @brief read response from the gripper. - * - * @param num_bytes Number of bytes to be read from device port. - * @throw serial::IOException on failure to successfully communicate with gripper port + * With this command we send a request and wait for a response of given size. + * Behind the scene, if the response is not received, the software makes an attempt + * to resend the command up to 5 times before returning an empty response. + * @param request The command request. + * @param response_size The response expected size. + * @return The response or an empty vector if an en error occurred. */ - std::vector readResponse(size_t num_bytes); + std::vector send(const std::vector& request, size_t response_size) const; - /** - * @brief Send a command to the gripper. - * - * @param cmd The command. - * @throw serial::IOException on failure to successfully communicate with gripper port - */ - void sendCommand(const std::vector & cmd); + std::vector create_read_command(uint16_t first_register, uint8_t num_registers); + std::vector create_write_command(uint16_t first_register, const std::vector& data); /** * @brief Read the current status of the gripper, and update member variables as appropriate. * * @throw serial::IOException on failure to successfully communicate with gripper port */ - void updateStatus(); + void update_status(); - serial::Serial port_; - uint8_t slave_id_; - std::vector read_command_; + std::unique_ptr serial_ = nullptr; + uint8_t slave_address_; ActivationStatus activation_status_; ActionStatus action_status_; @@ -162,3 +122,4 @@ class RobotiqGripperInterface uint8_t commanded_gripper_speed_; uint8_t commanded_gripper_force_; }; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp b/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp new file mode 100644 index 0000000..142ce80 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/default_driver_factory.hpp @@ -0,0 +1,59 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include + +#include +#include + +#include + +namespace robotiq_driver +{ +/** + * This class is used to create a default driver to interact with the hardware. + */ +class DefaultDriverFactory : public DriverFactory +{ +public: + DefaultDriverFactory() = default; + + /** + * @brief Create a driver. + * @param info The hardware information. + * @return A driver to interact with the hardware. + */ + std::unique_ptr create(const hardware_interface::HardwareInfo& info) const; + +protected: + // Seam for testing. + virtual std::unique_ptr create_driver(const hardware_interface::HardwareInfo& info) const; +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/default_serial.hpp b/robotiq_driver/include/robotiq_driver/default_serial.hpp new file mode 100644 index 0000000..9f60022 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/default_serial.hpp @@ -0,0 +1,76 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include + +#include +#include +#include + +#include + +namespace serial +{ +class Serial; +} + +namespace robotiq_driver +{ +class DefaultSerial : public Serial +{ +public: + /** + * Creates a Serial object to send and receive bytes to and from the serial + * port. + */ + DefaultSerial(); + + void open() override; + + [[nodiscard]] bool is_open() const override; + + void close() override; + + [[nodiscard]] std::vector read(size_t size = 1) override; + void write(const std::vector& data) override; + + void set_port(const std::string& port) override; + [[nodiscard]] std::string get_port() const override; + + void set_timeout(std::chrono::milliseconds timeout_ms) override; + [[nodiscard]] std::chrono::milliseconds get_timeout() const override; + + void set_baudrate(uint32_t baudrate) override; + [[nodiscard]] uint32_t get_baudrate() const override; + +private: + std::unique_ptr serial_ = nullptr; +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/default_serial_factory.hpp b/robotiq_driver/include/robotiq_driver/default_serial_factory.hpp new file mode 100644 index 0000000..e635674 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/default_serial_factory.hpp @@ -0,0 +1,58 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include + +#include +#include +#include + +namespace robotiq_driver +{ +/** + * This class is used to create a default driver to interact with the hardware. + */ +class DefaultSerialFactory : public SerialFactory +{ +public: + DefaultSerialFactory() = default; + + /** + * @brief Create a serial interface. + * @param info The hardware information. + * @return A sarial interface to communicate with the hardware. + */ + std::unique_ptr create(const hardware_interface::HardwareInfo& info) const; + +protected: + // Seam for testing. + virtual std::unique_ptr create_serial() const; +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/driver.hpp b/robotiq_driver/include/robotiq_driver/driver.hpp new file mode 100644 index 0000000..9309b39 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/driver.hpp @@ -0,0 +1,121 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include + +/** + * @brief This interface describes how to communicate with the gripper hardware. + */ +namespace robotiq_driver +{ +class Driver +{ +public: + enum class ActivationStatus + { + RESET, + ACTIVE + }; + + enum class ActionStatus + { + STOPPED, + MOVING + }; + + enum class GripperStatus + { + RESET, + IN_PROGRESS, + COMPLETED, + }; + + enum class ObjectDetectionStatus + { + MOVING, + OBJECT_DETECTED_OPENING, + OBJECT_DETECTED_CLOSING, + AT_REQUESTED_POSITION + }; + + virtual void set_slave_address(uint8_t slave_address) = 0; + + /** Connect to the gripper serial connection. */ + virtual bool connect() = 0; + + /** Disconnect from the gripper serial connection. */ + virtual void disconnect() = 0; + + /** + * @brief Activates the gripper. + * @throw serial::IOException on failure to successfully communicate with gripper port + */ + virtual void activate() = 0; + + /** + * @brief Deactivates the gripper. + * @throw serial::IOException on failure to successfully communicate with gripper port + */ + virtual void deactivate() = 0; + + /** + * @brief Commands the gripper to move to the desired position. + * @param pos A value between 0x00 (fully open) and 0xFF (fully closed). + */ + virtual void set_gripper_position(uint8_t pos) = 0; + + /** + * @brief Return the current position of the gripper. + * + * @throw serial::IOException on failure to successfully communicate with gripper port + * + * @return uint8_t A value between 0x00 (fully open) and 0xFF (fully closed). + */ + virtual uint8_t get_gripper_position() = 0; + + /** + * @brief Returns true if the gripper is currently moving, false otherwise. + */ + virtual bool gripper_is_moving() = 0; + + /** + * @brief Set the speed of the gripper. + * + * @param speed A value between 0x00 (stopped) and 0xFF (full speed). + */ + virtual void set_speed(uint8_t speed) = 0; + + /** + * @brief Set how forcefully the gripper opens or closes. + * @param force A value between 0x00 (no force) or 0xFF (maximum force). + */ + virtual void set_force(uint8_t force) = 0; +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/driver_exception.hpp b/robotiq_driver/include/robotiq_driver/driver_exception.hpp new file mode 100644 index 0000000..291c544 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/driver_exception.hpp @@ -0,0 +1,66 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include +#include +#include + +namespace robotiq_driver +{ +/** + * This is a custom exception thrown by the Driver. + */ +class DriverException : public std::exception +{ + std::string what_; + +public: + explicit DriverException(const std::string& description) + { + std::stringstream ss; + ss << "DriverException: " << description << "."; + what_ = ss.str(); + } + + DriverException(const DriverException& other) : what_(other.what_) + { + } + + ~DriverException() override = default; + + // Disable copy constructors + DriverException& operator=(const DriverException&) = delete; + + [[nodiscard]] const char* what() const throw() override + { + return what_.c_str(); + } +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/driver_factory.hpp b/robotiq_driver/include/robotiq_driver/driver_factory.hpp new file mode 100644 index 0000000..d06d96c --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/driver_factory.hpp @@ -0,0 +1,53 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include + +#include + +#include + +namespace robotiq_driver +{ +/** + * The hardware interface internally uses a factory to create and configure a + * driver to interact with the Robotiq Gripper. + * A factory is used to keep the code cleaner but also to simplify testing. + * With a factory, we can test that the parameters read by the hardware + * interface are correctly parsed and passed down to the driver. + * A factory can also be mocked to return different implementation of the + * Driver (or mocks) that do not require interaction with the real hardware. + */ +class DriverFactory +{ +public: + virtual std::unique_ptr create(const hardware_interface::HardwareInfo& info) const = 0; +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/fake/fake_driver.hpp b/robotiq_driver/include/robotiq_driver/fake/fake_driver.hpp new file mode 100644 index 0000000..ddb5b95 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/fake/fake_driver.hpp @@ -0,0 +1,70 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include + +namespace robotiq_driver +{ +/** + * This is a fake driver that can be used for testing interactions with the + * hardware interface or the controller without being connected to the real + * hardware. At the moment the fake driver is very basic but it can be + * improved to behave as close as possible to the real hardware. + * To use this driver you have to enable the following parameter in your + * hardware interface configuration in the robot URDF. + * + * + * true + */ +class FakeDriver : public Driver +{ +public: + void set_slave_address(uint8_t slave_address) override; + bool connect() override; + void disconnect() override; + void activate() override; + void deactivate() override; + void set_gripper_position(uint8_t position) override; + uint8_t get_gripper_position() override; + bool gripper_is_moving() override; + void set_speed(uint8_t speed) override; + void set_force(uint8_t force) override; + +private: + uint8_t slave_address_ = 0x00; + bool connected_ = false; + bool activated_ = false; + uint8_t position_ = 0; + bool gripper_is_moving_ = false; + uint8_t speed_ = 0; + uint8_t force_ = 0; +}; + +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp index 06fd8e3..f9a18b2 100644 --- a/robotiq_driver/include/robotiq_driver/hardware_interface.hpp +++ b/robotiq_driver/include/robotiq_driver/hardware_interface.hpp @@ -34,14 +34,18 @@ #include #include -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" -#include "rclcpp/macros.hpp" -#include "rclcpp/rclcpp.hpp" -#include "robotiq_driver/robotiq_gripper_interface.hpp" -#include "robotiq_driver/visibility_control.h" +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include namespace robotiq_driver { @@ -50,45 +54,100 @@ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterfa public: RCLCPP_SHARED_PTR_DEFINITIONS(RobotiqGripperHardwareInterface) + /** + * Default constructor. + */ ROBOTIQ_DRIVER_PUBLIC RobotiqGripperHardwareInterface(); ROBOTIQ_DRIVER_PUBLIC - CallbackReturn on_init(const hardware_interface::HardwareInfo & info) override; + ~RobotiqGripperHardwareInterface(); + + /** + * Constructor with a driver factory. This method is used for testing. + * @param driver_factory The driver that interact with the hardware. + */ + explicit RobotiqGripperHardwareInterface(std::unique_ptr driver_factory); + + /** + * Initialization of the hardware interface from data parsed from the + * robot's URDF. + * @param hardware_info Structure with data from URDF. + * @returns CallbackReturn::SUCCESS if required data are provided and can be + * parsed or CallbackReturn::ERROR if any error happens or data are missing. + */ + ROBOTIQ_DRIVER_PUBLIC + CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override; + + /** + * Connect to the hardware. + * @param previous_state The previous state. + * @returns CallbackReturn::SUCCESS if required data are provided and can be + * parsed or CallbackReturn::ERROR if any error happens or data are missing. + */ + ROBOTIQ_DRIVER_PUBLIC + CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + /** + * This method exposes position and velocity of joints for reading. + */ ROBOTIQ_DRIVER_PUBLIC std::vector export_state_interfaces() override; + /** + * This method exposes the joints targets for writing. + */ ROBOTIQ_DRIVER_PUBLIC std::vector export_command_interfaces() override; + /** + * This method is invoked when the hardware is connected. + * @param previous_state Unconfigured, Inactive, Active or Finalized. + * @returns CallbackReturn::SUCCESS or CallbackReturn::ERROR. + */ ROBOTIQ_DRIVER_PUBLIC - CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + /** + * This method is invoked when the hardware is disconnected. + * @param previous_state Unconfigured, Inactive, Active or Finalized. + * @returns CallbackReturn::SUCCESS or CallbackReturn::ERROR. + */ ROBOTIQ_DRIVER_PUBLIC - CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + /** + * Read data from the hardware. + */ ROBOTIQ_DRIVER_PUBLIC - hardware_interface::return_type read( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; + /** + * Write data to hardware. + */ ROBOTIQ_DRIVER_PUBLIC - hardware_interface::return_type write( - const rclcpp::Time & time, const rclcpp::Duration & period) override; + hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override; private: + // Interface to send binary data to the hardware using the serial port. + std::unique_ptr driver_; + + // Factory to create the driver during the initialization step. + std::unique_ptr driver_factory_; + + // We use a thread to read/write to the driver so that we dont block the hardware_interface read/write. + std::thread communication_thread_; + std::atomic communication_thread_is_running_; + void background_task(); + + double gripper_closed_pos_; + static constexpr double NO_NEW_CMD_ = std::numeric_limits::quiet_NaN(); double gripper_position_; double gripper_velocity_; double gripper_position_command_; - std::unique_ptr gripper_interface_; - - double gripper_closed_pos_; - std::string com_port_; - std::thread command_interface_; - std::atomic command_interface_is_running_; std::atomic write_command_; std::atomic gripper_current_state_; diff --git a/robotiq_driver/include/robotiq_driver/serial.hpp b/robotiq_driver/include/robotiq_driver/serial.hpp new file mode 100644 index 0000000..6d665db --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/serial.hpp @@ -0,0 +1,123 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include +#include +#include +#include + +namespace robotiq_driver +{ +/** + * The driver talks to the hardware through an implementation of the + * serial interface. + * We can mock the serial connection to check that a high level command + * (e.g activate) is converted into the correct sequence of bytes + * including CRC. + */ +class Serial +{ +public: + virtual ~Serial() = default; + + /** + * Opens the serial port as long as the port is set and the port isn't + * already open. + */ + virtual void open() = 0; + + /** + * Gets the open status of the serial port. + * @return Returns true if the port is open, false otherwise. + */ + [[nodiscard]] virtual bool is_open() const = 0; + + /** Closes the serial port. */ + virtual void close() = 0; + + /** + * Read a given amount of bytes from the serial port. + * @param size The number of bytes to read. + * @return The sequence of bytes. + * @throw serial::PortNotOpenedException + * @throw serial::SerialException + */ + [[nodiscard]] virtual std::vector read(size_t size) = 0; + + /** + * Write a sequence of bytes to the serial port. + * @param data A vector containing data to be written to the serial port. + * @throw serial::PortNotOpenedException + * @throw serial::SerialException + * @throw serial::IOException + */ + virtual void write(const std::vector& data) = 0; + + /** + * Sets the serial port identifier. + * @param port A const std::string reference containing the address of the + * serial port, which would be something like "COM1" on Windows and + * "/dev/ttyS0" on Linux. + * @throw std::invalid_argument + */ + virtual void set_port(const std::string& port) = 0; + + /** + * Gets the serial port identifier. + * @see SerialInterface::setPort + * @throw std::invalid_argument + */ + [[nodiscard]] virtual std::string get_port() const = 0; + + /** + * Set read timeout in milliseconds. + * @param timeout Read timeout in milliseconds. + */ + virtual void set_timeout(const std::chrono::milliseconds timeout) = 0; + + /** + * Get read timeout in milliseconds. + * @return Read timeout in milliseconds. + */ + [[nodiscard]] virtual std::chrono::milliseconds get_timeout() const = 0; + + /** + * Sets the baudrate for the serial port. + * @param baudrate An integer that sets the baud rate for the serial port. + */ + virtual void set_baudrate(uint32_t baudrate) = 0; + + /** + * Gets the baudrate for the serial port. + * @return An integer that sets the baud rate for the serial port. + */ + [[nodiscard]] virtual uint32_t get_baudrate() const = 0; +}; +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/serial_factory.hpp b/robotiq_driver/include/robotiq_driver/serial_factory.hpp new file mode 100644 index 0000000..8236215 --- /dev/null +++ b/robotiq_driver/include/robotiq_driver/serial_factory.hpp @@ -0,0 +1,55 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include + +#include + +#include + +namespace robotiq_driver +{ +/** + * This factory is used to create and configure a Serial interface + * implementation that is used by the driver to interact with the hardware. + * A factory is used to keep the code cleaner but also to simplify testing. + * With a factory, we can test that the parameters read by the hardware + * interface are correctly parsed and passed down to the serial interface. + * A factory can also be mocked to return different implementation of the + * serial interface (or mocks) that do not require interaction with the real + * hardware. + */ +class SerialFactory +{ +public: + virtual std::unique_ptr create(const hardware_interface::HardwareInfo& info) const = 0; +}; + +} // namespace robotiq_driver diff --git a/robotiq_driver/include/robotiq_driver/visibility_control.h b/robotiq_driver/include/robotiq_driver/visibility_control.hpp similarity index 93% rename from robotiq_driver/include/robotiq_driver/visibility_control.h rename to robotiq_driver/include/robotiq_driver/visibility_control.hpp index df5a56e..19918d3 100644 --- a/robotiq_driver/include/robotiq_driver/visibility_control.h +++ b/robotiq_driver/include/robotiq_driver/visibility_control.hpp @@ -19,8 +19,8 @@ * library cannot have, but the consuming code must have inorder to link. */ -#ifndef ROBOTIQ_DRIVER__VISIBILITY_CONTROL_H_ -#define ROBOTIQ_DRIVER__VISIBILITY_CONTROL_H_ +#ifndef ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_ +#define ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility @@ -53,4 +53,4 @@ #define ROBOTIQ_DRIVER_PUBLIC_TYPE #endif -#endif // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_H_ +#endif // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_ diff --git a/robotiq_driver/package.xml b/robotiq_driver/package.xml index 5004d57..4c5733d 100644 --- a/robotiq_driver/package.xml +++ b/robotiq_driver/package.xml @@ -12,17 +12,29 @@ ament_cmake hardware_interface + lifecycle_msgs pluginlib rclcpp rclcpp_lifecycle serial - gripper_controllers - joint_state_broadcaster - + + ament_clang_format + + ament_clang_tidy + + ament_cmake_copyright + + ament_cmake_lint_cmake + ament_lint_auto + ament_lint_common + + ament_cmake_gmock + ros2_control_test_assets + ament_cmake diff --git a/robotiq_driver/src/crc.cpp b/robotiq_driver/src/crc.cpp deleted file mode 100644 index f6a8563..0000000 --- a/robotiq_driver/src/crc.cpp +++ /dev/null @@ -1,81 +0,0 @@ -// Copyright (c) 2022 PickNik, Inc. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#include "robotiq_driver/crc.hpp" - -/* Table of CRC values for high–order byte */ -static unsigned char kCRCHiTable[] = { - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, - 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40}; - -/* Table of CRC values for low–order byte */ -static unsigned char kCRCLoTable[] = { - 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, - 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, - 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, - 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, - 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, - 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, - 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, - 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, - 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, - 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, - 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, - 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, - 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, - 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, - 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, - 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40}; - -uint16_t computeCRC(const std::vector & cmd) -{ - uint16_t crc_hi = 0x00FF; - uint16_t crc_lo = 0x00FF; - - for (auto byte : cmd) { - std::size_t index = crc_lo ^ byte; - crc_lo = crc_hi ^ kCRCHiTable[index]; - crc_hi = kCRCLoTable[index]; - } - - return (crc_lo << 8) + crc_hi; -} diff --git a/robotiq_driver/src/crc_utils.cpp b/robotiq_driver/src/crc_utils.cpp new file mode 100644 index 0000000..8cba7f8 --- /dev/null +++ b/robotiq_driver/src/crc_utils.cpp @@ -0,0 +1,131 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include + +namespace robotiq_driver::crc_utils +{ +/** + * Following are the tables for the CRC-16 MODBUS protocol. This particular CRC + * is commonly used in the Modbus RTU serial communications protocol, which is + * a de facto standard communication protocol and an ISO standard used by many + * industrial devices. + */ + +/* Table of CRC values for high–order byte */ + +// clang-format off +constexpr std::array kCRCHiTable = { + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40 +}; + +/* Table of CRC values for low–order byte */ +constexpr std::array kCRCLoTable = { + 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, + 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, + 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, + 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09, 0x08, 0xC8, + 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, + 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, + 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, + 0xD2, 0x12, 0x13, 0xD3, 0x11, 0xD1, 0xD0, 0x10, + 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, + 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4, + 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, + 0xFA, 0x3A, 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, + 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, + 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, + 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26, + 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, + 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, + 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, + 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F, 0x6E, 0xAE, + 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, + 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, + 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, + 0xB4, 0x74, 0x75, 0xB5, 0x77, 0xB7, 0xB6, 0x76, + 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, + 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92, + 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, + 0x9C, 0x5C, 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, + 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, + 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, + 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C, + 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, + 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80, 0x40 +}; +// clang-format on + +uint16_t compute_crc(const std::vector& cmd) +{ + uint16_t crc_hi = 0x00FF; + uint16_t crc_lo = 0x00FF; + + for (uint8_t byte : cmd) + { + uint8_t index = crc_lo ^ byte; + crc_lo = crc_hi ^ kCRCHiTable[index]; + crc_hi = kCRCLoTable[index]; + } + + return (crc_lo << 8) + crc_hi; +} +} // namespace robotiq_driver::crc_utils diff --git a/robotiq_driver/src/data_utils.cpp b/robotiq_driver/src/data_utils.cpp new file mode 100644 index 0000000..3f24af8 --- /dev/null +++ b/robotiq_driver/src/data_utils.cpp @@ -0,0 +1,99 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include + +#include + +namespace robotiq_driver::data_utils +{ +constexpr std::array vChars = { + '0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F' +}; + +std::string to_hex(const std::vector& bytes) +{ + std::string hex; + for (auto it = std::begin(bytes); it != std::end(bytes); ++it) + { + if (it != bytes.begin()) + { + hex += " "; + } + hex += ""; + uint8_t ch = *it; + hex += vChars[((ch >> 4) & 0xF)]; + hex += vChars[(ch & 0xF)]; + } + + return hex; +} + +std::string to_hex(const std::vector& bytes) +{ + std::string hex; + for (auto it = std::begin(bytes); it != std::end(bytes); ++it) + { + if (it != bytes.begin()) + { + hex += " "; + } + hex += ""; + uint16_t ch = *it; + hex += vChars[((ch >> 12) & 0xF)]; + hex += vChars[((ch >> 8) & 0xF)]; + hex += vChars[((ch >> 4) & 0xF)]; + hex += vChars[(ch & 0xF)]; + } + + return hex; +} + +std::string to_binary_string(const uint8_t byte) +{ + std::string result = ""; + for (int i = 7; i >= 0; --i) + { + result += ((byte >> i) & 1) ? '1' : '0'; + } + return result; +} + +uint8_t get_msb(uint16_t value) +{ + return static_cast(value >> 8) & 0xFF; +} + +uint8_t get_lsb(uint16_t value) +{ + return static_cast(value & 0xFF); +} + +} // namespace robotiq_driver::data_utils diff --git a/robotiq_driver/src/default_driver.cpp b/robotiq_driver/src/default_driver.cpp new file mode 100644 index 0000000..0824e89 --- /dev/null +++ b/robotiq_driver/src/default_driver.cpp @@ -0,0 +1,296 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include +#include +#include +#include + +#include "robotiq_driver/data_utils.hpp" +#include "robotiq_driver/default_driver.hpp" +#include +#include + +#include + +namespace robotiq_driver +{ +const auto kLogger = rclcpp::get_logger("DefaultDriver"); + +constexpr uint8_t kReadFunctionCode = 0x03; +constexpr uint16_t kFirstOutputRegister = 0x07D0; +constexpr uint16_t kNumOutputRegisters = 0x0006; + +// The response to a read request consists of: +// slave ID (1 byte) +// function code (1 byte) +// number of data bytes (1 byte) +// data bytes (2 bytes per register) +// CRC (2 bytes) +constexpr int kReadResponseSize = 2 * kNumOutputRegisters + 5; + +constexpr uint8_t kWriteFunctionCode = 0x10; +constexpr uint16_t kActionRequestRegister = 0x03E8; + +// The response to a write command consists of: +// slave ID (1 byte) +// function code (1 byte) +// address of the first register that was written (2 bytes) +// number of registers written (2 bytes) +// CRC (2 bytes) +constexpr int kWriteResponseSize = 8; + +constexpr size_t kResponseHeaderSize = 3; +constexpr size_t kGripperStatusIndex = 0; +constexpr size_t kPositionIndex = 4; + +// If the gripper connection is not stable we may want to try sending the command again. +constexpr auto kMaxRetries = 5; + +DefaultDriver::DefaultDriver(std::unique_ptr serial) + : serial_{ std::move(serial) }, commanded_gripper_speed_(0x80), commanded_gripper_force_(0x80) +{ +} + +std::vector DefaultDriver::send(const std::vector& request, size_t response_size) const +{ + std::vector response; + response.reserve(response_size); + + int retry_count = 0; + while (retry_count < kMaxRetries) + { + try + { + serial_->write(request); + response = serial_->read(response_size); + break; + } + catch (const serial::IOException& e) + { + RCLCPP_WARN(kLogger, "Resending the command because the previous attempt (%d of %d) failed: %s", retry_count + 1, + kMaxRetries, e.what()); + retry_count++; + } + } + + if (retry_count == kMaxRetries) + { + RCLCPP_ERROR(kLogger, "Reached maximum retries. Operation failed."); + return {}; + } + + return response; +} + +bool DefaultDriver::connect() +{ + serial_->open(); + return serial_->is_open(); +} + +void DefaultDriver::disconnect() +{ + serial_->close(); +} + +void DefaultDriver::set_slave_address(uint8_t slave_address) +{ + slave_address_ = slave_address; +} + +void DefaultDriver::activate() +{ + RCLCPP_INFO(kLogger, "Activate..."); + + // set rACT to 1, clear all other registers. + const auto request = create_write_command(kActionRequestRegister, { 0x0100, 0x0000, 0x0000 }); + auto response = send(request, kWriteResponseSize); + if (response.empty()) + { + throw DriverException{ "Failed to activate the gripper." }; + } + + update_status(); + if (gripper_status_ == GripperStatus::COMPLETED) + { + return; + } + while (gripper_status_ == GripperStatus::IN_PROGRESS) + { + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + update_status(); + } +} + +void DefaultDriver::deactivate() +{ + RCLCPP_INFO(kLogger, "Deactivate..."); + + const auto request = create_write_command(kActionRequestRegister, { 0x0000, 0x0000, 0x0000 }); + auto response = send(request, kWriteResponseSize); + if (response.empty()) + { + throw DriverException{ "Failed to deactivate the gripper." }; + } +} + +void DefaultDriver::set_gripper_position(uint8_t pos) +{ + uint8_t action_register = 0x09; + uint8_t gripper_options_1 = 0x00; + uint8_t gripper_options_2 = 0x00; + + const auto request = + create_write_command(kActionRequestRegister, + { uint16_t(action_register << 8 | gripper_options_1), uint16_t(gripper_options_2 << 8 | pos), + uint16_t(commanded_gripper_speed_ << 8 | commanded_gripper_force_) }); + + auto response = send(request, kWriteResponseSize); + if (response.empty()) + { + throw DriverException{ "Failed to set gripper position." }; + } +} + +uint8_t DefaultDriver::get_gripper_position() +{ + update_status(); + return gripper_position_; +} + +bool DefaultDriver::gripper_is_moving() +{ + update_status(); + return object_detection_status_ == ObjectDetectionStatus::MOVING; +} + +void DefaultDriver::set_speed(uint8_t speed) +{ + commanded_gripper_speed_ = speed; +} + +void DefaultDriver::set_force(uint8_t force) +{ + commanded_gripper_force_ = force; +} + +std::vector DefaultDriver::create_read_command(uint16_t first_register, uint8_t num_registers) +{ + std::vector request = { slave_address_, + kReadFunctionCode, + data_utils::get_msb(first_register), + data_utils::get_lsb(first_register), + data_utils::get_msb(num_registers), + data_utils::get_lsb(num_registers) }; + auto crc = crc_utils::compute_crc(request); + request.push_back(data_utils::get_msb(crc)); + request.push_back(data_utils::get_lsb(crc)); + return request; +} + +std::vector DefaultDriver::create_write_command(uint16_t first_register, const std::vector& data) +{ + uint16_t num_registers = data.size(); + uint8_t num_bytes = 2 * num_registers; + + std::vector request = { slave_address_, + kWriteFunctionCode, + data_utils::get_msb(first_register), + data_utils::get_lsb(first_register), + data_utils::get_msb(num_registers), + data_utils::get_lsb(num_registers), + num_bytes }; + for (auto d : data) + { + request.push_back(data_utils::get_msb(d)); + request.push_back(data_utils::get_lsb(d)); + } + + auto crc = crc_utils::compute_crc(request); + request.push_back(data_utils::get_msb(crc)); + request.push_back(data_utils::get_lsb(crc)); + + return request; +} + +void DefaultDriver::update_status() +{ + const auto request = create_read_command(kFirstOutputRegister, kNumOutputRegisters); + auto response = send(request, kReadResponseSize); + if (response.empty()) + { + throw DriverException{ "Failed to read the gripper status." }; + } + + // Process the response. + uint8_t gripper_status_byte = response[kResponseHeaderSize + kGripperStatusIndex]; + + // Activation status. + activation_status_ = ((gripper_status_byte & 0x01) == 0x00) ? ActivationStatus::RESET : ActivationStatus::ACTIVE; + + // Action status. + action_status_ = ((gripper_status_byte & 0x08) == 0x00) ? ActionStatus::STOPPED : ActionStatus::MOVING; + + // Gripper status. + switch ((gripper_status_byte & 0x30) >> 4) + { + case 0x00: + gripper_status_ = GripperStatus::RESET; + break; + case 0x01: + gripper_status_ = GripperStatus::IN_PROGRESS; + break; + case 0x03: + gripper_status_ = GripperStatus::COMPLETED; + break; + } + + // Object detection status. + switch ((gripper_status_byte & 0xC0) >> 6) + { + case 0x00: + object_detection_status_ = ObjectDetectionStatus::MOVING; + break; + case 0x01: + object_detection_status_ = ObjectDetectionStatus::OBJECT_DETECTED_OPENING; + break; + case 0x02: + object_detection_status_ = ObjectDetectionStatus::OBJECT_DETECTED_CLOSING; + break; + case 0x03: + object_detection_status_ = ObjectDetectionStatus::AT_REQUESTED_POSITION; + break; + } + + // Read the current gripper position. + gripper_position_ = response[kResponseHeaderSize + kPositionIndex]; +} +} // namespace robotiq_driver diff --git a/robotiq_driver/src/default_driver_factory.cpp b/robotiq_driver/src/default_driver_factory.cpp new file mode 100644 index 0000000..05d66b5 --- /dev/null +++ b/robotiq_driver/src/default_driver_factory.cpp @@ -0,0 +1,100 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include +#include +#include +#include +#include + +#include + +namespace robotiq_driver +{ +const auto kLogger = rclcpp::get_logger("DefaultDriverFactory"); + +constexpr auto kSlaveAddressParamName = "slave_address"; +constexpr uint8_t kSlaveAddressParamDefault = 0x09; + +constexpr auto kGripperSpeedMultiplierParamName = "gripper_speed_multiplier"; +constexpr double kGripperSpeedMultiplierParamDefault = 1.0; + +constexpr auto kGripperForceMultiplierParamName = "gripper_force_multiplier"; +constexpr double kGripperForceMultiplierParamDefault = 1.0; + +constexpr auto kUseDummyParamName = "use_dummy"; +constexpr auto kUseDummyParamDefault = "false"; + +std::unique_ptr DefaultDriverFactory::create(const hardware_interface::HardwareInfo& info) const +{ + RCLCPP_INFO(kLogger, "Reading %s...", kSlaveAddressParamName); + // Convert base-16 address stored as a string (for example, "0x9") into an integer + const uint8_t slave_address = + info.hardware_parameters.count(kSlaveAddressParamName) ? + static_cast(std::stoul(info.hardware_parameters.at(kSlaveAddressParamName), nullptr, 16)) : + kSlaveAddressParamDefault; + RCLCPP_INFO(kLogger, "%s: %d", kSlaveAddressParamName, slave_address); + + RCLCPP_INFO(kLogger, "Reading %s...", kGripperSpeedMultiplierParamName); + double gripper_speed = info.hardware_parameters.count(kGripperSpeedMultiplierParamName) ? + std::clamp(stod(info.hardware_parameters.at(kGripperSpeedMultiplierParamName)), 0.0, 1.0) : + kGripperSpeedMultiplierParamDefault; + RCLCPP_INFO(kLogger, "%s: %fs", kGripperSpeedMultiplierParamName, gripper_speed); + + RCLCPP_INFO(kLogger, "Reading %s...", kGripperForceMultiplierParamName); + double gripper_force = info.hardware_parameters.count(kGripperForceMultiplierParamName) ? + std::clamp(stod(info.hardware_parameters.at(kGripperForceMultiplierParamName)), 0.0, 1.0) : + kGripperForceMultiplierParamDefault; + RCLCPP_INFO(kLogger, "%s: %fs", kGripperForceMultiplierParamName, gripper_force); + + auto driver = create_driver(info); + driver->set_slave_address(slave_address); + driver->set_speed(gripper_speed * 0xFF); + driver->set_force(gripper_force * 0xFF); + + return driver; +} + +std::unique_ptr DefaultDriverFactory::create_driver(const hardware_interface::HardwareInfo& info) const +{ + // We give the user an option to startup a dummy gripper for testing purposes. + if (info.hardware_parameters.count(kUseDummyParamName) && + info.hardware_parameters.at(kUseDummyParamName) != kUseDummyParamDefault) + { + RCLCPP_INFO(kLogger, "You are connected to a dummy driver, not a real hardware."); + return std::make_unique(); + } + else + { + auto serial = DefaultSerialFactory().create(info); + return std::make_unique(std::move(serial)); + } +} +} // namespace robotiq_driver diff --git a/robotiq_driver/src/default_serial.cpp b/robotiq_driver/src/default_serial.cpp new file mode 100644 index 0000000..0f3e976 --- /dev/null +++ b/robotiq_driver/src/default_serial.cpp @@ -0,0 +1,111 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include + +namespace robotiq_driver +{ + +DefaultSerial::DefaultSerial() : serial_{ std::make_unique() } +{ +} + +void DefaultSerial::open() +{ + serial_->open(); +} + +bool DefaultSerial::is_open() const +{ + return serial_->isOpen(); +} + +void DefaultSerial::close() +{ + serial_->close(); +} + +std::vector DefaultSerial::read(size_t size) +{ + std::vector data; + size_t bytes_read = serial_->read(data, size); + if (bytes_read != size) + { + const auto error_msg = "Requested " + std::to_string(size) + " bytes, but got " + std::to_string(bytes_read); + THROW(serial::IOException, error_msg.c_str()); + } + return data; +} + +void DefaultSerial::write(const std::vector& data) +{ + std::size_t num_bytes_written = serial_->write(data); + serial_->flush(); + if (num_bytes_written != data.size()) + { + const auto error_msg = + "Attempted to write " + std::to_string(data.size()) + " bytes, but wrote " + std::to_string(num_bytes_written); + THROW(serial::IOException, error_msg.c_str()); + } +} + +void DefaultSerial::set_port(const std::string& port) +{ + serial_->setPort(port); +} + +std::string DefaultSerial::get_port() const +{ + return serial_->getPort(); +} + +void DefaultSerial::set_timeout(std::chrono::milliseconds timeout) +{ + serial::Timeout simple_timeout = serial::Timeout::simpleTimeout(static_cast(timeout.count())); + serial_->setTimeout(simple_timeout); +} + +std::chrono::milliseconds DefaultSerial::get_timeout() const +{ + uint32_t timeout = serial_->getTimeout().read_timeout_constant; + return std::chrono::milliseconds{ timeout }; +} + +void DefaultSerial::set_baudrate(uint32_t baudrate) +{ + serial_->setBaudrate(baudrate); +} + +uint32_t DefaultSerial::get_baudrate() const +{ + return serial_->getBaudrate(); +} + +} // namespace robotiq_driver diff --git a/robotiq_driver/src/default_serial_factory.cpp b/robotiq_driver/src/default_serial_factory.cpp new file mode 100644 index 0000000..598805e --- /dev/null +++ b/robotiq_driver/src/default_serial_factory.cpp @@ -0,0 +1,80 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include + +#include + +namespace robotiq_driver +{ + +const auto kLogger = rclcpp::get_logger("DefaultSerialFactory"); + +constexpr auto kUsbPortParamName = "COM_port"; +constexpr auto kUsbPortParamDefault = "/dev/ttyUSB0"; + +constexpr auto kBaudrateParamName = "baudrate"; +constexpr auto kBaudrateAddressParamDefault = 115200; + +constexpr auto kTimeoutParamName = "timeout"; +constexpr auto kTimeoutParamDefault = 0.5; + +std::unique_ptr DefaultSerialFactory::create(const hardware_interface::HardwareInfo& info) const +{ + RCLCPP_INFO(kLogger, "Reading %s...", kUsbPortParamName); + std::string usb_port = info.hardware_parameters.count(kUsbPortParamName) ? + info.hardware_parameters.at(kUsbPortParamName) : + kUsbPortParamDefault; + RCLCPP_INFO(kLogger, "%s: %s", kUsbPortParamName, usb_port.c_str()); + + RCLCPP_INFO(kLogger, "Reading %s...", kBaudrateParamName); + uint32_t baudrate = info.hardware_parameters.count(kBaudrateParamName) ? + static_cast(std::stoul(info.hardware_parameters.at(kBaudrateParamName))) : + kBaudrateAddressParamDefault; + RCLCPP_INFO(kLogger, "%s: %dbps", kBaudrateParamName, baudrate); + + RCLCPP_INFO(kLogger, "Reading %s...", kTimeoutParamName); + double timeout = info.hardware_parameters.count(kTimeoutParamName) ? + std::stod(info.hardware_parameters.at(kTimeoutParamName)) : + kTimeoutParamDefault; + RCLCPP_INFO(kLogger, "%s: %fs", kTimeoutParamName, timeout); + + auto serial = create_serial(); + serial->set_port(usb_port); + serial->set_baudrate(baudrate); + serial->set_timeout(std::chrono::duration_cast(std::chrono::duration(timeout))); + return serial; +} + +std::unique_ptr DefaultSerialFactory::create_serial() const +{ + return std::make_unique(); +} + +} // namespace robotiq_driver diff --git a/robotiq_driver/src/fake/fake_driver.cpp b/robotiq_driver/src/fake/fake_driver.cpp new file mode 100644 index 0000000..1dc87e9 --- /dev/null +++ b/robotiq_driver/src/fake/fake_driver.cpp @@ -0,0 +1,95 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include + +namespace robotiq_driver +{ +const auto kLogger = rclcpp::get_logger("FakeDriver"); + +void FakeDriver::set_slave_address(uint8_t slave_address) +{ + slave_address_ = slave_address; + RCLCPP_INFO(kLogger, "slave_address set to: %d", slave_address); +} + +bool FakeDriver::connect() +{ + connected_ = true; + RCLCPP_INFO(kLogger, "Gripper connected."); + return true; +} + +void FakeDriver::disconnect() +{ + RCLCPP_INFO(kLogger, "Gripper disconnected."); + connected_ = false; +} + +void FakeDriver::activate() +{ + RCLCPP_INFO(kLogger, "Gripper activated."); + activated_ = true; +} + +void FakeDriver::deactivate() +{ + RCLCPP_INFO(kLogger, "Gripper deactivated."); + activated_ = false; +} + +void FakeDriver::set_gripper_position(uint8_t position) +{ + position_ = position; +} + +uint8_t FakeDriver::get_gripper_position() +{ + return position_; +} + +bool FakeDriver::gripper_is_moving() +{ + return gripper_is_moving_; +} + +void FakeDriver::set_speed(uint8_t speed) +{ + RCLCPP_INFO(kLogger, "Set gripper speed."); + speed_ = speed; +} + +void FakeDriver::set_force(uint8_t force) +{ + RCLCPP_INFO(kLogger, "Set gripper force."); + force_ = force; +} + +} // namespace robotiq_driver diff --git a/robotiq_driver/src/gripper_interface_test.cpp b/robotiq_driver/src/gripper_interface_test.cpp deleted file mode 100644 index c93a8de..0000000 --- a/robotiq_driver/src/gripper_interface_test.cpp +++ /dev/null @@ -1,97 +0,0 @@ -// Copyright (c) 2022 PickNik, Inc. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#include -#include - -#include "robotiq_driver/robotiq_gripper_interface.hpp" - -constexpr auto kComPort = "/dev/ttyUSB0"; -constexpr auto kSlaveID = 0x09; - -int main() -{ - try { - RobotiqGripperInterface gripper(kComPort, kSlaveID); - - std::cout << "Deactivating gripper...\n"; - gripper.deactivateGripper(); - - std::cout << "Activating gripper...\n"; - gripper.activateGripper(); - - std::cout << "Gripper successfully activated.\n"; - - std::cout << "Closing gripper...\n"; - gripper.setGripperPosition(0xFF); - while (gripper.gripperIsMoving()) { - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - } - - std::cout << "Opening gripper...\n"; - gripper.setGripperPosition(0x00); - while (gripper.gripperIsMoving()) { - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - } - - std::cout << "Half closing gripper...\n"; - gripper.setGripperPosition(0x80); - while (gripper.gripperIsMoving()) { - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - } - - std::cout << "Opening gripper...\n"; - gripper.setGripperPosition(0x00); - while (gripper.gripperIsMoving()) { - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - } - - std::cout << "Decreasing gripper speed...\n"; - gripper.setSpeed(0x0F); - - std::cout << "Closing gripper...\n"; - gripper.setGripperPosition(0xFF); - while (gripper.gripperIsMoving()) { - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - } - - std::cout << "Increasing gripper speed...\n"; - gripper.setSpeed(0xFF); - - std::cout << "Opening gripper...\n"; - gripper.setGripperPosition(0x00); - while (gripper.gripperIsMoving()) { - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - } - } catch (const serial::IOException & e) { - std::cout << "Failed to communicating with the Gripper. Please check the Gripper connection"; - return 1; - } - - return 0; -} diff --git a/robotiq_driver/src/hardware_interface.cpp b/robotiq_driver/src/hardware_interface.cpp index 0f9aa8b..2a293cb 100644 --- a/robotiq_driver/src/hardware_interface.cpp +++ b/robotiq_driver/src/hardware_interface.cpp @@ -26,46 +26,61 @@ // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE // POSSIBILITY OF SUCH DAMAGE. -#include "robotiq_driver/hardware_interface.hpp" - -#include - #include #include #include #include #include -#include "hardware_interface/actuator_interface.hpp" -#include "hardware_interface/types/hardware_interface_type_values.hpp" -#include "rclcpp/rclcpp.hpp" +#include +#include + +#include +#include + +#include + +const auto kLogger = rclcpp::get_logger("RobotiqGripperHardwareInterface"); constexpr uint8_t kGripperMinPos = 3; constexpr uint8_t kGripperMaxPos = 230; constexpr uint8_t kGripperRange = kGripperMaxPos - kGripperMinPos; -const auto kLogger = rclcpp::get_logger("RobotiqGripperHardwareInterface"); +constexpr auto kGripperCommsLoopPeriod = std::chrono::milliseconds{ 10 }; namespace robotiq_driver { -RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface() {} +RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface() +{ + driver_factory_ = std::make_unique(); +} + +RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface() +{ + communication_thread_is_running_.store(false); + if (communication_thread_.joinable()) + { + communication_thread_.join(); + } +} + +// This constructor is use for testing only. +RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface(std::unique_ptr driver_factory) + : driver_factory_{ std::move(driver_factory) } +{ +} -hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init( - const hardware_interface::HardwareInfo & info) +hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info) { - if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { + RCLCPP_DEBUG(kLogger, "on_init"); + + if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) + { return CallbackReturn::ERROR; } // Read parameters. gripper_closed_pos_ = stod(info_.hardware_parameters["gripper_closed_position"]); - com_port_ = info_.hardware_parameters["COM_port"]; - double gripper_speed = stod(info_.hardware_parameters["gripper_speed_multiplier"]); - double gripper_force = stod(info_.hardware_parameters["gripper_force_multiplier"]); - - // Speed and force must lie between 0.0 and 1.0. - gripper_speed = std::min(1.0, std::max(0.0, gripper_speed)); - gripper_force = std::min(1.0, std::max(0.0, gripper_force)); gripper_position_ = std::numeric_limits::quiet_NaN(); gripper_velocity_ = std::numeric_limits::quiet_NaN(); @@ -73,172 +88,184 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init( reactivate_gripper_cmd_ = NO_NEW_CMD_; reactivate_gripper_async_cmd_.store(false); - const hardware_interface::ComponentInfo & joint = info_.joints[0]; + const hardware_interface::ComponentInfo& joint = info_.joints[0]; // There is one command interface: position. - if (joint.command_interfaces.size() != 1) { - RCLCPP_FATAL( - kLogger, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), - joint.command_interfaces.size()); + if (joint.command_interfaces.size() != 1) + { + RCLCPP_FATAL(kLogger, "Joint '%s' has %zu command interfaces found. 1 expected.", joint.name.c_str(), + joint.command_interfaces.size()); return CallbackReturn::ERROR; } - if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) { - RCLCPP_FATAL( - kLogger, "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(), - joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); + if (joint.command_interfaces[0].name != hardware_interface::HW_IF_POSITION) + { + RCLCPP_FATAL(kLogger, "Joint '%s' has %s command interfaces found. '%s' expected.", joint.name.c_str(), + joint.command_interfaces[0].name.c_str(), hardware_interface::HW_IF_POSITION); return CallbackReturn::ERROR; } // There are two state interfaces: position and velocity. - if (joint.state_interfaces.size() != 2) { - RCLCPP_FATAL( - kLogger, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), - joint.state_interfaces.size()); + if (joint.state_interfaces.size() != 2) + { + RCLCPP_FATAL(kLogger, "Joint '%s' has %zu state interface. 2 expected.", joint.name.c_str(), + joint.state_interfaces.size()); return CallbackReturn::ERROR; } - for (int i = 0; i < 2; ++i) { + for (int i = 0; i < 2; ++i) + { if (!(joint.state_interfaces[i].name == hardware_interface::HW_IF_POSITION || - joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY)) { - RCLCPP_FATAL( - kLogger, "Joint '%s' has %s state interface. Expected %s or %s.", joint.name.c_str(), - joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION, - hardware_interface::HW_IF_VELOCITY); + joint.state_interfaces[i].name == hardware_interface::HW_IF_VELOCITY)) + { + RCLCPP_FATAL(kLogger, "Joint '%s' has %s state interface. Expected %s or %s.", joint.name.c_str(), + joint.state_interfaces[i].name.c_str(), hardware_interface::HW_IF_POSITION, + hardware_interface::HW_IF_VELOCITY); return CallbackReturn::ERROR; } } - try { - // Create the interface to the gripper. - gripper_interface_ = std::make_unique(com_port_); - gripper_interface_->setSpeed(gripper_speed * 0xFF); - gripper_interface_->setForce(gripper_force * 0xFF); - } catch (const serial::IOException & e) { - RCLCPP_FATAL(kLogger, "Failed to open gripper port."); + try + { + driver_ = driver_factory_->create(info_); + } + catch (const std::exception& e) + { + RCLCPP_FATAL(kLogger, "Failed to create a driver: %s", e.what()); return CallbackReturn::ERROR; } return CallbackReturn::SUCCESS; } -std::vector -RobotiqGripperHardwareInterface::export_state_interfaces() +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state) { + RCLCPP_DEBUG(kLogger, "on_configure"); + try + { + if (hardware_interface::SystemInterface::on_configure(previous_state) != CallbackReturn::SUCCESS) + { + return CallbackReturn::ERROR; + } + + // Open the serial port and handshake. + bool connected = driver_->connect(); + if (!connected) + { + RCLCPP_ERROR(kLogger, "Cannot connect to the Robotiq gripper"); + return CallbackReturn::ERROR; + } + } + catch (const std::exception& e) + { + RCLCPP_ERROR(kLogger, "Cannot configure the Robotiq gripper: %s", e.what()); + return CallbackReturn::ERROR; + } + return CallbackReturn::SUCCESS; +} + +std::vector RobotiqGripperHardwareInterface::export_state_interfaces() +{ + RCLCPP_DEBUG(kLogger, "export_state_interfaces"); + std::vector state_interfaces; - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_)); return state_interfaces; } -std::vector -RobotiqGripperHardwareInterface::export_command_interfaces() +std::vector RobotiqGripperHardwareInterface::export_command_interfaces() { + RCLCPP_DEBUG(kLogger, "export_command_interfaces"); + std::vector command_interfaces; command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_)); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - "reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_)); + info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_command_)); + command_interfaces.emplace_back( + hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_cmd", &reactivate_gripper_cmd_)); command_interfaces.emplace_back(hardware_interface::CommandInterface( - "reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_)); + "reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_)); return command_interfaces; } -hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) +hardware_interface::CallbackReturn +RobotiqGripperHardwareInterface::on_activate(const rclcpp_lifecycle::State& /*previous_state*/) { + RCLCPP_DEBUG(kLogger, "on_activate"); + // set some default values for joints - if (std::isnan(gripper_position_)) { + if (std::isnan(gripper_position_)) + { gripper_position_ = 0; gripper_velocity_ = 0; gripper_position_command_ = 0; } // Activate the gripper. - try { - gripper_interface_->deactivateGripper(); - gripper_interface_->activateGripper(); - } catch (const serial::IOException & e) { - RCLCPP_FATAL(kLogger, "Failed to communicate with Gripper. Check Gripper connection."); + try + { + driver_->deactivate(); + driver_->activate(); + + communication_thread_is_running_.store(true); + communication_thread_ = std::thread([this] { this->background_task(); }); + } + catch (const std::exception& e) + { + RCLCPP_FATAL(kLogger, "Failed to communicate with the Robotiq gripper: %s", e.what()); return CallbackReturn::ERROR; } RCLCPP_INFO(kLogger, "Robotiq Gripper successfully activated!"); - - command_interface_is_running_.store(true); - - command_interface_ = std::thread([this] { - // Read from and write to the gripper at 100 Hz. - const auto io_interval = std::chrono::milliseconds(10); - auto last_io = std::chrono::high_resolution_clock::now(); - - while (command_interface_is_running_.load()) { - const auto now = std::chrono::high_resolution_clock::now(); - if (now - last_io > io_interval) { - try { - // Re-activate the gripper - // (this can be used, for example, to re-run the auto-calibration). - if (reactivate_gripper_async_cmd_.load()) { - this->gripper_interface_->deactivateGripper(); - this->gripper_interface_->activateGripper(); - reactivate_gripper_async_cmd_.store(false); - reactivate_gripper_async_response_.store(true); - } - - // Write the latest command to the gripper. - this->gripper_interface_->setGripperPosition(write_command_.load()); - - // Read the state of the gripper. - gripper_current_state_.store(this->gripper_interface_->getGripperPosition()); - - last_io = now; - } catch (serial::IOException & e) { - RCLCPP_ERROR( - kLogger, "Check Robotiq Gripper connection and restart drivers. ERROR: %s", e.what()); - command_interface_is_running_.store(false); - } - } - } - }); - return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) +hardware_interface::CallbackReturn +RobotiqGripperHardwareInterface::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) { - command_interface_is_running_.store(false); - command_interface_.join(); + RCLCPP_DEBUG(kLogger, "on_deactivate"); - try { - gripper_interface_->deactivateGripper(); - } catch (const std::exception & e) { - RCLCPP_ERROR(kLogger, "Failed to deactivate gripper. Check Gripper connection"); - return CallbackReturn::ERROR; + communication_thread_is_running_.store(false); + communication_thread_.join(); + if (communication_thread_.joinable()) + { + communication_thread_.join(); } + try + { + driver_->deactivate(); + } + catch (const std::exception& e) + { + RCLCPP_ERROR(kLogger, "Failed to deactivate the Robotiq gripper: %s", e.what()); + return CallbackReturn::ERROR; + } + RCLCPP_INFO(kLogger, "Robotiq Gripper successfully deactivated!"); return CallbackReturn::SUCCESS; } -hardware_interface::return_type RobotiqGripperHardwareInterface::read( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { - gripper_position_ = - gripper_closed_pos_ * (gripper_current_state_.load() - kGripperMinPos) / kGripperRange; + gripper_position_ = gripper_closed_pos_ * (gripper_current_state_.load() - kGripperMinPos) / kGripperRange; - if (!std::isnan(reactivate_gripper_cmd_)) { + if (!std::isnan(reactivate_gripper_cmd_)) + { RCLCPP_INFO(kLogger, "Sending gripper reactivation request."); reactivate_gripper_async_cmd_.store(true); reactivate_gripper_cmd_ = NO_NEW_CMD_; } - if (reactivate_gripper_async_response_.load().has_value()) { + if (reactivate_gripper_async_response_.load().has_value()) + { reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value(); reactivate_gripper_async_response_.store(std::nullopt); } @@ -246,20 +273,49 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::read( return hardware_interface::return_type::OK; } -hardware_interface::return_type RobotiqGripperHardwareInterface::write( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) +hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) { - double gripper_pos = - (gripper_position_command_ / gripper_closed_pos_) * kGripperRange + kGripperMinPos; + double gripper_pos = (gripper_position_command_ / gripper_closed_pos_) * kGripperRange + kGripperMinPos; gripper_pos = std::max(std::min(gripper_pos, 255.0), 0.0); write_command_.store(uint8_t(gripper_pos)); return hardware_interface::return_type::OK; } +void RobotiqGripperHardwareInterface::background_task() +{ + while (communication_thread_is_running_.load()) + { + try + { + // Re-activate the gripper + // (this can be used, for example, to re-run the auto-calibration). + if (reactivate_gripper_async_cmd_.load()) + { + this->driver_->deactivate(); + this->driver_->activate(); + reactivate_gripper_async_cmd_.store(false); + reactivate_gripper_async_response_.store(true); + } + + // Write the latest command to the gripper. + this->driver_->set_gripper_position(write_command_.load()); + + // Read the state of the gripper. + gripper_current_state_.store(this->driver_->get_gripper_position()); + } + catch (std::exception& e) + { + RCLCPP_ERROR(kLogger, "Error: %s", e.what()); + } + + std::this_thread::sleep_for(kGripperCommsLoopPeriod); + } +} + } // namespace robotiq_driver #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS( - robotiq_driver::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(robotiq_driver::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface) diff --git a/robotiq_driver/src/robotiq_gripper_interface.cpp b/robotiq_driver/src/robotiq_gripper_interface.cpp deleted file mode 100644 index 58b6d24..0000000 --- a/robotiq_driver/src/robotiq_gripper_interface.cpp +++ /dev/null @@ -1,308 +0,0 @@ -// Copyright (c) 2022 PickNik, Inc. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#include "robotiq_driver/robotiq_gripper_interface.hpp" - -#include -#include -#include -#include - -#include "robotiq_driver/crc.hpp" - -constexpr int kBaudRate = 115200; -constexpr auto kTimeoutMilliseconds = 1000; - -constexpr uint8_t kReadFunctionCode = 0x03; -constexpr uint16_t kFirstOutputRegister = 0x07D0; -constexpr uint16_t kNumOutputRegisters = 0x0006; -// The response to a read request consists of: -// slave ID (1 byte) -// function code (1 byte) -// number of data bytes (1 byte) -// data bytes (2 bytes per register) -// CRC (2 bytes) -constexpr int kReadResponseSize = 2 * kNumOutputRegisters + 5; - -constexpr uint8_t kWriteFunctionCode = 0x10; -constexpr uint16_t kActionRequestRegister = 0x03E8; -// The response to a write command consists of: -// slave ID (1 byte) -// function code (1 byte) -// address of the first register that was written (2 bytes) -// number of registers written (2 bytes) -// CRC (2 bytes) -constexpr int kWriteResponseSize = 8; - -constexpr size_t kResponseHeaderSize = 3; -constexpr size_t kGripperStatusIndex = 0; -constexpr size_t kPositionIndex = 4; - -static uint8_t getFirstByte(uint16_t val) { return (val & 0xFF00) >> 8; } - -static uint8_t getSecondByte(uint16_t val) { return val & 0x00FF; } - -RobotiqGripperInterface::RobotiqGripperInterface(const std::string & com_port, uint8_t slave_id) -: port_(com_port, kBaudRate, serial::Timeout::simpleTimeout(kTimeoutMilliseconds)), - slave_id_(slave_id), - read_command_(createReadCommand(kFirstOutputRegister, kNumOutputRegisters)), - commanded_gripper_speed_(0x80), - commanded_gripper_force_(0x80) -{ - if (!port_.isOpen()) { - THROW(serial::IOException, "Failed to open gripper port."); - } -} - -void RobotiqGripperInterface::activateGripper() -{ - const auto cmd = createWriteCommand( - kActionRequestRegister, {0x0100, 0x0000, 0x0000} // set rACT to 1, clear all - // other registers. - ); - - try { - sendCommand(cmd); - readResponse(kWriteResponseSize); - - updateStatus(); - - if (gripper_status_ == GripperStatus::COMPLETED) { - return; - } - - while (gripper_status_ == GripperStatus::IN_PROGRESS) { - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - updateStatus(); - } - } catch (const serial::IOException & e) { - // catch connection error and rethrow - std::cerr << "Failed to activate gripper"; - throw; - } -} - -void RobotiqGripperInterface::deactivateGripper() -{ - const auto cmd = createWriteCommand(kActionRequestRegister, {0x0000, 0x0000, 0x0000}); - try { - sendCommand(cmd); - readResponse(kWriteResponseSize); - } catch (const serial::IOException & e) { - // catch connection error and rethrow - std::cerr << "Failed to activate gripper"; - throw; - } -} - -void RobotiqGripperInterface::setGripperPosition(uint8_t pos) -{ - uint8_t action_register = 0x09; - uint8_t gripper_options_1 = 0x00; - uint8_t gripper_options_2 = 0x00; - - const auto cmd = createWriteCommand( - kActionRequestRegister, - {uint16_t(action_register << 8 | gripper_options_1), uint16_t(gripper_options_2 << 8 | pos), - uint16_t(commanded_gripper_speed_ << 8 | commanded_gripper_force_)}); - try { - sendCommand(cmd); - readResponse(kWriteResponseSize); - } catch (const serial::IOException & e) { - // catch connection error and rethrow - std::cerr << "Failed to set gripper position\n"; - if (port_.isOpen()) { - std::cerr << "Error caught while reading or writing to device. Connection is open, " - "continuing to attempt " - "communication with gripper.\n ERROR: " - << e.what(); - return; - } - throw; - } -} - -uint8_t RobotiqGripperInterface::getGripperPosition() -{ - try { - updateStatus(); - } catch (const serial::IOException & e) { - // catch connection error and rethrow - std::cerr << "Failed to get gripper position\n"; - throw; - } - - return gripper_position_; -} - -bool RobotiqGripperInterface::gripperIsMoving() -{ - try { - updateStatus(); - } catch (const serial::IOException & e) { - // catch connection error and rethrow - std::cerr << "Failed to get gripper position\n"; - throw; - } - - return object_detection_status_ == ObjectDetectionStatus::MOVING; -} - -std::vector RobotiqGripperInterface::createReadCommand( - uint16_t first_register, uint8_t num_registers) -{ - std::vector cmd = { - slave_id_, - kReadFunctionCode, - getFirstByte(first_register), - getSecondByte(first_register), - getFirstByte(num_registers), - getSecondByte(num_registers)}; - auto crc = computeCRC(cmd); - cmd.push_back(getFirstByte(crc)); - cmd.push_back(getSecondByte(crc)); - return cmd; -} - -void RobotiqGripperInterface::setSpeed(uint8_t speed) { commanded_gripper_speed_ = speed; } - -void RobotiqGripperInterface::setForce(uint8_t force) { commanded_gripper_force_ = force; } - -std::vector RobotiqGripperInterface::createWriteCommand( - uint16_t first_register, const std::vector & data) -{ - uint16_t num_registers = data.size(); - uint8_t num_bytes = 2 * num_registers; - - std::vector cmd = { - slave_id_, - kWriteFunctionCode, - getFirstByte(first_register), - getSecondByte(first_register), - getFirstByte(num_registers), - getSecondByte(num_registers), - num_bytes}; - for (auto d : data) { - cmd.push_back(getFirstByte(d)); - cmd.push_back(getSecondByte(d)); - } - - auto crc = computeCRC(cmd); - cmd.push_back(getFirstByte(crc)); - cmd.push_back(getSecondByte(crc)); - - return cmd; -} - -std::vector RobotiqGripperInterface::readResponse(size_t num_bytes_requested) -{ - std::vector response; - size_t num_bytes_read = port_.read(response, num_bytes_requested); - - if (num_bytes_read != num_bytes_requested) { - const auto error_msg = "Requested " + std::to_string(num_bytes_requested) + - " bytes, but only got " + std::to_string(num_bytes_read); - THROW(serial::IOException, error_msg.c_str()); - } - - return response; -} - -void RobotiqGripperInterface::sendCommand(const std::vector & cmd) -{ - size_t num_bytes_written = port_.write(cmd); - port_.flush(); - if (num_bytes_written != cmd.size()) { - const auto error_msg = "Attempted to write " + std::to_string(cmd.size()) + - " bytes, but only wrote " + std::to_string(num_bytes_written); - THROW(serial::IOException, error_msg.c_str()); - } -} - -void RobotiqGripperInterface::updateStatus() -{ - // Tell the gripper that we want to read its status. - try { - sendCommand(read_command_); - - const auto response = readResponse(kReadResponseSize); - - // Process the response. - uint8_t gripper_status_byte = response[kResponseHeaderSize + kGripperStatusIndex]; - - // Activation status. - activation_status_ = - ((gripper_status_byte & 0x01) == 0x00) ? ActivationStatus::RESET : ActivationStatus::ACTIVE; - - // Action status. - action_status_ = - ((gripper_status_byte & 0x08) == 0x00) ? ActionStatus::STOPPED : ActionStatus::MOVING; - - // Gripper status. - switch ((gripper_status_byte & 0x30) >> 4) { - case 0x00: - gripper_status_ = GripperStatus::RESET; - break; - case 0x01: - gripper_status_ = GripperStatus::IN_PROGRESS; - break; - case 0x03: - gripper_status_ = GripperStatus::COMPLETED; - break; - } - - // Object detection status. - switch ((gripper_status_byte & 0xC0) >> 6) { - case 0x00: - object_detection_status_ = ObjectDetectionStatus::MOVING; - break; - case 0x01: - object_detection_status_ = ObjectDetectionStatus::OBJECT_DETECTED_OPENING; - break; - case 0x02: - object_detection_status_ = ObjectDetectionStatus::OBJECT_DETECTED_CLOSING; - break; - case 0x03: - object_detection_status_ = ObjectDetectionStatus::AT_REQUESTED_POSITION; - break; - } - - // Read the current gripper position. - gripper_position_ = response[kResponseHeaderSize + kPositionIndex]; - } catch (const serial::IOException & e) { - std::cerr << "Failed to update gripper status.\n"; - if (port_.isOpen()) { - std::cerr << "Error caught while reading or writing to device. Connection is open, " - "continuing to attempt " - "communication with gripper.\n ERROR: " - << e.what(); - return; - } - throw; - } -} diff --git a/robotiq_driver/tests/CMakeLists.txt b/robotiq_driver/tests/CMakeLists.txt new file mode 100644 index 0000000..357e1d8 --- /dev/null +++ b/robotiq_driver/tests/CMakeLists.txt @@ -0,0 +1,72 @@ +# Add support for GTest. +find_package(ament_cmake_gtest REQUIRED) +find_package(ament_cmake_gmock REQUIRED) +find_package(ament_lint_auto REQUIRED) +find_package(ros2_control_test_assets REQUIRED) + +ament_lint_auto_find_test_dependencies() + +# GMock throws an error if we don't switch off this option in tests. +if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") + add_compile_options(-Wno-sign-conversion) +endif() + +############################################################################### +# test_robotiq_gripper_hardware_interface + +ament_add_gmock(test_robotiq_gripper_hardware_interface + test_robotiq_gripper_hardware_interface.cpp +) +target_include_directories(test_robotiq_gripper_hardware_interface + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} +) +target_link_libraries(test_robotiq_gripper_hardware_interface robotiq_driver) +ament_target_dependencies(test_robotiq_gripper_hardware_interface + ros2_control_test_assets +) + +############################################################################### +# test_default_serial_factory + +ament_add_gmock(test_default_serial_factory + test_default_serial_factory.cpp + mock/mock_serial.hpp +) +target_include_directories(test_default_serial_factory + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} +) +target_link_libraries(test_default_serial_factory robotiq_driver) + +############################################################################### +# test_default_driver_factory + +ament_add_gmock(test_default_driver_factory + test_default_driver_factory.cpp + mock/mock_driver.hpp +) +target_include_directories(test_default_driver_factory + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} +) +target_link_libraries(test_default_driver_factory robotiq_driver) + +############################################################################### +# test_crc_utils + +ament_add_gmock(test_crc_utils + test_crc_utils.cpp +) +target_include_directories(test_crc_utils + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} +) +target_link_libraries(test_crc_utils robotiq_driver) + +############################################################################### +# test_data_utils + +ament_add_gmock(test_data_utils + test_data_utils.cpp +) +target_include_directories(test_data_utils + PRIVATE ${CMAKE_CURRENT_SOURCE_DIR} +) +target_link_libraries(test_data_utils robotiq_driver) diff --git a/robotiq_driver/tests/mock/mock_driver.hpp b/robotiq_driver/tests/mock/mock_driver.hpp new file mode 100644 index 0000000..41f730a --- /dev/null +++ b/robotiq_driver/tests/mock/mock_driver.hpp @@ -0,0 +1,53 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include + +#include + +#include + +namespace robotiq_driver::test +{ +class MockDriver : public robotiq_driver::Driver +{ +public: + MOCK_METHOD(bool, connect, (), (override)); + MOCK_METHOD(void, disconnect, (), (override)); + MOCK_METHOD(void, activate, (), (override)); + MOCK_METHOD(void, deactivate, (), (override)); + MOCK_METHOD(void, set_slave_address, (uint8_t slave_address), (override)); + MOCK_METHOD(void, set_gripper_position, (uint8_t position), (override)); + MOCK_METHOD(uint8_t, get_gripper_position, (), (override)); + MOCK_METHOD(bool, gripper_is_moving, (), (override)); + MOCK_METHOD(void, set_speed, (uint8_t speed), (override)); + MOCK_METHOD(void, set_force, (uint8_t force), (override)); +}; +} // namespace robotiq_driver::test diff --git a/robotiq_driver/tests/mock/mock_serial.hpp b/robotiq_driver/tests/mock/mock_serial.hpp new file mode 100644 index 0000000..b7b8980 --- /dev/null +++ b/robotiq_driver/tests/mock/mock_serial.hpp @@ -0,0 +1,55 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include + +#include +#include + +#include "robotiq_driver/serial.hpp" + +namespace robotiq_driver::test +{ +class MockSerial : public robotiq_driver::Serial +{ +public: + MOCK_METHOD(void, open, (), (override)); + MOCK_METHOD(bool, is_open, (), (override, const)); + MOCK_METHOD(void, close, (), (override)); + MOCK_METHOD(std::vector, read, (size_t size), (override)); + MOCK_METHOD(void, write, (const std::vector& buffer), (override)); + MOCK_METHOD(void, set_port, (const std::string& port), (override)); + MOCK_METHOD(std::string, get_port, (), (override, const)); + MOCK_METHOD(void, set_timeout, (std::chrono::milliseconds timeout), (override)); + MOCK_METHOD(std::chrono::milliseconds, get_timeout, (), (override, const)); + MOCK_METHOD(void, set_baudrate, (uint32_t baudrate), (override)); + MOCK_METHOD(uint32_t, get_baudrate, (), (override, const)); +}; +} // namespace robotiq_driver::test diff --git a/robotiq_driver/tests/test_crc_utils.cpp b/robotiq_driver/tests/test_crc_utils.cpp new file mode 100644 index 0000000..31bb900 --- /dev/null +++ b/robotiq_driver/tests/test_crc_utils.cpp @@ -0,0 +1,43 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include + +namespace robotiq_driver::test +{ +TEST(TestCrcUtils, calculate_crc) +{ + ASSERT_EQ(crc_utils::compute_crc({ 0xA1, 0xB2, 0xC3, 0xD4, 0xE5, 0xF6 }), 0x97DA); + ASSERT_EQ(crc_utils::compute_crc({ 0xE2, 0x12, 0xF1, 0xFF, 0x00, 0xD2 }), 0x2D0B); + ASSERT_EQ(crc_utils::compute_crc({ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }), 0x0194); + ASSERT_EQ(crc_utils::compute_crc({ 0x00, 0x00, 0x00, 0x00, 0x00, 0x00 }), 0x001B); + ASSERT_EQ(crc_utils::compute_crc({ 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 0x38, 0x39 }), 0x374B); + ASSERT_EQ(crc_utils::compute_crc({ 0x80, 0x00, 0x00, 0x03 }), 0x69E5); +} +} // namespace robotiq_driver::test diff --git a/robotiq_driver/tests/test_data_utils.cpp b/robotiq_driver/tests/test_data_utils.cpp new file mode 100644 index 0000000..61312c4 --- /dev/null +++ b/robotiq_driver/tests/test_data_utils.cpp @@ -0,0 +1,58 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. +#include +#include + +namespace robotiq_driver::test +{ +TEST(TestDataUtils, uint8_t_to_hex) +{ + ASSERT_EQ(data_utils::to_hex(std::vector{ 255, 121, 56, 33, 125, 60 }), "FF 79 38 21 7D 3C"); +} + +TEST(TestDataUtils, uint16_t_to_hex) +{ + ASSERT_EQ(data_utils::to_hex(std::vector{ 1169, 58544, 14917, 42884, 36112, 16512, 33207, 62584, 30418 }), + "0491 E4B0 3A45 A784 8D10 4080 81B7 F478 76D2"); +} + +TEST(TestDataUtils, to_binary_string) +{ + ASSERT_EQ(data_utils::to_binary_string(155), "10011011"); +} + +TEST(TestDataUtils, get_msb) +{ + ASSERT_EQ(data_utils::get_msb(0x14A2), 0x14); +} + +TEST(TestDataUtils, get_lsb) +{ + ASSERT_EQ(data_utils::get_lsb(0x14A2), 0xA2); +} +} // namespace robotiq_driver::test diff --git a/robotiq_driver/tests/test_default_driver_factory.cpp b/robotiq_driver/tests/test_default_driver_factory.cpp new file mode 100644 index 0000000..b0a9913 --- /dev/null +++ b/robotiq_driver/tests/test_default_driver_factory.cpp @@ -0,0 +1,108 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include + +#include + +#include + +namespace robotiq_driver::test +{ +// This factory will populate the injected mock with data read form the HardwareInfo. +class TestDriverFactory : public DefaultDriverFactory +{ +public: + explicit TestDriverFactory(std::unique_ptr driver) : driver_{ std::move(driver) } + { + } + +protected: + std::unique_ptr create_driver([[maybe_unused]] const hardware_interface::HardwareInfo& info) const override + { + return std::move(driver_); + } + +private: + mutable std::unique_ptr driver_; +}; + +/** + * Here we test the driver factory with default parameters. + */ +TEST(TestDefaultDriverFactory, create_with_default_parameters) +{ + hardware_interface::HardwareInfo info; + + auto driver = std::make_unique(); + + // This line is only required when running the test inside the IDE. + testing::Mock::AllowLeak(driver.get()); + + EXPECT_CALL(*driver, set_slave_address(0x9)); + EXPECT_CALL(*driver, set_speed(0xFF)); + EXPECT_CALL(*driver, set_force(0xFF)); + EXPECT_CALL(*driver, connect()).Times(0); + EXPECT_CALL(*driver, disconnect()).Times(0); + EXPECT_CALL(*driver, activate()).Times(0); + EXPECT_CALL(*driver, deactivate()).Times(0); + + TestDriverFactory driver_factory{ std::move(driver) }; + auto created_driver = driver_factory.create(info); +} + +/** + * Here we test the driver factory with given parameters. + */ +TEST(TestDefaultDriverFactory, create_with_given_parameters) +{ + hardware_interface::HardwareInfo info; + + info.hardware_parameters.emplace("slave_address", "1"); + info.hardware_parameters.emplace("gripper_speed_multiplier", "0.5"); + info.hardware_parameters.emplace("gripper_force_multiplier", "0.5"); + + auto driver = std::make_unique(); + + // This line is only required when running the test inside the IDE. + testing::Mock::AllowLeak(driver.get()); + + EXPECT_CALL(*driver, set_slave_address(0x1)); + EXPECT_CALL(*driver, set_speed(0x7F)); + EXPECT_CALL(*driver, set_force(0x7F)); + EXPECT_CALL(*driver, connect()).Times(0); + EXPECT_CALL(*driver, disconnect()).Times(0); + EXPECT_CALL(*driver, activate()).Times(0); + EXPECT_CALL(*driver, deactivate()).Times(0); + + TestDriverFactory driver_factory{ std::move(driver) }; + auto created_driver = driver_factory.create(info); +} +} // namespace robotiq_driver::test diff --git a/robotiq_driver/tests/test_default_serial_factory.cpp b/robotiq_driver/tests/test_default_serial_factory.cpp new file mode 100644 index 0000000..2a34257 --- /dev/null +++ b/robotiq_driver/tests/test_default_serial_factory.cpp @@ -0,0 +1,99 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include + +#include + +#include + +namespace robotiq_driver::test +{ +// This factory will populate the injected mock with data read form the HardwareInfo. +class StubSerialFactory : public DefaultSerialFactory +{ +public: + explicit StubSerialFactory(std::unique_ptr serial) : serial_{ std::move(serial) } + { + } + +protected: + std::unique_ptr create_serial() const override + { + return std::move(serial_); + } + +private: + mutable std::unique_ptr serial_; +}; + +/** + * Here we test the serial factory with default parameters. + */ +TEST(TestDefaultSerialFactory, create_with_default_parameters) +{ + hardware_interface::HardwareInfo info; + + auto serial = std::make_unique(); + + // This line is only required when running the test inside the IDE. + testing::Mock::AllowLeak(serial.get()); + + EXPECT_CALL(*serial, set_port("/dev/ttyUSB0")); + EXPECT_CALL(*serial, set_baudrate(115200)); + EXPECT_CALL(*serial, set_timeout(std::chrono::milliseconds{ 500 })); + + StubSerialFactory serial_factory(std::move(serial)); + auto created_serial = serial_factory.create(info); +} + +/** + * Here we test the serial factory with default parameters. + */ +TEST(TestDefaultSerialFactory, create_with_given_parameters) +{ + hardware_interface::HardwareInfo info; + info.hardware_parameters.emplace("COM_port", "/dev/ttyUSB1"); + info.hardware_parameters.emplace("baudrate", "9600"); + info.hardware_parameters.emplace("timeout", "0.1"); + + auto serial = std::make_unique(); + + // This line is only required when running the test inside the IDE. + testing::Mock::AllowLeak(serial.get()); + + EXPECT_CALL(*serial, set_port("/dev/ttyUSB1")); + EXPECT_CALL(*serial, set_baudrate(9600)); + EXPECT_CALL(*serial, set_timeout(std::chrono::milliseconds(100))); + + StubSerialFactory serial_factory(std::move(serial)); + auto created_serial = serial_factory.create(info); +} +} // namespace robotiq_driver::test diff --git a/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp b/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp new file mode 100644 index 0000000..7b058fd --- /dev/null +++ b/robotiq_driver/tests/test_robotiq_gripper_hardware_interface.cpp @@ -0,0 +1,84 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include + +#include + +#include +#include + +#include +#include +#include +#include + +#include +#include + +#include +#include + +namespace robotiq_driver::test +{ + +/** + * This test generates a minimal xacro robot configuration and loads the + * hardware interface plugin. + */ +TEST(TestRobotiqGripperHardwareInterface, load_urdf) +{ + std::string urdf_control_ = + R"( + + + robotiq_driver/RobotiqGripperHardwareInterface + 1.0 + 0.5 + /dev/ttyUSB0 + 0.7929 + + + + + 0.7929 + + + + + )"; + + auto urdf = ros2_control_test_assets::urdf_head + urdf_control_ + ros2_control_test_assets::urdf_tail; + hardware_interface::ResourceManager rm(urdf); + + // Check interfaces + EXPECT_EQ(1u, rm.system_components_size()); +} + +} // namespace robotiq_driver::test diff --git a/robotiq_hardware_tests/CMakeLists.txt b/robotiq_hardware_tests/CMakeLists.txt new file mode 100644 index 0000000..668cf31 --- /dev/null +++ b/robotiq_hardware_tests/CMakeLists.txt @@ -0,0 +1,36 @@ +# See https://docs.ros.org/en/foxy/How-To-Guides/Ament-CMake-Documentation.html + +cmake_minimum_required(VERSION 3.8) +project(robotiq_hardware_tests LANGUAGES CXX) + +# This module provides installation directories as per the GNU coding standards. +include(GNUInstallDirs) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +find_package(ament_cmake REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(pluginlib REQUIRED) +find_package(rclcpp REQUIRED) +find_package(serial REQUIRED) +find_package(robotiq_driver REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + ament_cmake + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + serial +) + +add_executable(full_test + src/gripper_interface_test.cpp + src/command_line_utility.hpp + src/command_line_utility.cpp +) +ament_target_dependencies(full_test robotiq_driver) + +ament_package() diff --git a/robotiq_hardware_tests/package.xml b/robotiq_hardware_tests/package.xml new file mode 100644 index 0000000..3ab47ff --- /dev/null +++ b/robotiq_hardware_tests/package.xml @@ -0,0 +1,33 @@ + + + + robotiq_hardware_tests + 0.0.1 + ROS2 driver for the Robotiq gripper. + Giovanni Remigi + BSD + Giovanni Remigi + + ament_cmake + + launch + launch_ros + robotiq_driver + + + ament_clang_format + + ament_clang_tidy + + ament_cmake_copyright + + ament_cmake_lint_cmake + + ament_lint_auto + + ament_lint_common + + + ament_cmake + + diff --git a/robotiq_hardware_tests/src/command_line_utility.cpp b/robotiq_hardware_tests/src/command_line_utility.cpp new file mode 100644 index 0000000..2d72482 --- /dev/null +++ b/robotiq_hardware_tests/src/command_line_utility.cpp @@ -0,0 +1,87 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include + +#include "command_line_utility.hpp" + +void CommandLineUtility::registerHandler(const std::string& parameter, ParameterHandler handler, bool isMandatory) +{ + handlers[parameter] = handler; + if (isMandatory) + { + mandatoryParams.insert(parameter); + } +} + +bool CommandLineUtility::parse(int argc, char* argv[]) +{ + for (int i = 1; i < argc; i++) + { + auto it = handlers.find(argv[i]); + if (it != handlers.end()) + { + receivedParams.insert(it->first); + + if (std::holds_alternative(it->second)) + { + auto& handler = std::get(it->second); + i++; + if (i < argc) + { + handler(argv[i]); + } + else + { + std::cerr << it->first << " requires a value.\n"; + } + } + else if (std::holds_alternative(it->second)) + { + auto& handler = std::get(it->second); + handler(); + } + } + else + { + std::cerr << "Unknown argument: " << argv[i] << "\n"; + return false; + } + } + + for (const auto& param : mandatoryParams) + { + if (receivedParams.find(param) == receivedParams.end()) + { + std::cerr << "Missing mandatory argument: " << param << "\n"; + return false; + } + } + + return true; +} diff --git a/robotiq_hardware_tests/src/command_line_utility.hpp b/robotiq_hardware_tests/src/command_line_utility.hpp new file mode 100644 index 0000000..edb5666 --- /dev/null +++ b/robotiq_hardware_tests/src/command_line_utility.hpp @@ -0,0 +1,73 @@ +// Copyright (c) 2023 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#pragma once + +#include +#include +#include +#include +#include + +/** + * Class to parse the command line. + */ +class CommandLineUtility +{ + using LambdaWithValue = std::function; + using LambdaWithoutValue = std::function; + using ParameterHandler = std::variant; + +public: + /** + * Assign to each command parameter a lambda function to handle it. + * @param parameter The parameter to handle. + * @param handler The lambda function to handle the parameter value. + * @param isMandatory True if the parameter is mandatory, else otherwise. + */ + void registerHandler(const std::string& parameter, ParameterHandler handler, bool isMandatory = false); + + /** + * Parse the command line and read all parameters. + * @param argc The number of tokens in the command line. + * @param argv The list of tokens. + * @return True if the parsing is successful. + */ + bool parse(int argc, char* argv[]); + +private: + // Map that associates a lambda function to each parameter to process the expected value. + std::map handlers; + + // Store all mandatory parameters. + std::set mandatoryParams; + + // Store the parameters which are parsed in the command line. If a parameter is mandatory, + // but cannot be found in the received parameters, then print an error. + std::set receivedParams; +}; diff --git a/robotiq_hardware_tests/src/gripper_interface_test.cpp b/robotiq_hardware_tests/src/gripper_interface_test.cpp new file mode 100644 index 0000000..54d18e9 --- /dev/null +++ b/robotiq_hardware_tests/src/gripper_interface_test.cpp @@ -0,0 +1,175 @@ +// Copyright (c) 2022 PickNik, Inc. +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include +#include + +#include +#include + +#include "command_line_utility.hpp" + +constexpr auto kComPort = "/dev/ttyUSB0"; +constexpr auto kBaudRate = 115200; +constexpr auto kTimeout = 1; +constexpr auto kSlaveAddress = 0x09; + +using robotiq_driver::DefaultDriver; +using robotiq_driver::DefaultSerial; + +int main(int argc, char* argv[]) +{ + CommandLineUtility cli; + + std::string port = kComPort; + cli.registerHandler( + "--port", [&port](const char* value) { port = value; }, false); + + int baudrate = kBaudRate; + cli.registerHandler( + "--baudrate", [&baudrate](const char* value) { baudrate = std::stoi(value); }, false); + + double timeout = kTimeout; + cli.registerHandler( + "--timeout", [&timeout](const char* value) { timeout = std::stod(value); }, false); + + int slave_address = kSlaveAddress; + cli.registerHandler( + "--slave-address", [&slave_address](const char* value) { slave_address = std::stoi(value); }, false); + + cli.registerHandler("-h", [&]() { + std::cout << "Usage: ./set_relative_pressure [OPTIONS]\n" + << "Options:\n" + << " --port VALUE Set the com port (default " << kComPort << ")\n" + << " --baudrate VALUE Set the baudrate (default " << kBaudRate << "bps)\n" + << " --timeout VALUE Set the read/write timeout (default " << kTimeout << "ms)\n" + << " --slave-address VALUE Set the slave address (default " << kSlaveAddress << ")\n" + << " -h Show this help message\n"; + exit(0); + }); + + if (!cli.parse(argc, argv)) + { + return 1; + } + + try + { + auto serial = std::make_unique(); + serial->set_port(port); + serial->set_baudrate(baudrate); + serial->set_timeout(std::chrono::duration_cast(std::chrono::duration(timeout))); + + auto driver = std::make_unique(std::move(serial)); + driver->set_slave_address(slave_address); + + std::cout << "Using the following parameters: " << std::endl; + std::cout << " - port: " << port << std::endl; + std::cout << " - baudrate: " << baudrate << "bps" << std::endl; + std::cout << " - read/write timeout: " << timeout << "s" << std::endl; + std::cout << " - slave address: " << slave_address << std::endl; + + const bool connected = driver->connect(); + if (!connected) + { + std::cout << "The gripper is not connected" << std::endl; + return 1; + } + + std::cout << "The gripper is connected." << std::endl; + std::cout << "Deactivating the gripper..." << std::endl; + ; + + driver->deactivate(); + + std::cout << "The gripper is deactivated." << std::endl; + std::cout << "Activating gripper..." << std::endl; + ; + + driver->activate(); + + std::cout << "The gripper is activated." << std::endl; + std::cout << "Closing the gripper..." << std::endl; + + driver->set_gripper_position(0xFF); + while (driver->gripper_is_moving()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + + std::cout << "Opening the gripper..." << std::endl; + driver->set_gripper_position(0x00); + while (driver->gripper_is_moving()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + + std::cout << "Half closing the gripper..." << std::endl; + driver->set_gripper_position(0x80); + while (driver->gripper_is_moving()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + + std::cout << "Opening gripper..." << std::endl; + driver->set_gripper_position(0x00); + while (driver->gripper_is_moving()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + + std::cout << "Decreasing gripper speed..." << std::endl; + driver->set_speed(0x0F); + + std::cout << "Closing gripper...\n"; + driver->set_gripper_position(0xFF); + while (driver->gripper_is_moving()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + + std::cout << "Increasing gripper speed..." << std::endl; + driver->set_speed(0xFF); + + std::cout << "Opening gripper..." << std::endl; + driver->set_gripper_position(0x00); + while (driver->gripper_is_moving()) + { + std::this_thread::sleep_for(std::chrono::milliseconds(500)); + } + } + catch (const std::exception& e) + { + std::cout << "Failed to communicating with the gripper: " << e.what() << std::endl; + return 1; + } + + return 0; +}