From 719502e6311755f239d79f572f18fa19e113b77a Mon Sep 17 00:00:00 2001 From: Niko Date: Thu, 11 Apr 2024 18:28:14 +0200 Subject: [PATCH] debugging --- UR_Robotiq.humble.repos | 8 - .../robotiq_2f_controllers/CMakeLists.txt | 26 -- .../robotiq_2f_controllers/package.xml | 18 - .../urdf/robotiq_2f_140.ros2_control.xacro | 4 +- .../urdf/robotiq_2f_140.urdf.xacro | 13 + .../urdf/robotiq_2f_140_macro.urdf.xacro | 421 ++++++++++++++++++ .../urdf/robotiq_2f_85.ros2_control.xacro | 87 ++++ .../urdf/robotiq_2f_85.urdf.xacro | 13 + .../urdf/robotiq_2f_85_macro.urdf.xacro | 291 ++++++++++++ .../robotiq_2f_interface/CMakeLists.txt | 54 ++- .../hardware_interface_plugin.xml | 9 + .../MockRobotiq2fSocketAdapter.hpp | 101 +++++ .../Robotiq2fSocketAdapter.hpp | 49 +- .../SocketAdapterBase.hpp | 25 ++ .../hardware_interface.hpp | 17 +- .../robotiq_2f_interface/package.xml | 1 - .../src/Robotiq2fSocketAdapter.cpp | 76 ++-- .../src/hardware_interface.cpp | 26 +- .../robotiq_controllers/CHANGELOG.rst | 9 + .../robotiq_controllers/CMakeLists.txt | 84 ++++ .../controller_plugins.xml | 7 + .../robotiq_activation_controller.hpp | 64 +++ .../robotiq_controllers/package.xml | 23 + .../src/robotiq_activation_controller.cpp | 123 +++++ 24 files changed, 1400 insertions(+), 149 deletions(-) delete mode 100644 src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt delete mode 100644 src/robotiq_2f/robotiq_2f_controllers/package.xml create mode 100644 src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml create mode 100644 src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp create mode 100644 src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/SocketAdapterBase.hpp create mode 100644 src/robotiq_2f/robotiq_controllers/CHANGELOG.rst create mode 100644 src/robotiq_2f/robotiq_controllers/CMakeLists.txt create mode 100644 src/robotiq_2f/robotiq_controllers/controller_plugins.xml create mode 100644 src/robotiq_2f/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp create mode 100644 src/robotiq_2f/robotiq_controllers/package.xml create mode 100644 src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp diff --git a/UR_Robotiq.humble.repos b/UR_Robotiq.humble.repos index aa37cd5..221b408 100644 --- a/UR_Robotiq.humble.repos +++ b/UR_Robotiq.humble.repos @@ -7,14 +7,6 @@ repositories: type: git url: https://github.com/NikoFeith/Universal_Robots_ROS2_Driver.git version: humble - serial: - type: git - url: https://github.com/tylerjw/serial.git - version: ros2 - ros2_robotiq_gripper: - type: git - url: https://github.com/NikoFeith/ros2_robotiq_gripper.git - version: main cartesian_controllers: type: git url: https://github.com/NikoFeith/cartesian_controllers.git diff --git a/src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt b/src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt deleted file mode 100644 index f96d87b..0000000 --- a/src/robotiq_2f/robotiq_2f_controllers/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -cmake_minimum_required(VERSION 3.8) -project(robotiq_2f_controllers) - -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) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) - -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) - ament_lint_auto_find_test_dependencies() -endif() - -ament_package() diff --git a/src/robotiq_2f/robotiq_2f_controllers/package.xml b/src/robotiq_2f/robotiq_2f_controllers/package.xml deleted file mode 100644 index ab0ddb2..0000000 --- a/src/robotiq_2f/robotiq_2f_controllers/package.xml +++ /dev/null @@ -1,18 +0,0 @@ - - - - robotiq_2f_controllers - 0.0.0 - TODO: Package description - niko - TODO: License declaration - - ament_cmake - - ament_lint_auto - ament_lint_common - - - ament_cmake - - diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro index 519b1c5..ee21c97 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro @@ -19,7 +19,7 @@ robotiq_2f_interface/RobotiqGripperHardwareInterface - 0.14 + 0.695 ${robot_ip} ${robot_port} 1.0 @@ -32,7 +32,7 @@ - 0.14 + 0.695 diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.urdf.xacro index e69de29..1688de1 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.urdf.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.urdf.xacro @@ -0,0 +1,13 @@ + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro index e69de29..c9509e6 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro @@ -0,0 +1,421 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.ros2_control.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.ros2_control.xacro index e69de29..f9bf678 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.ros2_control.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.ros2_control.xacro @@ -0,0 +1,87 @@ + + + + + + + + mock_components/GenericSystem + ${fake_sensor_commands} + 0.0 + + + robotiq_2f_interface/RobotiqGripperHardwareInterface + 0.7929 + ${robot_ip} + ${robot_port} + 1.0 + 0.5 + + + + + + + + + 0.7929 + + + + + + + ${prefix}finger_joint + -1 + + + + + + ${prefix}finger_joint + 1 + + + + + + ${prefix}finger_joint + -1 + + + + + + ${prefix}robotiq_140_left_knuckle_joint + -1 + + + + + + ${prefix}robotiq_140_left_knuckle_joint + 1 + + + + + + + + + + + + + + + + diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.urdf.xacro index e69de29..498b7d8 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.urdf.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85.urdf.xacro @@ -0,0 +1,13 @@ + + + + + + + + + + + + + diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro index e69de29..b4bc13c 100644 --- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro +++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro @@ -0,0 +1,291 @@ + + + + + + + robot_port="${robot_port}"/> + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + 100000.0 + 100000.0 + + + + + 1e+5 + 1 + 0 + 0.2 + 0.002 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + 100000.0 + 100000.0 + + + + + 1e+5 + 1 + 0 + 0.2 + 0.002 + 0 + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt b/src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt index 2f1bfeb..cfdb1a4 100644 --- a/src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt +++ b/src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt @@ -1,28 +1,48 @@ -cmake_minimum_required(VERSION 3.8) +cmake_minimum_required(VERSION 3.5) project(robotiq_2f_interface) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) +# Default to C++17 +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 17) endif() -# find dependencies find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) find_package(hardware_interface REQUIRED) -find_package(lifecycle_msgs REQUIRED) find_package(pluginlib REQUIRED) -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) - ament_lint_auto_find_test_dependencies() -endif() +include_directories(include) + +# Declare a C++ library +add_library(${PROJECT_NAME} SHARED + src/hardware_interface.cpp + src/Robotiq2fSocketAdapter.cpp +) + +ament_target_dependencies(${PROJECT_NAME} + rclcpp + hardware_interface + pluginlib +) + +pluginlib_export_plugin_description_file(hardware_interface hardware_interface_plugin.xml) + +# Install targets +install(TARGETS ${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) + +# Install header files +install(DIRECTORY include/ + DESTINATION include +) + +# Install plugin description file +install(FILES + hardware_interface_plugin.xml + DESTINATION share/${PROJECT_NAME} +) ament_package() diff --git a/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml b/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml new file mode 100644 index 0000000..5245a01 --- /dev/null +++ b/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml @@ -0,0 +1,9 @@ + + + + ROS2 controller for the Robotiq gripper. + + + diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp new file mode 100644 index 0000000..4669e9f --- /dev/null +++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp @@ -0,0 +1,101 @@ +// MockRobotiq2fSocketAdapter.hpp + +#ifndef MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP +#define MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP + +#include +#include +#include +#include + +namespace robotiq{ +class MockRobotiq2fSocketAdapter : public SocketAdapterBase { +public: + MockRobotiq2fSocketAdapter() : isConnected(false), gripperPosition(0), gripperForce(0), gripperSpeed(0), isActive(false) {} + + ~MockRobotiq2fSocketAdapter() override { + if (movementThread && movementThread->joinable()) { + movementThread->join(); + } + } + + bool connect(const std::string& hostname, int port) override { + isConnected.store(true); + return true; // Simulate successful connection + } + + void disconnect() override { + isConnected.store(false); + } + + int getGripperPosition() override { + return gripperPosition.load(); + } + + int getGripperForce() override { + // Assuming this is supposed to mimic `force()` + return gripperForce.load(); + } + + int force() override { + return gripperForce.load(); + } + + int speed() override { + return gripperSpeed.load(); + } + + int position() override { + return gripperPosition.load(); + } + + bool is_active() override { + return isActive.load(); + } + + void activate(bool autoCalibrate = true) override { + isActive.store(true); + } + + void deactivate() override { + isActive.store(false); + } + + std::tuple move(int targetPosition, int speed, int force) override{ + // Ensure only one movement operation is active at a time + if (movementThread && movementThread->joinable()) { + movementThread->join(); // Wait for any ongoing movement to complete + } + + // Start a new movement operation in a background thread + movementThread = std::make_unique(&MockRobotiq2fSocketAdapter::simulateMovement, this, targetPosition, speed, force); + + return std::make_tuple(true, targetPosition); // Immediately return success and the target position + } + +private: + std::atomic isConnected; + std::atomic gripperPosition; + std::atomic gripperForce; + std::atomic gripperSpeed; + std::atomic isActive; + + std::unique_ptr movementThread; + + void simulateMovement(int targetPosition, int speed, int force) { + int step = (targetPosition > gripperPosition) ? 1 : -1; + + // Simulate moving towards the target position + while (gripperPosition != targetPosition) { + gripperPosition += step; + std::this_thread::sleep_for(std::chrono::milliseconds(100)); // Simulate time to move a step + } + + // Here, you simulate setting the speed and force, though for simplicity, we're just logging it + // In a real mock, you might update some internal state to reflect these changes + std::cout << "Simulated movement to position " << targetPosition << " with speed " << speed << " and force " << force << std::endl; + } +}; +} // end namespace + +#endif // MOCK_ROBOTIQ2F_SOCKET_ADAPTER_HPP \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp index 2b8b280..5bd482a 100644 --- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp +++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp @@ -25,6 +25,7 @@ #include // For errno #include +namespace robotiq{ // Enum for Gripper Status enum class GripperStatus { RESET = 0, @@ -41,11 +42,11 @@ enum class ObjectStatus { AT_DEST = 3 }; -class Robotiq2fSocketAdapter { +class Robotiq2fSocketAdapter : public SocketAdapterBase { public: Robotiq2fSocketAdapter() ; - ~Robotiq2fSocketAdapter(); + ~Robotiq2fSocketAdapter() override; // Connection management /** @@ -60,7 +61,7 @@ public: * * @throws std::runtime_error if socket creation fails. */ - bool connect(const std::string& hostname, int port); + bool connect(const std::string& hostname, int port) override; /** * Closes the connection to the Robotiq gripper. * @@ -68,7 +69,7 @@ public: * is already closed or was never connected, this method has no effect. This method is * thread-safe and can be called concurrently with other operations on the adapter. */ - void disconnect(); + void disconnect() override; /** @@ -119,12 +120,9 @@ public: int getGripperVariable(const std::string& variable); // Gripper status - bool isGripperActive(); - int getGripperPosition(); - int getGripperForce(); - int force(); - int speed(); - int position(); + int force() override; + int speed() override; + int position() override; /** * Checks if the gripper is currently active. @@ -137,7 +135,7 @@ public: * * @throws std::runtime_error if unable to retrieve the gripper's status, typically due to communication issues. */ - bool is_active(); + bool is_active() override; // Activates the gripper and optionally performs auto-calibration @@ -158,7 +156,7 @@ public: * activation command fails, or if the gripper does not reach the expected state within * a reasonable timeframe. */ - void activate(bool autoCalibrate = true); + void activate(bool autoCalibrate = true) override; /** * Performs auto-calibration of the gripper. * @@ -175,7 +173,7 @@ public: * @throws std::runtime_error if the adapter is not connected to the gripper, if the gripper is not in an * active state, or if the calibration sequence fails. */ - void autoCalibrate(bool log=true); + void auto_calibrate(bool log=true) override; /** * Deactivates the gripper. @@ -188,7 +186,7 @@ public: * * @throws std::runtime_error if the adapter is not connected to the gripper or if the deactivation command fails. */ - void deactivate(); + void deactivate() override; // Move Commands /** @@ -204,7 +202,7 @@ public: * * @throws std::runtime_error if there's an error in sending the move command or if the adapter is not connected to a gripper. */ - std::tuple move(int position, int speed, int force); + std::tuple move(int position, int speed, int force) override; /** * Commands the gripper to move to a specified position and waits for the move to complete. * @@ -236,19 +234,19 @@ private: }; // Smart pointer to manage the socket descriptor with the custom deleter - std::unique_ptr _sockfd; - std::mutex socketMutex; // Mutex for socket access synchronization + std::unique_ptr sockfd_; + std::mutex socketMutex_; // Mutex for socket access synchronization // bounds of the encoded gripper states - int _min_position = 0; - int _max_position = 255; - int _min_speed = 0; - int _max_speed = 255; - int _min_force = 0; - int _max_force = 255; + int min_position_ = 0; + int max_position_ = 255; + int min_speed_ = 0; + int max_speed_ = 255; + int min_force_ = 0; + int max_force_ = 255; - int open_position = _min_position; - int closed_position = _max_position; + int open_position_ = min_position_; + int closed_position_ = max_position_; /** @@ -352,5 +350,6 @@ private: // Constants buffer sizes const size_t BUFFER_SIZE = 1024; // Adjust as necessary }; +} // end namespace #endif // ROBOTIQ2F_SOCKET_ADAPTER_HPP diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/SocketAdapterBase.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/SocketAdapterBase.hpp new file mode 100644 index 0000000..43ea205 --- /dev/null +++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/SocketAdapterBase.hpp @@ -0,0 +1,25 @@ +// IGripperAdapter.hpp + +#ifndef SOCKET_ADAPTER_BASE_HPP +#define SOCKET_ADAPTER_BASE_HPP + +#include +#include + +namespace robotiq{ +class SocketAdapterBase { +public: + virtual ~SocketAdapterBase() = default; + + virtual bool connect(const std::string& hostname, int port) = 0; + virtual void disconnect() = 0; + virtual int force() = 0; + virtual int speed() = 0; + virtual int position() = 0; + virtual bool is_active() = 0; + virtual void activate(bool autoCalibrate = true) = 0; + virtual void deactivate() = 0; + virtual std::tuple move(int position, int speed, int force) = 0; +}; +} // end namespace +#endif // SOCKET_ADAPTER_BASE_HPP \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp index f0754fc..ee9394c 100644 --- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp +++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp @@ -10,18 +10,18 @@ #include #include -#include +#include "robotiq_2f_interface/SocketAdapterBase.hpp" +#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp" +#include "robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp" #include #include #include -#include #include -namespace robotiq_driver -{ +namespace robotiq{ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface { public: @@ -46,8 +46,9 @@ public: protected: // Likely changes the access to protected from private // Members for driver interaction - std::unique_ptr socket_adapter_; - // std::unique_ptr driver_factory_; + bool use_mock_hardware_; + std::unique_ptr socket_adapter_; + void RobotiqGripperHardwareInterface::configureAdapter(bool useMock); // Background communication thread std::thread communication_thread_; @@ -74,6 +75,8 @@ protected: // Likely changes the access to protected from private double gripper_velocity_ = 0.0; double gripper_position_command_ = 0.0; + rclcpp::Time last_update_time_; + double reactivate_gripper_cmd_ = 0.0; std::atomic reactivate_gripper_async_cmd_; double reactivate_gripper_response_ = 0.0; @@ -99,6 +102,6 @@ protected: // Likely changes the access to protected from private // Logger const auto logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface"); }; -} +} // end namespace #endif //ROBOTIQ2F_HARDWARE_INTERFACE_HPP \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_interface/package.xml b/src/robotiq_2f/robotiq_2f_interface/package.xml index 2b5bbdb..3f0fa2b 100644 --- a/src/robotiq_2f/robotiq_2f_interface/package.xml +++ b/src/robotiq_2f/robotiq_2f_interface/package.xml @@ -12,7 +12,6 @@ rclcpp rclcpp_lifecycle hardware_interface - lifecycle_msgs pluginlib ament_lint_auto diff --git a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp index 9d99ab7..d1144e8 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp +++ b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp @@ -1,7 +1,8 @@ -#include "Robotiq2fSocketAdapter.hpp" +#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp" -Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : _sockfd(new int(-1)) { +namespace robotiq{ +Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) { } @@ -11,8 +12,8 @@ Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() { // Connection and disconnection bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) { - std::lock_guard lock(socketMutex); - if (*_sockfd >= 0) { + std::lock_guard lock(socketMutex_); + if (*sockfd_ >= 0) { std::cerr << "Already connected. Disconnecting to reconnect.\n"; disconnect(); } @@ -22,7 +23,7 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) { std::cerr << "Socket creation failed: " << strerror(errno) << "\n"; return false; } - *_sockfd = sock; + *sockfd_ = sock; sockaddr_in serv_addr{}; memset(&serv_addr, 0, sizeof(serv_addr)); @@ -31,15 +32,15 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) { if (inet_pton(AF_INET, hostname.c_str(), &serv_addr.sin_addr) <= 0) { std::cerr << "Invalid address: " << hostname << "\n"; - close(*_sockfd); - *_sockfd = -1; + close(*sockfd_); + *sockfd_ = -1; return false; } - if (::connect(_sockfd, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) { + if (::connect(sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0) { std::cerr << "Connection to " << hostname << ":" << port << " failed: " << strerror(errno) << "\n"; - close(*_sockfd); - *_sockfd = -1; + close(*sockfd_); + *sockfd_ = -1; return false; } @@ -47,26 +48,26 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) { } void Robotiq2fSocketAdapter::disconnect() { - std::lock_guard lock(socketMutex); - if (*_sockfd >= 0) { - if (close(*_sockfd) == -1) { + std::lock_guard lock(socketMutex_); + if (*sockfd_ >= 0) { + if (close(*sockfd_) == -1) { std::cerr << "Error closing socket: " << strerror(errno) << "\n"; } else { std::cout << "Disconnected successfully.\n"; } - *_sockfd = -1; + *sockfd_ = -1; } } // Utility methods bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) { - std::lock_guard lock(socketMutex); - if (*_sockfd < 0) { + std::lock_guard lock(socketMutex_); + if (*sockfd_ < 0) { std::cerr << "Attempted to send command on a disconnected socket.\n"; return false; } - ssize_t result = send(*_sockfd, cmd.c_str(), cmd.length(), 0); + ssize_t result = send(*sockfd_, cmd.c_str(), cmd.length(), 0); if (result < 0) { std::cerr << "Failed to send command: " << strerror(errno) << "\n"; return false; @@ -76,8 +77,8 @@ bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) { } std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) { - std::lock_guard lock(socketMutex); - if (*_sockfd < 0) { + std::lock_guard lock(socketMutex_); + if (*sockfd_ < 0) { std::cerr << "Attempted to receive response on a disconnected socket.\n"; return ""; } @@ -94,9 +95,9 @@ std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) { while (!complete_response) { fd_set readfds; FD_ZERO(&readfds); - FD_SET(*_sockfd, &readfds); + FD_SET(*sockfd_, &readfds); - int activity = select(*_sockfd + 1, &readfds, nullptr, nullptr, &tv); + int activity = select(*sockfd_ + 1, &readfds, nullptr, nullptr, &tv); if (activity == -1) { std::cerr << "[receiveResponse] Error in select(): " << strerror(errno) << "\n"; return ""; @@ -105,7 +106,7 @@ std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) { return ""; } - ssize_t bytes_read = recv(*_sockfd, buffer, BUFFER_SIZE - 1, 0); + ssize_t bytes_read = recv(*sockfd_, buffer, BUFFER_SIZE - 1, 0); if (bytes_read < 0) { if (errno != EAGAIN && errno != EWOULDBLOCK) { std::cerr << "Failed to receive response: " << strerror(errno) << "\n"; @@ -166,8 +167,8 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map lock(socketMutex); - if (*_sockfd < 0) { + std::lock_guard lock(socketMutex_); + if (*sockfd_ < 0) { throw std::runtime_error("Cannot set variables on a disconnected socket."); return false; } @@ -191,8 +192,8 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou std::string cmd = SET_COMMAND + " " + commandVariables.at(variable) + " " + ss.str() + "\n"; - std::lock_guard lock(socketMutex); - if (*_sockfd < 0) { + std::lock_guard lock(socketMutex_); + if (*sockfd_ < 0) { throw std::runtime_error("Cannot set variables on a disconnected socket."); return false; } @@ -209,8 +210,8 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) { std::string cmd = GET_COMMAND + " " + commandVariables.at(variable) + "\n"; - std::lock_guard lock(socketMutex); - if (*_sockfd < 0) { + std::lock_guard lock(socketMutex_); + if (*sockfd_ < 0) { throw std::runtime_error("Cannot get variables on a disconnected socket."); return -1; } @@ -288,9 +289,9 @@ int Robotiq2fSocketAdapter::position(){ // Movement Methods std::tuple Robotiq2fSocketAdapter::move(int position, int speed, int force) { - int clippedPosition = clip_val(_min_position, position, _max_position); - int clippedSpeed = clip_val(_min_speed, speed, _max_speed); - int clippedForce = clip_val(_min_force, force, _max_force); + int clippedPosition = clip_val(min_position_, position, max_position_); + int clippedSpeed = clip_val(min_speed_, speed, max_speed_); + int clippedForce = clip_val(min_force_, force, max_force_); // Create a map for gripper variables (similar to Python's OrderedDict) std::map variableMap = { @@ -305,9 +306,9 @@ std::tuple Robotiq2fSocketAdapter::move(int position, int speed, int } std::tuple Robotiq2fSocketAdapter::move_and_wait_for_pos(int position, int speed, int force) { - int clippedPosition = clip_val(min_position, position, max_position); - int clippedSpeed = clip_val(min_speed, speed, max_speed); - int clippedForce = clip_val(min_force, force, max_force); + int clippedPosition = clip_val(min_position_, position, max_position_); + int clippedSpeed = clip_val(min_speed_, speed, max_speed_); + int clippedForce = clip_val(min_force_, force, max_force_); // Set gripper variables to initiate the move std::map variableMap = { @@ -353,17 +354,18 @@ void Robotiq2fSocketAdapter::auto_calibrate(bool log=true) { if (std::get<1>(result) != ObjectStatus::AT_DEST) { throw std::runtime_error("Calibration failed because of an object."); } - _max_position = std::get<0>(result); // Update max position + max_position_ = std::get<0>(result); // Update max position // Open as far as possible result = move_and_wait_for_pos(open_position, 64, 1); if (std::get<1>(result) != ObjectStatus::AT_DEST) { throw std::runtime_error("Calibration failed because of an object."); } - _min_position = std::get<0>(result); // Update min position + min_position_ = std::get<0>(result); // Update min position if (log) { std::cout << "Gripper auto-calibrated to [" - << _min_position << ", " << _max_position << "]\n"; + << min_position_ << ", " << max_position_ << "]\n"; } } +} // end namespace \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp index 76d5adc..d6b3365 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp +++ b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp @@ -1,11 +1,10 @@ -#include "RobotiqGripperHardwareInterface.hpp" +#include "robotiq_2f_interface/hardware_interface.hpp" -namespace robotiq_driver +namespace robotiq { RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface() : communication_thread_is_running_(false) { - // socket_factory_ = std::make_unique(); } RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface() @@ -41,6 +40,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons { robot_ip_ = info_.hardware_parameters.at("robot_ip"); robot_port_ = std::stod(info_.hardware_parameters.at("robot_port")); + use_mock_hardware_ = std::stod(info_.hardware_parameters.at("use_fake_hardware")); node->get_parameter("min_position", min_position_); node->get_parameter("max_position", max_position_); node->get_parameter("min_speed", min_speed_); @@ -98,8 +98,6 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons } } - // Socket Factory -> not added yet - return CallbackReturn::SUCCESS; } @@ -249,7 +247,7 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::read(const rclc // Fetch current position and other states from the hardware try { - int raw_position = socket_adapter_->getGripperPosition(); // This should be an existing method in your adapter + int raw_position = socket_adapter_->position(); // This should be an existing method in your adapter double new_position = convertRawToPosition(raw_position); rclcpp::Time current_time = time; @@ -309,6 +307,7 @@ void RobotiqGripperHardwareInterface::background_task() { while (communication_thread_is_running_.load()) { + std::lock_guard guard(resource_mutex); try { // Re-activate the gripper @@ -331,10 +330,10 @@ void RobotiqGripperHardwareInterface::background_task() } // Read the state of the gripper. - int raw_position = socket_adapter_->getGripperPosition(); + int raw_position = socket_adapter_->position(); gripper_current_state_.store(convertRawToPosition(raw_position)); } - catch (std::exception& e) + catch (const std::exception& e) { RCLCPP_ERROR(logger_, "Error in background task: %s", e.what()); } @@ -344,6 +343,17 @@ void RobotiqGripperHardwareInterface::background_task() } +// Mock Hardware +void RobotiqGripperHardwareInterface::configureAdapter(bool useMock) { + if (useMock) { + RCLCPP_INFO(logger_, "Using mock Robotiq adapter"); + socket_adapter_ = std::make_unique(); + } else { + RCLCPP_INFO(logger_, "Using real Robotiq adapter"); + socket_adapter_ = std::make_unique(); + } +} + // Conversion methods double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) { diff --git a/src/robotiq_2f/robotiq_controllers/CHANGELOG.rst b/src/robotiq_2f/robotiq_controllers/CHANGELOG.rst new file mode 100644 index 0000000..467d0a3 --- /dev/null +++ b/src/robotiq_2f/robotiq_controllers/CHANGELOG.rst @@ -0,0 +1,9 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package robotiq_controllers +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +0.0.1 (2023-07-17) +------------------ +* Initial ROS 2 release of robotiq_controllers + * This package is not supported by Robotiq but is being maintained by PickNik Robotics +* Contributors: Alex Moriarty, Cory Crean diff --git a/src/robotiq_2f/robotiq_controllers/CMakeLists.txt b/src/robotiq_2f/robotiq_controllers/CMakeLists.txt new file mode 100644 index 0000000..c4334bc --- /dev/null +++ b/src/robotiq_2f/robotiq_controllers/CMakeLists.txt @@ -0,0 +1,84 @@ +cmake_minimum_required(VERSION 3.8) +project(robotiq_controllers) + +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(controller_interface REQUIRED) +find_package(std_srvs REQUIRED) + +set(THIS_PACKAGE_INCLUDE_DEPENDS + controller_interface + std_srvs +) + +include_directories(include) + +add_library(${PROJECT_NAME} SHARED + src/robotiq_activation_controller.cpp +) + +target_include_directories(${PROJECT_NAME} PRIVATE + include +) + +ament_target_dependencies(${PROJECT_NAME} + ${THIS_PACKAGE_INCLUDE_DEPENDS} +) + +pluginlib_export_plugin_description_file(controller_interface controller_plugins.xml) + +# # INSTALL +install( + TARGETS ${PROJECT_NAME} + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin +) +install( + DIRECTORY include/ + DESTINATION include +) + +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 xmllint + # ament_xmllint requires network and can timeout if on throttled networks + set(ament_cmake_xmllint_FOUND TRUE) + + ament_lint_auto_find_test_dependencies() +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} +) + +ament_package() diff --git a/src/robotiq_2f/robotiq_controllers/controller_plugins.xml b/src/robotiq_2f/robotiq_controllers/controller_plugins.xml new file mode 100644 index 0000000..9c5c8f5 --- /dev/null +++ b/src/robotiq_2f/robotiq_controllers/controller_plugins.xml @@ -0,0 +1,7 @@ + + + + This controller provides an interface to (re-)activate the Robotiq gripper. + + + diff --git a/src/robotiq_2f/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp b/src/robotiq_2f/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp new file mode 100644 index 0000000..d0dac29 --- /dev/null +++ b/src/robotiq_2f/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp @@ -0,0 +1,64 @@ +// 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 "controller_interface/controller_interface.hpp" +#include "std_srvs/srv/trigger.hpp" + +namespace robotiq_controllers +{ +class RobotiqActivationController : public controller_interface::ControllerInterface +{ +public: + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::InterfaceConfiguration state_interface_configuration() const 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_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); + + static constexpr double ASYNC_WAITING = 2.0; + enum CommandInterfaces + { + REACTIVATE_GRIPPER_CMD, + REACTIVATE_GRIPPER_RESPONSE + }; + + rclcpp::Service::SharedPtr reactivate_gripper_srv_; +}; +} // namespace robotiq_controllers diff --git a/src/robotiq_2f/robotiq_controllers/package.xml b/src/robotiq_2f/robotiq_controllers/package.xml new file mode 100644 index 0000000..562b784 --- /dev/null +++ b/src/robotiq_2f/robotiq_controllers/package.xml @@ -0,0 +1,23 @@ + + + + robotiq_controllers + 0.0.1 + Controllers for the Robotiq gripper. + Alex Moriarty + Marq Rasmussen + BSD 3-Clause + Cory Crean + + ament_cmake + + controller_interface + std_srvs + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp b/src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp new file mode 100644 index 0000000..aa63d21 --- /dev/null +++ b/src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp @@ -0,0 +1,123 @@ +// 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_controllers/robotiq_activation_controller.hpp" + +namespace robotiq_controllers +{ +controller_interface::InterfaceConfiguration RobotiqActivationController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + config.names.emplace_back("reactivate_gripper/reactivate_gripper_cmd"); + config.names.emplace_back("reactivate_gripper/reactivate_gripper_response"); + + return config; +} + +controller_interface::InterfaceConfiguration RobotiqActivationController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + return config; +} + +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*/) +{ + // 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()); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::ERROR; + } + + 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 (...) + { + return LifecycleNodeInterface::CallbackReturn::ERROR; + } + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn +RobotiqActivationController::on_deactivate(const rclcpp_lifecycle::State& /*previous_state*/) +{ + try + { + reactivate_gripper_srv_.reset(); + } + catch (...) + { + return LifecycleNodeInterface::CallbackReturn::ERROR; + } + + return LifecycleNodeInterface::CallbackReturn::SUCCESS; +} + +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) +{ + 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) + { + std::this_thread::sleep_for(std::chrono::milliseconds(50)); + } + resp->success = command_interfaces_[REACTIVATE_GRIPPER_RESPONSE].get_value(); + + return resp->success; +} +} // namespace robotiq_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(robotiq_controllers::RobotiqActivationController, controller_interface::ControllerInterface)