From 719502e6311755f239d79f572f18fa19e113b77a Mon Sep 17 00:00:00 2001 From: Niko Date: Thu, 11 Apr 2024 18:28:14 +0200 Subject: [PATCH 1/7] 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) From 7858d6ef12e7419d309d02fb1c48583e4e04f49a Mon Sep 17 00:00:00 2001 From: Niko Date: Thu, 11 Apr 2024 18:42:12 +0200 Subject: [PATCH 2/7] debugging --- .../robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp | 2 ++ .../robotiq_2f_interface/Robotiq2fSocketAdapter.hpp | 4 +++- .../include/robotiq_2f_interface/SocketAdapterBase.hpp | 2 +- .../robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp | 8 ++++---- 4 files changed, 10 insertions(+), 6 deletions(-) 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 index 4669e9f..d2b7fe4 100644 --- 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 @@ -8,6 +8,8 @@ #include #include +#include "robotiq_2f_interface/SocketAdapterBase.hpp" + namespace robotiq{ class MockRobotiq2fSocketAdapter : public SocketAdapterBase { public: 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 5bd482a..394da7b 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,8 @@ #include // For errno #include +#include "robotiq_2f_interface/SocketAdapterBase.hpp" + namespace robotiq{ // Enum for Gripper Status enum class GripperStatus { @@ -173,7 +175,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 auto_calibrate(bool log=true) override; + void auto_calibrate(bool log=true); /** * Deactivates the gripper. 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 index 43ea205..b455d2f 100644 --- 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 @@ -1,4 +1,4 @@ -// IGripperAdapter.hpp +// SocketAdapterBase.hpp #ifndef SOCKET_ADAPTER_BASE_HPP #define SOCKET_ADAPTER_BASE_HPP diff --git a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp index d1144e8..bac9d5e 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp +++ b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp @@ -37,7 +37,7 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) { 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; @@ -344,20 +344,20 @@ std::tuple Robotiq2fSocketAdapter::move_and_wait_for_pos(int void Robotiq2fSocketAdapter::auto_calibrate(bool log=true) { // Open in case we are holding an object - std::tuple result = move_and_wait_for_pos(open_position, 64, 1); + std::tuple result = move_and_wait_for_pos(open_position_, 64, 1); if (std::get<1>(result) != ObjectStatus::AT_DEST) { throw std::runtime_error("Calibration failed opening to start."); } // Close as far as possible - result = move_and_wait_for_pos(closed_position, 64, 1); + result = move_and_wait_for_pos(closed_position_, 64, 1); 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 // Open as far as possible - result = move_and_wait_for_pos(open_position, 64, 1); + 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."); } From 1be83c8f9321c1a994087c0c53fff19f9c0387b9 Mon Sep 17 00:00:00 2001 From: Niko Date: Thu, 11 Apr 2024 18:44:38 +0200 Subject: [PATCH 3/7] debugging --- .../robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp index bac9d5e..1228fae 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp +++ b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp @@ -76,7 +76,7 @@ bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) { return true; } -std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms=2000) { +std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) { std::lock_guard lock(socketMutex_); if (*sockfd_ < 0) { std::cerr << "Attempted to receive response on a disconnected socket.\n"; @@ -342,7 +342,7 @@ std::tuple Robotiq2fSocketAdapter::move_and_wait_for_pos(int } -void Robotiq2fSocketAdapter::auto_calibrate(bool log=true) { +void Robotiq2fSocketAdapter::auto_calibrate(bool log) { // Open in case we are holding an object std::tuple result = move_and_wait_for_pos(open_position_, 64, 1); if (std::get<1>(result) != ObjectStatus::AT_DEST) { From a59a248969688298f2f58b1aaad9cb465e817533 Mon Sep 17 00:00:00 2001 From: Niko Date: Fri, 12 Apr 2024 11:26:22 +0200 Subject: [PATCH 4/7] build complete now launch debugging --- .idea/vcs.xml | 2 - .../robotiq_2f_description/CMakeLists.txt | 26 ++--- .../config/robotiq_2f_140_config.yaml | 17 ++- .../launch/robotiq_control.launch.py | 0 .../launch/view_gripper.launch.py | 0 .../robotiq_2f_description/package.xml | 14 +++ .../urdf/robotiq_2f_140.ros2_control.xacro | 2 +- .../urdf/robotiq_2f_140_macro.urdf.xacro | 9 +- .../urdf/robotiq_2f_85_macro.urdf.xacro | 9 +- .../hardware_interface_plugin.xml | 2 +- .../MockRobotiq2fSocketAdapter.hpp | 9 -- .../Robotiq2fSocketAdapter.hpp | 4 +- .../hardware_interface.hpp | 15 ++- .../visibility_control.hpp | 56 +++++++++ .../src/Robotiq2fSocketAdapter.cpp | 20 ++-- .../src/hardware_interface.cpp | 107 ++++++++---------- 16 files changed, 179 insertions(+), 113 deletions(-) create mode 100644 src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py create mode 100644 src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py create mode 100644 src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/visibility_control.hpp diff --git a/.idea/vcs.xml b/.idea/vcs.xml index 588e956..4dc5475 100644 --- a/.idea/vcs.xml +++ b/.idea/vcs.xml @@ -5,7 +5,5 @@ - - \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_description/CMakeLists.txt b/src/robotiq_2f/robotiq_2f_description/CMakeLists.txt index 20cbfc6..a4d9266 100644 --- a/src/robotiq_2f/robotiq_2f_description/CMakeLists.txt +++ b/src/robotiq_2f/robotiq_2f_description/CMakeLists.txt @@ -5,22 +5,16 @@ 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() +install( + DIRECTORY + config + launch + meshes + rviz + urdf + DESTINATION + share/${PROJECT_NAME} +) ament_package() diff --git a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml index ce06c22..b38c13c 100644 --- a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml +++ b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml @@ -1,9 +1,8 @@ -robotiq_2f_140_gripper: - ros__parameters: - # Physical limits - min_position: 0.0 # Meters (fully closed) - max_position: 0.14 # Meters (fully open) - min_speed: 0.02 # Meters per second - max_speed: 0.15 # Meters per second - min_force: 20.0 # Newtons - max_force: 235.0 # Newtons \ No newline at end of file +robotiq_2f_gripper: + # Physical limits + min_position: 0.0 # Meters (fully closed) + max_position: 0.14 # Meters (fully open) + min_speed: 0.02 # Meters per second + max_speed: 0.15 # Meters per second + min_force: 20.0 # Newtons + max_force: 235.0 # Newtons \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py b/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py new file mode 100644 index 0000000..e69de29 diff --git a/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py b/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py new file mode 100644 index 0000000..e69de29 diff --git a/src/robotiq_2f/robotiq_2f_description/package.xml b/src/robotiq_2f/robotiq_2f_description/package.xml index 4118eee..59e3d99 100644 --- a/src/robotiq_2f/robotiq_2f_description/package.xml +++ b/src/robotiq_2f/robotiq_2f_description/package.xml @@ -9,6 +9,20 @@ ament_cmake + joint_state_publisher_gui + launch + launch_ros + robot_state_publisher + rviz2 + urdf + xacro + + ros2_control + ros2_controllers + + gripper_controllers + joint_state_broadcaster + ament_lint_auto ament_lint_common 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 ee21c97..1eaaf26 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 @@ -13,7 +13,7 @@ - mock_components/GenericSystem + robotiq_2f_interface/RobotiqGripperHardwareInterface ${fake_sensor_commands} 0.0 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 c9509e6..dc70eee 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 @@ -1,5 +1,6 @@ + + robot_port="${robot_port}" + min_position="${robotiq_2f_gripper.min_position}" + max_position="${robotiq_2f_gripper.max_position}" + min_speed="${robotiq_2f_gripper.min_speed}" + max_speed="${robotiq_2f_gripper.max_speed}" + min_force="${robotiq_2f_gripper.min_force}" + max_force="${robotiq_2f_gripper.max_force}"/> 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 b4bc13c..90ba2c1 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 @@ -1,5 +1,6 @@ + - robot_port="${robot_port}"/> + robot_port="${robot_port}" + min_position="${robotiq_2f_gripper.min_position}" + max_position="${robotiq_2f_gripper.max_position}" + min_speed="${robotiq_2f_gripper.min_speed}" + max_speed="${robotiq_2f_gripper.max_speed}" + min_force="${robotiq_2f_gripper.min_force}" + max_force="${robotiq_2f_gripper.max_force}"/> diff --git a/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml b/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml index 5245a01..0788a67 100644 --- a/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml +++ b/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml @@ -1,6 +1,6 @@ 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 index d2b7fe4..cd19292 100644 --- 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 @@ -30,15 +30,6 @@ public: 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(); } 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 394da7b..52020f4 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 @@ -237,7 +237,7 @@ 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::mutex socket_mutex_; // Mutex for socket access synchronization // bounds of the encoded gripper states int min_position_ = 0; @@ -351,6 +351,8 @@ private: // Constants buffer sizes const size_t BUFFER_SIZE = 1024; // Adjust as necessary + const int MAX_RETRIES = 5; + const int RETRY_DELAY_MS = 20; }; } // end namespace 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 ee9394c..7054954 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,13 +10,17 @@ #include #include +#include "robotiq_2f_interface/visibility_control.hpp" + #include "robotiq_2f_interface/SocketAdapterBase.hpp" #include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp" #include "robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp" -#include +#include +#include +#include #include - +#include #include #include @@ -48,11 +52,12 @@ protected: // Likely changes the access to protected from private // Members for driver interaction bool use_mock_hardware_; std::unique_ptr socket_adapter_; - void RobotiqGripperHardwareInterface::configureAdapter(bool useMock); + void configureAdapter(bool useMock); // Background communication thread std::thread communication_thread_; std::atomic communication_thread_is_running_; + std::mutex resource_mutex_; void background_task(); static constexpr double NO_NEW_CMD_ = std::numeric_limits::quiet_NaN(); @@ -97,10 +102,10 @@ protected: // Likely changes the access to protected from private double max_force_; // loop time - constexpr auto gripperCommsLoopPeriod = std::chrono::milliseconds{ 10 }; + const std::chrono::milliseconds gripperCommsLoopPeriod = std::chrono::milliseconds{ 10 }; // Logger - const auto logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface"); + const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface"); }; } // end namespace diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/visibility_control.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/visibility_control.hpp new file mode 100644 index 0000000..a04ea16 --- /dev/null +++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/visibility_control.hpp @@ -0,0 +1,56 @@ +// Copyright 2017 Open Source Robotics Foundation, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#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 + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ROBOTIQ_DRIVER_EXPORT __attribute__((dllexport)) +#define ROBOTIQ_DRIVER_IMPORT __attribute__((dllimport)) +#else +#define ROBOTIQ_DRIVER_EXPORT __declspec(dllexport) +#define ROBOTIQ_DRIVER_IMPORT __declspec(dllimport) +#endif +#ifdef ROBOTIQ_DRIVER_BUILDING_DLL +#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_EXPORT +#else +#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_IMPORT +#endif +#define ROBOTIQ_DRIVER_PUBLIC_TYPE ROBOTIQ_DRIVER_PUBLIC +#define ROBOTIQ_DRIVER_LOCAL +#else +#define ROBOTIQ_DRIVER_EXPORT __attribute__((visibility("default"))) +#define ROBOTIQ_DRIVER_IMPORT +#if __GNUC__ >= 4 +#define ROBOTIQ_DRIVER_PUBLIC __attribute__((visibility("default"))) +#define ROBOTIQ_DRIVER_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROBOTIQ_DRIVER_PUBLIC +#define ROBOTIQ_DRIVER_LOCAL +#endif +#define ROBOTIQ_DRIVER_PUBLIC_TYPE +#endif + +#endif // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp index 1228fae..f21a245 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp +++ b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp @@ -12,7 +12,7 @@ Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() { // Connection and disconnection bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) { - std::lock_guard lock(socketMutex_); + std::lock_guard lock(socket_mutex_); if (*sockfd_ >= 0) { std::cerr << "Already connected. Disconnecting to reconnect.\n"; disconnect(); @@ -48,7 +48,7 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) { } void Robotiq2fSocketAdapter::disconnect() { - std::lock_guard lock(socketMutex_); + std::lock_guard lock(socket_mutex_); if (*sockfd_ >= 0) { if (close(*sockfd_) == -1) { std::cerr << "Error closing socket: " << strerror(errno) << "\n"; @@ -61,7 +61,7 @@ void Robotiq2fSocketAdapter::disconnect() { // Utility methods bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) { - std::lock_guard lock(socketMutex_); + std::lock_guard lock(socket_mutex_); if (*sockfd_ < 0) { std::cerr << "Attempted to send command on a disconnected socket.\n"; return false; @@ -77,7 +77,7 @@ bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) { } std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) { - std::lock_guard lock(socketMutex_); + std::lock_guard lock(socket_mutex_); if (*sockfd_ < 0) { std::cerr << "Attempted to receive response on a disconnected socket.\n"; return ""; @@ -162,12 +162,12 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map lock(socketMutex_); + std::lock_guard lock(socket_mutex_); if (*sockfd_ < 0) { throw std::runtime_error("Cannot set variables on a disconnected socket."); return false; @@ -190,9 +190,9 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou std::stringstream ss; ss << value; // Convert the value to a string - std::string cmd = SET_COMMAND + " " + commandVariables.at(variable) + " " + ss.str() + "\n"; + std::string cmd = SET_COMMAND + " " + variable + " " + ss.str() + "\n"; - std::lock_guard lock(socketMutex_); + std::lock_guard lock(socket_mutex_); if (*sockfd_ < 0) { throw std::runtime_error("Cannot set variables on a disconnected socket."); return false; @@ -208,9 +208,9 @@ 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::string cmd = GET_COMMAND + " " + variable + "\n"; - std::lock_guard lock(socketMutex_); + std::lock_guard lock(socket_mutex_); if (*sockfd_ < 0) { throw std::runtime_error("Cannot get variables on a disconnected socket."); return -1; 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 d6b3365..ed19dcb 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp +++ b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp @@ -9,14 +9,14 @@ RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface() RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface() { - if (communication_thread_is_running_.load()) - { - communication_thread_is_running_.store(false); - if (communication_thread_.joinable()) + if (communication_thread_is_running_.load()) { - communication_thread_.join(); + communication_thread_is_running_.store(false); + if (communication_thread_.joinable()) + { + communication_thread_.join(); + } } - } } hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info) @@ -28,25 +28,17 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons return CallbackReturn::ERROR; } - // Loading paramters from Hardware info and .yaml file - auto node = this->get_node(); - node->declare_parameter("min_position", 0.0); - node->declare_parameter("max_position", 0.14); - node->declare_parameter("min_speed", 0.02); - node->declare_parameter("max_speed", 0.15); - node->declare_parameter("min_force", 20.0); - node->declare_parameter("max_force", 235.0); try { - robot_ip_ = info_.hardware_parameters.at("robot_ip"); - robot_port_ = std::stod(info_.hardware_parameters.at("robot_port")); + 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_); - node->get_parameter("max_speed", max_speed_); - node->get_parameter("min_force", min_force_); - node->get_parameter("max_force", max_force_); + min_position_ = std::stod(info_.hardware_parameters.at("min_position")); + max_position_ = std::stod(info_.hardware_parameters.at("max_position")); + min_speed_ = std::stod(info_.hardware_parameters.at("min_speed")); + max_speed_ = std::stod(info_.hardware_parameters.at("max_speed")); + min_force_ = std::stod(info_.hardware_parameters.at("min_force")); + max_force_ = std::stod(info_.hardware_parameters.at("max_force")); } catch (const std::exception& e) { @@ -101,7 +93,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons return CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& /*previous_state*/) +hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state) { RCLCPP_DEBUG(logger_, "on_configure"); try @@ -305,41 +297,42 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rcl void RobotiqGripperHardwareInterface::background_task() { - while (communication_thread_is_running_.load()) - { - std::lock_guard guard(resource_mutex); - try + while (communication_thread_is_running_.load()) { - // Re-activate the gripper - // This can be used, for example, to re-run the auto-calibration. - if (reactivate_gripper_async_cmd_.load()) - { - socket_adapter_->deactivate(); - socket_adapter_->activate(); - reactivate_gripper_async_cmd_.store(false); - reactivate_gripper_async_response_.store(true); - } + std::lock_guard guard(resource_mutex_); + try + { + // Re-activate the gripper + // This can be used, for example, to re-run the auto-calibration. + if (reactivate_gripper_async_cmd_.load()) + { + socket_adapter_->deactivate(); + socket_adapter_->activate(); + reactivate_gripper_async_cmd_.store(false); + reactivate_gripper_async_response_.store(true); + } - // Write the latest command to the gripper. - int scaled_position = write_command_.load(); - int scaled_speed = write_speed_.load(); - int scaled_force = write_force_.load(); + // Write the latest command to the gripper. + int scaled_position = write_command_.load(); + int scaled_speed = write_speed_.load(); + int scaled_force = write_force_.load(); - if (!socket_adapter_->move(scaled_position, scaled_speed, scaled_force)) { - throw std::runtime_error("Failed to send move command to Robotiq gripper."); - } + auto result = socket_adapter_->move(scaled_position, scaled_speed, scaled_force); + if (!std::get<0>(result)) { + throw std::runtime_error("Failed to send move command to Robotiq gripper."); + } - // Read the state of the gripper. - int raw_position = socket_adapter_->position(); - gripper_current_state_.store(convertRawToPosition(raw_position)); + // Read the state of the gripper. + int raw_position = socket_adapter_->position(); + gripper_current_state_.store(convertRawToPosition(raw_position)); + } + catch (const std::exception& e) + { + RCLCPP_ERROR(logger_, "Error in background task: %s", e.what()); + } + + std::this_thread::sleep_for(gripperCommsLoopPeriod); // Sleep for a predefined period } - catch (const std::exception& e) - { - RCLCPP_ERROR(logger_, "Error in background task: %s", e.what()); - } - - std::this_thread::sleep_for(gripperCommsLoopPeriod); // Sleep for a predefined period - } } @@ -364,7 +357,7 @@ double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) { return std::max(min_position_, std::min(scaled_position, max_position_)); } -uint8_t RobotiqGripperHardwareInterface::convertPositionToRaw(double position) { +int RobotiqGripperHardwareInterface::convertPositionToRaw(double position) { if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) { throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly."); } @@ -372,7 +365,7 @@ uint8_t RobotiqGripperHardwareInterface::convertPositionToRaw(double position) { return static_cast(std::clamp(scaled, 0.0, 255.0)); } -uint8_t RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) { +int RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) { if (std::isnan(min_speed_) || std::isnan(max_speed_) || min_speed_ >= max_speed_) { throw std::runtime_error("Invalid gripper speed limits: min_speed_ or max_speed_ are not configured correctly."); } @@ -380,7 +373,7 @@ uint8_t RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) { return static_cast(std::clamp(scaled, 0.0, 255.0)); } -uint8_t RobotiqGripperHardwareInterface::convertForceToRaw(double force) { +int RobotiqGripperHardwareInterface::convertForceToRaw(double force) { if (std::isnan(min_force_) || std::isnan(max_force_) || min_force_ >= max_force_) { throw std::runtime_error("Invalid gripper force limits: min_force_ or max_force_ are not configured correctly."); } @@ -392,4 +385,4 @@ uint8_t RobotiqGripperHardwareInterface::convertForceToRaw(double force) { #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(robotiq_2f_interface::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(robotiq::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface) From d2de869e867ad801fc38fb1ea98e872479f3ff8a Mon Sep 17 00:00:00 2001 From: Niko Date: Fri, 12 Apr 2024 17:07:41 +0200 Subject: [PATCH 5/7] Mock system running Controllers not tested --- .../config/robotiq_2f_85_config.yaml | 19 +- .../config/robotiq_controllers.yaml | 20 ++ .../launch/robotiq_control.launch.py | 148 +++++++++++ .../launch/view_gripper.launch.py | 75 ++++++ .../rviz/view_urdf.rviz | 234 ++++++++++++++++++ .../urdf/robotiq_2f_140.ros2_control.xacro | 38 +-- .../urdf/robotiq_2f_140_macro.urdf.xacro | 27 +- .../urdf/robotiq_2f_85.ros2_control.xacro | 21 +- .../urdf/robotiq_2f_85.urdf.xacro | 2 +- .../urdf/robotiq_2f_85_macro.urdf.xacro | 29 ++- .../robotiq_2f_interface/CMakeLists.txt | 2 +- .../hardware_interface_plugin.xml | 4 +- .../MockRobotiq2fSocketAdapter.hpp | 5 +- .../Robotiq2fSocketAdapter.hpp | 5 +- .../SocketAdapterBase.hpp | 5 +- .../hardware_interface.hpp | 31 ++- .../visibility_control.hpp | 36 +-- .../src/Robotiq2fSocketAdapter.cpp | 5 +- .../src/hardware_interface.cpp | 165 ++++++++---- 19 files changed, 738 insertions(+), 133 deletions(-) create mode 100644 src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz diff --git a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_85_config.yaml b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_85_config.yaml index 6a106a5..c0c1ee9 100644 --- a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_85_config.yaml +++ b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_85_config.yaml @@ -1,11 +1,8 @@ -robotiq_2f_140_gripper: - ros__parameters: - robot_ip: "192.168.1.11" - robot_port: 63352 - # Physical limits - min_position: 0.0 # Meters (fully closed) - max_position: 0.085 # Meters (fully open) - min_speed: 0.02 # Meters per second - max_speed: 0.15 # Meters per second - min_force: 20.0 # Newtons - max_force: 235.0 # Newtons \ No newline at end of file +robotiq_2f_gripper: + # Physical limits + min_position: 0.0 # Meters (fully closed) + max_position: 0.085 # Meters (fully open) + min_speed: 0.02 # Meters per second + max_speed: 0.15 # Meters per second + min_force: 20.0 # Newtons + max_force: 235.0 # Newtons \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml b/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml index e69de29..abf1af8 100644 --- a/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml +++ b/src/robotiq_2f/robotiq_2f_description/config/robotiq_controllers.yaml @@ -0,0 +1,20 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + robotiq_gripper_controller: + type: position_controllers/GripperActionController + robotiq_activation_controller: + type: robotiq_controllers/RobotiqActivationController + +robotiq_gripper_controller: + ros__parameters: + default: true + joint: finger_joint + use_effort_interface: true + use_speed_interface: true + +robotiq_activation_controller: + ros__parameters: + default: true \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py b/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py index e69de29..04a18dc 100644 --- a/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py +++ b/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py @@ -0,0 +1,148 @@ +# 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. + +import launch +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +from launch.conditions import IfCondition +import launch_ros +import os + + +def generate_launch_description(): + description_pkg_share = launch_ros.substitutions.FindPackageShare( + package="robotiq_description" + ).find("robotiq_2f_description") + default_model_path = os.path.join( + description_pkg_share, "urdf", "robotiq_2f_140.urdf.xacro" + ) + default_rviz_config_path = os.path.join( + description_pkg_share, "rviz", "view_urdf.rviz" + ) + + args = [] + args.append( + launch.actions.DeclareLaunchArgument( + name="model", + default_value=default_model_path, + description="Absolute path to gripper URDF file", + ) + ) + args.append( + launch.actions.DeclareLaunchArgument( + name="rvizconfig", + default_value=default_rviz_config_path, + description="Absolute path to rviz config file", + ) + ) + args.append( + launch.actions.DeclareLaunchArgument( + name="launch_rviz", default_value="true", description="Launch RViz?" + ) + ) + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + LaunchConfiguration("model"), + " ", + "use_fake_hardware:=true", + ] + ) + robot_description_param = { + "robot_description": launch_ros.parameter_descriptions.ParameterValue( + robot_description_content, value_type=str + ) + } + + controllers_file = "robotiq_controllers.yaml" + initial_joint_controllers = PathJoinSubstitution( + [description_pkg_share, "config", controllers_file] + ) + + control_node = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ + robot_description_param, + initial_joint_controllers, + ], + ) + + robot_state_publisher_node = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[robot_description_param], + ) + + rviz_node = launch_ros.actions.Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + arguments=["-d", LaunchConfiguration("rvizconfig")], + condition=IfCondition(LaunchConfiguration("launch_rviz")), + ) + + joint_state_broadcaster_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + robotiq_gripper_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["robotiq_gripper_controller", "-c", "/controller_manager"], + ) + + robotiq_activation_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["robotiq_activation_controller", "-c", "/controller_manager"], + ) + + nodes = [ + control_node, + robot_state_publisher_node, + joint_state_broadcaster_spawner, + robotiq_gripper_controller_spawner, + robotiq_activation_controller_spawner, + rviz_node, + ] + + return launch.LaunchDescription(args + nodes) \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py b/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py index e69de29..5983666 100644 --- a/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py +++ b/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py @@ -0,0 +1,75 @@ +import launch +from launch.substitutions import ( + Command, + FindExecutable, + LaunchConfiguration, + PathJoinSubstitution, +) +import launch_ros +import os + + +def generate_launch_description(): + pkg_share = launch_ros.substitutions.FindPackageShare( + package="robotiq_2f_description" + ).find("robotiq_2f_description") + default_model_path = os.path.join( + pkg_share, "urdf", "robotiq_2f_85.urdf.xacro" + ) + default_rviz_config_path = os.path.join(pkg_share, "rviz", "view_urdf.rviz") + + args = [] + args.append( + launch.actions.DeclareLaunchArgument( + name="model", + default_value=default_model_path, + description="Absolute path to gripper URDF file", + ) + ) + args.append( + launch.actions.DeclareLaunchArgument( + name="rvizconfig", + default_value=default_rviz_config_path, + description="Absolute path to rviz config file", + ) + ) + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + LaunchConfiguration("model"), + ] + ) + robot_description_param = { + "robot_description": launch_ros.parameter_descriptions.ParameterValue( + robot_description_content, value_type=str + ) + } + + robot_state_publisher_node = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[robot_description_param], + ) + + joint_state_publisher_node = launch_ros.actions.Node( + package="joint_state_publisher_gui", + executable="joint_state_publisher_gui", + ) + + rviz_node = launch_ros.actions.Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="screen", + arguments=["-d", LaunchConfiguration("rvizconfig")], + ) + + nodes = [ + robot_state_publisher_node, + joint_state_publisher_node, + rviz_node, + ] + + return launch.LaunchDescription(args + nodes) \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz b/src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz new file mode 100644 index 0000000..c23ac44 --- /dev/null +++ b/src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz @@ -0,0 +1,234 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Status1 + Splitter Ratio: 0.6264705657958984 + Tree Height: 555 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + arm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + dummy_link: + Alpha: 1 + Show Axes: false + Show Trail: false + end_effector_link: + Alpha: 1 + Show Axes: false + Show Trail: false + forearm_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + gripper_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_finger_dist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + left_finger_prox_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + lower_wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_finger_dist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + right_finger_prox_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + shoulder_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + table: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tool_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + upper_wrist_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + world: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.1567115783691406 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.09681572020053864 + Y: -0.10843408107757568 + Z: 0.1451336145401001 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1200 + X: 1989 + Y: 261 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 1eaaf26..b5b1cdc 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 @@ -4,27 +4,37 @@ + robotiq_2f_interface/RobotiqGripperHardwareInterface - robotiq_2f_interface/RobotiqGripperHardwareInterface ${fake_sensor_commands} 0.0 - - robotiq_2f_interface/RobotiqGripperHardwareInterface - 0.695 - ${robot_ip} - ${robot_port} - 1.0 - 0.5 - + 0.695 + ${robot_ip} + ${robot_port} + ${use_fake_hardware} + 1.0 + 0.5 + ${min_position} + ${max_position} + ${min_speed} + ${max_speed} + ${min_force} + ${max_force} @@ -37,7 +47,7 @@ - + ${prefix}finger_joint -1 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 dc70eee..9a15bff 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 @@ -1,6 +1,5 @@ - + + + + + + + + + + - + + min_position="${min_position}" + max_position="${max_position}" + min_speed="${min_speed}" + max_speed="${max_speed}" + min_force="${min_force}" + max_force="${max_force}"/> 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 f9bf678..1b51269 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 @@ -4,10 +4,16 @@ @@ -22,8 +28,15 @@ 0.7929 ${robot_ip} ${robot_port} + ${use_fake_hardware} 1.0 0.5 + ${min_position} + ${max_position} + ${min_speed} + ${max_speed} + ${min_force} + ${max_force} 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 498b7d8..ea8f448 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 @@ -1,7 +1,7 @@ - + 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 90ba2c1..f61540b 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 @@ -1,6 +1,5 @@ - + + + + + + + + + + - + + robot_ip="${robot_ip}" robot_port="${robot_port}" - min_position="${robotiq_2f_gripper.min_position}" - max_position="${robotiq_2f_gripper.max_position}" - min_speed="${robotiq_2f_gripper.min_speed}" - max_speed="${robotiq_2f_gripper.max_speed}" - min_force="${robotiq_2f_gripper.min_force}" - max_force="${robotiq_2f_gripper.max_force}"/> + min_position="${min_position}" + max_position="${max_position}" + min_speed="${min_speed}" + max_speed="${max_speed}" + min_force="${min_force}" + max_force="${max_force}"/> diff --git a/src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt b/src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt index cfdb1a4..1aaea4f 100644 --- a/src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt +++ b/src/robotiq_2f/robotiq_2f_interface/CMakeLists.txt @@ -1,4 +1,4 @@ -cmake_minimum_required(VERSION 3.5) +cmake_minimum_required(VERSION 3.8) project(robotiq_2f_interface) # Default to C++17 diff --git a/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml b/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml index 0788a67..2c3aa02 100644 --- a/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml +++ b/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml @@ -1,6 +1,6 @@ - + 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 index cd19292..bed6529 100644 --- 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 @@ -10,7 +10,8 @@ #include "robotiq_2f_interface/SocketAdapterBase.hpp" -namespace robotiq{ +namespace robotiq_interface +{ class MockRobotiq2fSocketAdapter : public SocketAdapterBase { public: MockRobotiq2fSocketAdapter() : isConnected(false), gripperPosition(0), gripperForce(0), gripperSpeed(0), isActive(false) {} @@ -89,6 +90,6 @@ private: std::cout << "Simulated movement to position " << targetPosition << " with speed " << speed << " and force " << force << std::endl; } }; -} // end namespace +} // end robotiq_interface #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 52020f4..8b952e0 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 @@ -27,7 +27,8 @@ #include "robotiq_2f_interface/SocketAdapterBase.hpp" -namespace robotiq{ +namespace robotiq_interface +{ // Enum for Gripper Status enum class GripperStatus { RESET = 0, @@ -354,6 +355,6 @@ private: const int MAX_RETRIES = 5; const int RETRY_DELAY_MS = 20; }; -} // end namespace +} // end robotiq_interface #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 index b455d2f..e7282a8 100644 --- 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 @@ -6,7 +6,8 @@ #include #include -namespace robotiq{ +namespace robotiq_interface +{ class SocketAdapterBase { public: virtual ~SocketAdapterBase() = default; @@ -21,5 +22,5 @@ public: virtual void deactivate() = 0; virtual std::tuple move(int position, int speed, int force) = 0; }; -} // end namespace +} // end robotiq_interface #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 7054954..9417b5e 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 @@ -21,32 +21,35 @@ #include #include #include + #include +#include #include -namespace robotiq{ +namespace robotiq_interface +{ class RobotiqGripperHardwareInterface : public hardware_interface::SystemInterface { public: // Constructors - ROBOTIQ_DRIVER_PUBLIC RobotiqGripperHardwareInterface(); - ROBOTIQ_DRIVER_PUBLIC ~RobotiqGripperHardwareInterface(); + ROBOTIQ_INTERFACE_PUBLIC RobotiqGripperHardwareInterface(); + ROBOTIQ_INTERFACE_PUBLIC ~RobotiqGripperHardwareInterface(); // ROBOTIQ_DRIVER_PUBLIC explicit RobotiqGripperHardwareInterface(std::unique_ptr socket_factory); // Lifecycle Management - ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override; - ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; - ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; - ROBOTIQ_DRIVER_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; + ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_init(const hardware_interface::HardwareInfo& info) override; + ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_activate(const rclcpp_lifecycle::State& previous_state) override; + ROBOTIQ_INTERFACE_PUBLIC CallbackReturn on_deactivate(const rclcpp_lifecycle::State& previous_state) override; // State and Command Interfaces - ROBOTIQ_DRIVER_PUBLIC std::vector export_state_interfaces() override; - ROBOTIQ_DRIVER_PUBLIC std::vector export_command_interfaces() override; + ROBOTIQ_INTERFACE_PUBLIC std::vector export_state_interfaces() override; + ROBOTIQ_INTERFACE_PUBLIC std::vector export_command_interfaces() override; // Data Access (Read/Write) - ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; - ROBOTIQ_DRIVER_PUBLIC hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override; + ROBOTIQ_INTERFACE_PUBLIC hardware_interface::return_type read(const rclcpp::Time& time, const rclcpp::Duration& period) override; + ROBOTIQ_INTERFACE_PUBLIC hardware_interface::return_type write(const rclcpp::Time& time, const rclcpp::Duration& period) override; protected: // Likely changes the access to protected from private // Members for driver interaction @@ -80,7 +83,9 @@ 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_; + rclcpp::Clock::SharedPtr clock_ = std::make_shared(RCL_ROS_TIME); + rclcpp::Time last_update_time_ = clock_ -> now(); + rclcpp::Time current_time_ = clock_ -> now(); double reactivate_gripper_cmd_ = 0.0; std::atomic reactivate_gripper_async_cmd_; @@ -107,6 +112,6 @@ protected: // Likely changes the access to protected from private // Logger const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface"); }; -} // end namespace +} // end robotiq_interface #endif //ROBOTIQ2F_HARDWARE_INTERFACE_HPP \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/visibility_control.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/visibility_control.hpp index a04ea16..ca7a6d6 100644 --- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/visibility_control.hpp +++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/visibility_control.hpp @@ -19,38 +19,38 @@ * library cannot have, but the consuming code must have inorder to link. */ -#ifndef ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_ -#define ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_ +#ifndef ROBOTIQ_INTERFACE__VISIBILITY_CONTROL_HPP_ +#define ROBOTIQ_INTERFACE__VISIBILITY_CONTROL_HPP_ // This logic was borrowed (then namespaced) from the examples on the gcc wiki: // https://gcc.gnu.org/wiki/Visibility #if defined _WIN32 || defined __CYGWIN__ #ifdef __GNUC__ -#define ROBOTIQ_DRIVER_EXPORT __attribute__((dllexport)) -#define ROBOTIQ_DRIVER_IMPORT __attribute__((dllimport)) +#define ROBOTIQ_INTERFACE_EXPORT __attribute__((dllexport)) +#define ROBOTIQ_INTERFACE_IMPORT __attribute__((dllimport)) #else -#define ROBOTIQ_DRIVER_EXPORT __declspec(dllexport) -#define ROBOTIQ_DRIVER_IMPORT __declspec(dllimport) +#define ROBOTIQ_INTERFACE_EXPORT __declspec(dllexport) +#define ROBOTIQ_INTERFACE_IMPORT __declspec(dllimport) #endif -#ifdef ROBOTIQ_DRIVER_BUILDING_DLL -#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_EXPORT +#ifdef ROBOTIQ_INTERFACE_BUILDING_DLL +#define ROBOTIQ_INTERFACE_PUBLIC ROBOTIQ_INTERFACE_EXPORT #else -#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_IMPORT +#define ROBOTIQ_INTERFACE_PUBLIC ROBOTIQ_INTERFACE_IMPORT #endif -#define ROBOTIQ_DRIVER_PUBLIC_TYPE ROBOTIQ_DRIVER_PUBLIC -#define ROBOTIQ_DRIVER_LOCAL +#define ROBOTIQ_INTERFACE_PUBLIC_TYPE ROBOTIQ_INTERFACE_PUBLIC +#define ROBOTIQ_INTERFACE_LOCAL #else -#define ROBOTIQ_DRIVER_EXPORT __attribute__((visibility("default"))) -#define ROBOTIQ_DRIVER_IMPORT +#define ROBOTIQ_INTERFACE_EXPORT __attribute__((visibility("default"))) +#define ROBOTIQ_INTERFACE_IMPORT #if __GNUC__ >= 4 -#define ROBOTIQ_DRIVER_PUBLIC __attribute__((visibility("default"))) -#define ROBOTIQ_DRIVER_LOCAL __attribute__((visibility("hidden"))) +#define ROBOTIQ_INTERFACE_PUBLIC __attribute__((visibility("default"))) +#define ROBOTIQ_INTERFACE_LOCAL __attribute__((visibility("hidden"))) #else -#define ROBOTIQ_DRIVER_PUBLIC -#define ROBOTIQ_DRIVER_LOCAL +#define ROBOTIQ_INTERFACE_PUBLIC +#define ROBOTIQ_INTERFACE_LOCAL #endif -#define ROBOTIQ_DRIVER_PUBLIC_TYPE +#define ROBOTIQ_INTERFACE_PUBLIC_TYPE #endif #endif // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_ \ No newline at end of file diff --git a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp index f21a245..435ed69 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 "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp" -namespace robotiq{ +namespace robotiq_interface +{ Robotiq2fSocketAdapter::Robotiq2fSocketAdapter() : sockfd_(new int(-1)) { } @@ -368,4 +369,4 @@ void Robotiq2fSocketAdapter::auto_calibrate(bool log) { << min_position_ << ", " << max_position_ << "]\n"; } } -} // end namespace \ No newline at end of file +} // end robotiq_interface \ 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 ed19dcb..a330b20 100644 --- a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp +++ b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp @@ -1,10 +1,39 @@ #include "robotiq_2f_interface/hardware_interface.hpp" -namespace robotiq +namespace robotiq_interface { + +const std::string robot_ip_parameter_name = "robot_ip"; +const std::string robot_ip_default = "192.168.1.1"; + +const std::string robot_port_parameter_name = "robot_port"; +const int robot_port_default = 63352; + +const std::string use_mock_hardware_parameter_name = "use_fake_hardware"; +const bool use_mock_hardware_default = true; + +const std::string min_position_parameter_name = "min_position"; +const double min_position_default = 0.01; + +const std::string max_position_parameter_name = "max_position"; +const double max_position_default = 0.0; + +const std::string min_speed_parameter_name = "min_speed"; +const double min_speed_default = 0.0; + +const std::string max_speed_parameter_name = "max_speed"; +const double max_speed_default = 0.0; + +const std::string min_force_parameter_name = "min_force"; +const double min_force_default = 0.0; + +const std::string max_force_parameter_name = "max_force"; +const double max_force_default = 0.0; + RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface() : communication_thread_is_running_(false) { +RCLCPP_INFO(logger_, "Constructor"); } RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface() @@ -21,7 +50,7 @@ RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface() hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info) { - RCLCPP_DEBUG(rclcpp::get_logger("RobotiqGripperHardwareInterface"), "on_init"); + RCLCPP_DEBUG(logger_, "on_init"); if (hardware_interface::SystemInterface::on_init(info) != CallbackReturn::SUCCESS) { @@ -30,20 +59,74 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons try { - 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")); - min_position_ = std::stod(info_.hardware_parameters.at("min_position")); - max_position_ = std::stod(info_.hardware_parameters.at("max_position")); - min_speed_ = std::stod(info_.hardware_parameters.at("min_speed")); - max_speed_ = std::stod(info_.hardware_parameters.at("max_speed")); - min_force_ = std::stod(info_.hardware_parameters.at("min_force")); - max_force_ = std::stod(info_.hardware_parameters.at("max_force")); + if (info.hardware_parameters.count(robot_ip_parameter_name) > 0) { + robot_ip_ = info.hardware_parameters.at(robot_ip_parameter_name); + } else { + robot_ip_ = robot_ip_default; + } + RCLCPP_INFO(logger_, "Accessing parameter: Robot IP, Value: %s", robot_ip_.c_str()); + + if (info.hardware_parameters.count(robot_port_parameter_name) > 0) { + robot_port_ = std::stoi(info.hardware_parameters.at(robot_port_parameter_name)); + } else { + robot_port_ = robot_port_default; + } + RCLCPP_INFO(logger_, "Accessing parameter: Robot Port, Value: %d", robot_port_); + + std::string param_value; + if (info.hardware_parameters.count(use_mock_hardware_parameter_name) > 0) { + param_value = info.hardware_parameters.at(use_mock_hardware_parameter_name); + use_mock_hardware_ = (param_value == "true" || param_value == "True"); + } else { + use_mock_hardware_ = use_mock_hardware_default; + } + RCLCPP_INFO(logger_, "Accessing parameter: use fake hardware , Value: %s", param_value.c_str()); + + if (info.hardware_parameters.count(min_position_parameter_name) > 0) { + min_position_ = std::stod(info.hardware_parameters.at(min_position_parameter_name)); + } else { + min_position_ = min_position_default; + } + RCLCPP_INFO(logger_, "Accessing parameter: min position, Value: %f", min_position_); + + if (info.hardware_parameters.count(max_position_parameter_name) > 0) { + max_position_ = std::stod(info.hardware_parameters.at(max_position_parameter_name)); + } else { + max_position_ = max_position_default; + } + RCLCPP_INFO(logger_, "Accessing parameter: max position, Value: %f", max_position_); + + if (info.hardware_parameters.count(min_speed_parameter_name) > 0) { + min_speed_ = std::stod(info.hardware_parameters.at(min_speed_parameter_name)); + } else { + min_speed_ = min_speed_default; + } + RCLCPP_INFO(logger_, "Accessing parameter: min speed, Value: %f", min_speed_); + + if (info.hardware_parameters.count(max_speed_parameter_name) > 0) { + max_speed_ = std::stod(info.hardware_parameters.at(max_speed_parameter_name)); + } else { + max_speed_ = max_speed_default; + } + RCLCPP_INFO(logger_, "Accessing parameter: max speed, Value: %f", max_speed_); + + if (info.hardware_parameters.count(min_force_parameter_name) > 0) { + min_force_ = std::stod(info.hardware_parameters.at(min_force_parameter_name)); + } else { + min_force_ = robot_port_default; + } + RCLCPP_INFO(logger_, "Accessing parameter: min force, Value: %f", min_force_); + + if (info.hardware_parameters.count(max_force_parameter_name) > 0) { + max_force_ = std::stod(info.hardware_parameters.at(max_force_parameter_name)); + } else { + max_force_ = max_force_default; + } + RCLCPP_INFO(logger_, "Accessing parameter: max force, Value: %f", max_force_); } catch (const std::exception& e) { - RCLCPP_FATAL(rclcpp::get_logger("RobotiqGripperHardwareInterface"), - "Failed to load parameters: %s", e.what()); + RCLCPP_FATAL(logger_, "Failed to load parameters: %s", e.what()); return CallbackReturn::ERROR; } @@ -103,7 +186,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure return CallbackReturn::ERROR; } - socket_adapter_ = std::make_unique(); + configureAdapter(use_mock_hardware_); if (!socket_adapter_->connect(robot_ip_, robot_port_)) { RCLCPP_ERROR(logger_, "Cannot connect to the Robotiq gripper at %s:%d", robot_ip_.c_str(), robot_port_); @@ -181,16 +264,14 @@ std::vector RobotiqGripperHardwareInterface: std::vector state_interfaces; state_interfaces.emplace_back( - hardware_interface::StateInterface( - info_.joints[0].name, hardware_interface::HW_IF_POSITION, &gripper_position_ - ) - ); - + 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_ - ) - ); + hardware_interface::StateInterface(info_.joints[0].name, hardware_interface::HW_IF_VELOCITY, &gripper_velocity_)); + + for (const auto& interface : state_interfaces) { + RCLCPP_DEBUG(logger_, "Exporting state interface for joint: %s type: %s", + interface.get_name().c_str(), interface.get_interface_name().c_str()); +} return state_interfaces; } @@ -230,41 +311,41 @@ std::vector RobotiqGripperHardwareInterfac command_interfaces.emplace_back( hardware_interface::CommandInterface("reactivate_gripper", "reactivate_gripper_response", &reactivate_gripper_response_)); + for (const auto& interface : command_interfaces) { + RCLCPP_DEBUG(logger_, "Exporting command interface for joint: %s type: %s", + interface.get_name().c_str(), interface.get_interface_name().c_str()); +} + return command_interfaces; } -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*/) { RCLCPP_DEBUG(logger_, "Reading state from the gripper"); - // Fetch current position and other states from the hardware + // Ensure that the node uses the appropriate clock source + + current_time_ = clock_->now(); + try { - int raw_position = socket_adapter_->position(); // This should be an existing method in your adapter + int raw_position = socket_adapter_->position(); double new_position = convertRawToPosition(raw_position); - rclcpp::Time current_time = time; if (!std::isnan(gripper_position_)) { - double time_difference = (current_time - last_update_time_).seconds(); // Calculate time difference in seconds + double time_difference = (current_time_ - last_update_time_).seconds(); if (time_difference > 0) { - gripper_velocity_ = (new_position - gripper_position_) / time_difference; // Calculate velocity + gripper_velocity_ = (new_position - gripper_position_) / time_difference; } } - gripper_position_ = new_position; // Update current position - last_update_time_ = current_time; // Update last update timestamp + gripper_position_ = new_position; + last_update_time_ = current_time_; } catch (const std::exception& e) { RCLCPP_ERROR(logger_, "Failed to read from Robotiq gripper: %s", e.what()); return hardware_interface::return_type::ERROR; } - // Handle reactivation command if set - if (!std::isnan(reactivate_gripper_cmd_) && reactivate_gripper_cmd_ != NO_NEW_CMD_) { - RCLCPP_INFO(logger_, "Sending gripper reactivation request."); - reactivate_gripper_async_cmd_.store(true); - reactivate_gripper_cmd_ = NO_NEW_CMD_; // Reset command - } - // Process asynchronous reactivation response if available if (reactivate_gripper_async_response_.load().has_value()) { reactivate_gripper_response_ = reactivate_gripper_async_response_.load().value(); @@ -339,10 +420,10 @@ void RobotiqGripperHardwareInterface::background_task() // Mock Hardware void RobotiqGripperHardwareInterface::configureAdapter(bool useMock) { if (useMock) { - RCLCPP_INFO(logger_, "Using mock Robotiq adapter"); + RCLCPP_INFO(logger_, "Using Mock Robotiq Adapter"); socket_adapter_ = std::make_unique(); } else { - RCLCPP_INFO(logger_, "Using real Robotiq adapter"); + RCLCPP_INFO(logger_, "Using Real Robotiq Adapter"); socket_adapter_ = std::make_unique(); } } @@ -381,8 +462,8 @@ int RobotiqGripperHardwareInterface::convertForceToRaw(double force) { return static_cast(std::clamp(scaled, 0.0, 255.0)); } -} // namespace robotiq_driver +} // namespace robotiq_interface #include "pluginlib/class_list_macros.hpp" -PLUGINLIB_EXPORT_CLASS(robotiq::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface) +PLUGINLIB_EXPORT_CLASS(robotiq_interface::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface) From 7ce6aaa70730f899c9453bb98c33d2819c9f594c Mon Sep 17 00:00:00 2001 From: ligerfotis Date: Fri, 12 Apr 2024 17:28:51 +0200 Subject: [PATCH 6/7] keyboard servo for ur and gripper --- .idea/.gitignore | 2 + src/__init__.py | 0 src/servo_keyboard/CMakeLists.txt | 52 +++ src/servo_keyboard/LICENSE | 202 +++++++++ src/servo_keyboard/package.xml | 26 ++ .../src/servo_keyboard_input.cpp | 389 ++++++++++++++++++ .../launch/ur_robotiq_control.launch.py | 38 +- src/ur_robotiq_servo/__init__.py | 0 src/ur_robotiq_servo/config/joy-params.yaml | 3 +- src/ur_robotiq_servo/config/kb-params.yaml | 6 + .../launch/kb_servo.launch.py | 46 +++ src/ur_robotiq_servo/package.xml | 1 + src/ur_robotiq_servo/servo_keyboard_input.py | 196 +++++++++ src/ur_robotiq_servo/setup.py | 4 +- .../ur_robotiq_servo/kb_control.py | 142 +++++++ 15 files changed, 1104 insertions(+), 3 deletions(-) create mode 100644 src/__init__.py create mode 100644 src/servo_keyboard/CMakeLists.txt create mode 100644 src/servo_keyboard/LICENSE create mode 100644 src/servo_keyboard/package.xml create mode 100644 src/servo_keyboard/src/servo_keyboard_input.cpp create mode 100644 src/ur_robotiq_servo/__init__.py create mode 100644 src/ur_robotiq_servo/config/kb-params.yaml create mode 100644 src/ur_robotiq_servo/launch/kb_servo.launch.py create mode 100644 src/ur_robotiq_servo/servo_keyboard_input.py create mode 100644 src/ur_robotiq_servo/ur_robotiq_servo/kb_control.py diff --git a/.idea/.gitignore b/.idea/.gitignore index 13566b8..a9d7db9 100644 --- a/.idea/.gitignore +++ b/.idea/.gitignore @@ -6,3 +6,5 @@ # Datasource local storage ignored files /dataSources/ /dataSources.local.xml +# GitHub Copilot persisted chat sessions +/copilot/chatSessions diff --git a/src/__init__.py b/src/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/servo_keyboard/CMakeLists.txt b/src/servo_keyboard/CMakeLists.txt new file mode 100644 index 0000000..5fba1ea --- /dev/null +++ b/src/servo_keyboard/CMakeLists.txt @@ -0,0 +1,52 @@ +cmake_minimum_required(VERSION 3.8) +project(servo_keyboard) + +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(rclcpp REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(control_msgs REQUIRED) +find_package(controller_manager_msgs REQUIRED) + +add_executable(servo_keyboard_input src/servo_keyboard_input.cpp) +target_include_directories(servo_keyboard_input PUBLIC + $ + $) +target_compile_features(servo_keyboard_input PUBLIC c_std_99 cxx_std_17) # Require C99 and C++17 + +# Include directories +target_include_directories(servo_keyboard_input PRIVATE + ${rclcpp_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ${control_msgs_INCLUDE_DIRS} + ${controller_manager_msgs_INCLUDE_DIRS} +) + +# Link the executable with required libraries +ament_target_dependencies(servo_keyboard_input + rclcpp + geometry_msgs + control_msgs + controller_manager_msgs +) + +install(TARGETS servo_keyboard_input + DESTINATION lib/${PROJECT_NAME}) + +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/servo_keyboard/LICENSE b/src/servo_keyboard/LICENSE new file mode 100644 index 0000000..d645695 --- /dev/null +++ b/src/servo_keyboard/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/servo_keyboard/package.xml b/src/servo_keyboard/package.xml new file mode 100644 index 0000000..9014e35 --- /dev/null +++ b/src/servo_keyboard/package.xml @@ -0,0 +1,26 @@ + + + + servo_keyboard + 0.0.0 + TODO: Package description + Fotios Lyegrakis + Apache-2.0 + + ament_cmake + + + rclcpp + control_msgs + geometry_msgs + controller_manager_msgs + controller_manager_msgs + + + ament_lint_auto + ament_lint_common + + + ament_cmake + + diff --git a/src/servo_keyboard/src/servo_keyboard_input.cpp b/src/servo_keyboard/src/servo_keyboard_input.cpp new file mode 100644 index 0000000..e29dc4a --- /dev/null +++ b/src/servo_keyboard/src/servo_keyboard_input.cpp @@ -0,0 +1,389 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, PickNik LLC + * All rights reserved. + * + * 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 PickNik LLC 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 OWNER 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. + *********************************************************************/ + +/* Title : servo_keyboard_input.cpp + * Project : moveit2_tutorials + * Created : 05/31/2021 + * Author : Adam Pettinger + */ + +#include +#include +#include +#include // Add necessary includes +#include // Add necessary includes + +#include +#include +#include +#include + +// Define used keys +#define KEYCODE_LEFT 0x44 +#define KEYCODE_RIGHT 0x43 +#define KEYCODE_UP 0x41 +#define KEYCODE_DOWN 0x42 +#define KEYCODE_PERIOD 0x2E +#define KEYCODE_SEMICOLON 0x3B +#define KEYCODE_E 0x65 +#define KEYCODE_W 0x77 +#define KEYCODE_1 0x31 +#define KEYCODE_2 0x32 +#define KEYCODE_3 0x33 +#define KEYCODE_4 0x34 +#define KEYCODE_5 0x35 +#define KEYCODE_6 0x36 +#define KEYCODE_R 0x72 +#define KEYCODE_Q 0x71 +#define KEYCODE_PLUS 0x2B // Keycode for the plus sign (+) +#define KEYCODE_MINUS 0x2D // Keycode for the minus sign (-) +#define KEYCODE_GRIPPER 0x67 // Keycode for the gripper control button (g) + +// Some constants used in the Servo Teleop demo +const std::string TWIST_TOPIC = "/servo_node/delta_twist_cmds"; +const std::string JOINT_TOPIC = "/servo_node/delta_joint_cmds"; +const std::string GRIPPER_TOPIC = "/forward_gripper_position_controller/commands"; +const size_t ROS_QUEUE_SIZE = 10; +const std::string EEF_FRAME_ID = "world"; +const std::string BASE_FRAME_ID = "tool0"; + +// A class for reading the key inputs from the terminal +class KeyboardReader +{ +public: + KeyboardReader() : kfd(0) + { + // get the console in raw mode + tcgetattr(kfd, &cooked); + struct termios raw; + memcpy(&raw, &cooked, sizeof(struct termios)); + raw.c_lflag &= ~(ICANON | ECHO); + // Setting a new line, then end of file + raw.c_cc[VEOL] = 1; + raw.c_cc[VEOF] = 2; + tcsetattr(kfd, TCSANOW, &raw); + } + void readOne(char* c) + { + int rc = read(kfd, c, 1); + if (rc < 0) + { + throw std::runtime_error("read failed"); + } + } + void shutdown() + { + tcsetattr(kfd, TCSANOW, &cooked); + } + +private: + int kfd; + struct termios cooked; +}; + +// Converts key-presses to Twist or Jog commands for Servo, in lieu of a controller +class KeyboardServo +{ +public: + KeyboardServo(); + int keyLoop(); + +private: + void spin(); + + void handlePlusPress(); + void handleMinusPress(); + + void publishGripperCommand(double finger_joint_angle); + + rclcpp::Node::SharedPtr nh_; + + rclcpp::Publisher::SharedPtr twist_pub_; + rclcpp::Publisher::SharedPtr joint_pub_; + rclcpp::Publisher::SharedPtr gripper_cmd_pub_; + + std::string frame_to_publish_; + double joint_vel_cmd_; + double gripper_speed_multiplier_; + double last_finger_joint_angle_; + double gripper_lower_limit_; + double gripper_upper_limit_; +}; + +KeyboardServo::KeyboardServo() + : frame_to_publish_(BASE_FRAME_ID), + joint_vel_cmd_(1.0), + gripper_speed_multiplier_(0.01), // Example value, adjust as needed + last_finger_joint_angle_(0.0), // Example value, adjust as needed + gripper_lower_limit_(0.0), // Example value, adjust as needed + gripper_upper_limit_(0.70) // Example value, adjust as needed +{ + nh_ = rclcpp::Node::make_shared("servo_keyboard_input"); + + twist_pub_ = nh_->create_publisher(TWIST_TOPIC, ROS_QUEUE_SIZE); + joint_pub_ = nh_->create_publisher(JOINT_TOPIC, ROS_QUEUE_SIZE); + gripper_cmd_pub_ = nh_->create_publisher(GRIPPER_TOPIC, ROS_QUEUE_SIZE); +} + +KeyboardReader input; + +void KeyboardServo::publishGripperCommand(double finger_joint_angle) +{ + auto msg = std::make_unique(); + msg->data.push_back(finger_joint_angle); + gripper_cmd_pub_->publish(std::move(msg)); +} + +void quit(int sig) +{ + (void)sig; + input.shutdown(); + rclcpp::shutdown(); + exit(0); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + KeyboardServo keyboard_servo; + + signal(SIGINT, quit); + + int rc = keyboard_servo.keyLoop(); + input.shutdown(); + rclcpp::shutdown(); + + return rc; +} + +void KeyboardServo::spin() +{ + while (rclcpp::ok()) + { + rclcpp::spin_some(nh_); + } +} + +void KeyboardServo::handlePlusPress() +{ + // Calculate the new finger joint angle + double delta_angle = 1.0 * gripper_speed_multiplier_; + double new_finger_joint_angle = last_finger_joint_angle_ + delta_angle; +// printf("New finger joint angle: %f\n", new_finger_joint_angle); + // Check if the new angle is within the limits + if (new_finger_joint_angle <= gripper_upper_limit_) + { + // Update the finger joint angle + last_finger_joint_angle_ = new_finger_joint_angle; +// printf("New finger joint angle: %f\n", last_finger_joint_angle_); + // Publish the gripper command with the new angle + publishGripperCommand(last_finger_joint_angle_); + } +} + +void KeyboardServo::handleMinusPress() +{ + // Calculate the new finger joint angle + double delta_angle = -1.0 * gripper_speed_multiplier_; + double new_finger_joint_angle = last_finger_joint_angle_ + delta_angle; + + // Check if the new angle is within the limits + if (new_finger_joint_angle >= gripper_lower_limit_) + { + // Update the finger joint angle + last_finger_joint_angle_ = new_finger_joint_angle; + + // Publish the gripper command with the new angle + publishGripperCommand(last_finger_joint_angle_); + } +} + + + +int KeyboardServo::keyLoop() +{ + char c; + bool publish_twist = false; + bool publish_joint = false; + + std::thread{ std::bind(&KeyboardServo::spin, this) }.detach(); + + puts("Reading from keyboard"); + puts("---------------------------"); + puts("Use arrow keys and the '.' and ';' keys to Cartesian jog"); + puts("Use 'W' to Cartesian jog in the world frame, and 'E' for the End-Effector frame"); + puts("Use 1|2|3|4|5|6|7 keys to joint jog. 'R' to reverse the direction of jogging."); + puts("Use '+' to open the gripper, '-' to close it."); + puts("'Q' to quit."); + + + for (;;) + { + // get the next event from the keyboard + try + { + input.readOne(&c); + } + catch (const std::runtime_error&) + { + perror("read():"); + return -1; + } + + RCLCPP_DEBUG(nh_->get_logger(), "value: 0x%02X\n", c); + + // // Create the messages we might publish + auto twist_msg = std::make_unique(); + auto joint_msg = std::make_unique(); + + // Use read key-press + switch (c) + { + case KEYCODE_LEFT: + RCLCPP_DEBUG(nh_->get_logger(), "LEFT"); + twist_msg->twist.linear.y = -1.0; + publish_twist = true; + break; + case KEYCODE_RIGHT: + RCLCPP_DEBUG(nh_->get_logger(), "RIGHT"); + twist_msg->twist.linear.y = 1.0; + publish_twist = true; + break; + case KEYCODE_UP: + RCLCPP_DEBUG(nh_->get_logger(), "UP"); + twist_msg->twist.linear.x = 1.0; + publish_twist = true; + break; + case KEYCODE_DOWN: + RCLCPP_DEBUG(nh_->get_logger(), "DOWN"); + twist_msg->twist.linear.x = -1.0; + publish_twist = true; + break; + case KEYCODE_PERIOD: + RCLCPP_DEBUG(nh_->get_logger(), "PERIOD"); + twist_msg->twist.linear.z = -1.0; + publish_twist = true; + break; + case KEYCODE_SEMICOLON: + RCLCPP_DEBUG(nh_->get_logger(), "SEMICOLON"); + twist_msg->twist.linear.z = 1.0; + publish_twist = true; + break; + case KEYCODE_E: + RCLCPP_DEBUG(nh_->get_logger(), "E"); + frame_to_publish_ = EEF_FRAME_ID; + break; + case KEYCODE_W: + RCLCPP_DEBUG(nh_->get_logger(), "W"); + frame_to_publish_ = BASE_FRAME_ID; + break; + case KEYCODE_1: + RCLCPP_DEBUG(nh_->get_logger(), "1"); + joint_msg->joint_names.push_back("shoulder_lift_joint"); + joint_msg->velocities.push_back(joint_vel_cmd_); + publish_joint = true; + break; + case KEYCODE_2: + RCLCPP_DEBUG(nh_->get_logger(), "2"); + joint_msg->joint_names.push_back("shoulder_pan_joint"); + joint_msg->velocities.push_back(joint_vel_cmd_); + publish_joint = true; + break; + case KEYCODE_3: + RCLCPP_DEBUG(nh_->get_logger(), "3"); + joint_msg->joint_names.push_back("elbow_joint"); + joint_msg->velocities.push_back(joint_vel_cmd_); + publish_joint = true; + break; + case KEYCODE_4: + RCLCPP_DEBUG(nh_->get_logger(), "4"); + joint_msg->joint_names.push_back("wrist_1_joint"); + joint_msg->velocities.push_back(joint_vel_cmd_); + publish_joint = true; + break; + case KEYCODE_5: + RCLCPP_DEBUG(nh_->get_logger(), "5"); + joint_msg->joint_names.push_back("wrist_2_joint"); + joint_msg->velocities.push_back(joint_vel_cmd_); + publish_joint = true; + break; + case KEYCODE_6: + RCLCPP_DEBUG(nh_->get_logger(), "6"); + joint_msg->joint_names.push_back("wrist_3_joint"); + joint_msg->velocities.push_back(joint_vel_cmd_); + publish_joint = true; + break; +// case KEYCODE_7: +// RCLCPP_DEBUG(nh_->get_logger(), "7"); +// joint_msg->joint_names.push_back("finger_joint"); +// joint_msg->velocities.push_back(joint_vel_cmd_); +// publish_joint = true; +// break; + case KEYCODE_R: + RCLCPP_DEBUG(nh_->get_logger(), "R"); + joint_vel_cmd_ *= -1; + break; + case KEYCODE_Q: + RCLCPP_DEBUG(nh_->get_logger(), "quit"); + return 0; + // Add cases for other keys as needed + case KEYCODE_PLUS: + RCLCPP_DEBUG(nh_->get_logger(), "PLUS"); + handlePlusPress(); + break; + case KEYCODE_MINUS: + RCLCPP_DEBUG(nh_->get_logger(), "MINUS"); + handleMinusPress(); + break; + } + + // If a key requiring a publish was pressed, publish the message now + if (publish_twist) + { + twist_msg->header.stamp = nh_->now(); + twist_msg->header.frame_id = frame_to_publish_; + twist_pub_->publish(std::move(twist_msg)); + publish_twist = false; + } + else if (publish_joint) + { + joint_msg->header.stamp = nh_->now(); + joint_msg->header.frame_id = BASE_FRAME_ID; + joint_pub_->publish(std::move(joint_msg)); + publish_joint = false; + } + } + + return 0; +} \ No newline at end of file diff --git a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py index 776bc8a..616a956 100644 --- a/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py +++ b/src/ur_robotiq_description/launch/ur_robotiq_control.launch.py @@ -6,7 +6,8 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument, OpaqueFunction from launch.conditions import IfCondition, UnlessCondition from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution, TextSubstitution - +# from moveit_configs_utils import MoveItConfigsBuilder +# from launch_param_builder import ParameterBuilder def launch_setup(context, *args, **kwargs): # Initialize Arguments @@ -170,6 +171,7 @@ def launch_setup(context, *args, **kwargs): robot_description = {"robot_description": robot_description_content} + initial_joint_controllers = PathJoinSubstitution( [FindPackageShare(description_package), "config", controllers_file] ) @@ -275,6 +277,38 @@ def launch_setup(context, *args, **kwargs): }, ], ) + ############################################################################################################ + # # Get parameters for the Servo node + # servo_params = ( + # ParameterBuilder("ur_robotiq_servo") + # .yaml( + # parameter_namespace="moveit_servo", + # file_path="config/ur_robotiq_simulated_config.yaml", + # ) + # .to_dict() + # ) + # print(servo_params) + # + # # A node to publish world -> panda_link0 transform + # static_tf = Node( + # package="tf2_ros", + # executable="static_transform_publisher", + # name="static_transform_publisher", + # output="log", + # arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + # ) + # + # # The servo cpp interface demo + # # Creates the Servo node and publishes commands to it + # servo_node = Node( + # package="moveit2_tutorials", + # executable="servo_cpp_interface_demo", + # output="screen", + # parameters=[ + # servo_params, + # robot_description, + # ], + # ) robot_state_publisher_node = Node( package="robot_state_publisher", @@ -353,6 +387,8 @@ def launch_setup(context, *args, **kwargs): ur_control_node, dashboard_client_node, controller_stopper_node, + # static_tf, + # servo_node, robot_state_publisher_node, rviz_node, initial_joint_controller_spawner_stopped, diff --git a/src/ur_robotiq_servo/__init__.py b/src/ur_robotiq_servo/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/ur_robotiq_servo/config/joy-params.yaml b/src/ur_robotiq_servo/config/joy-params.yaml index 8ad2d13..f8747b6 100644 --- a/src/ur_robotiq_servo/config/joy-params.yaml +++ b/src/ur_robotiq_servo/config/joy-params.yaml @@ -1,7 +1,8 @@ joy_node: ros__parameters: device_id: 0 - device_name: "PS5 Controller" +# device_name: "PS5 Controller" + device_name: "Keyboard" deadzone: 0.5 autorepeat_rate: 20.0 sticky_buttons: false diff --git a/src/ur_robotiq_servo/config/kb-params.yaml b/src/ur_robotiq_servo/config/kb-params.yaml new file mode 100644 index 0000000..acc2b1d --- /dev/null +++ b/src/ur_robotiq_servo/config/kb-params.yaml @@ -0,0 +1,6 @@ +keyboard_servo_node: + ros__parameters: + linear_speed_multiplier: 0.2 + gripper_speed_multiplier: 0.02 + gripper_lower_limit: 0.0 + gripper_upper_limit: 0.70 \ No newline at end of file diff --git a/src/ur_robotiq_servo/launch/kb_servo.launch.py b/src/ur_robotiq_servo/launch/kb_servo.launch.py new file mode 100644 index 0000000..14e3a40 --- /dev/null +++ b/src/ur_robotiq_servo/launch/kb_servo.launch.py @@ -0,0 +1,46 @@ +import os + +from launch import LaunchDescription +from launch_ros.actions import Node +import ament_index_python.packages +from launch_param_builder import ParameterBuilder +from moveit_configs_utils import MoveItConfigsBuilder + +def generate_launch_description(): + config_directory = os.path.join( + ament_index_python.packages.get_package_share_directory('ur_robotiq_servo'), + 'config') + joy_params = os.path.join(config_directory, 'joy-params.yaml') + ps5_params = os.path.join(config_directory, 'ps5-params.yaml') + + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + # Get parameters for the Servo node + servo_params = ( + ParameterBuilder("moveit_servo") + .yaml( + parameter_namespace="moveit_servo", + file_path="config/panda_simulated_config.yaml", + ) + .to_dict() + ) + + # The servo cpp interface demo + # Creates the Servo node and publishes commands to it + servo_node = Node( + package="moveit2_tutorials", + executable="servo_cpp_interface_demo", + output="screen", + parameters=[ + servo_params, + moveit_config.robot_description, + moveit_config.robot_description_semantic, + ], + ) + + return LaunchDescription([ + servo_node + ]) \ No newline at end of file diff --git a/src/ur_robotiq_servo/package.xml b/src/ur_robotiq_servo/package.xml index 3099325..88eaedd 100644 --- a/src/ur_robotiq_servo/package.xml +++ b/src/ur_robotiq_servo/package.xml @@ -14,6 +14,7 @@ std_srvs joy moveit_servo + keyboard ament_copyright ament_flake8 diff --git a/src/ur_robotiq_servo/servo_keyboard_input.py b/src/ur_robotiq_servo/servo_keyboard_input.py new file mode 100644 index 0000000..4e3dc49 --- /dev/null +++ b/src/ur_robotiq_servo/servo_keyboard_input.py @@ -0,0 +1,196 @@ +#!/usr/bin/env python3 + +import sys +import tty +import termios +import threading +import signal +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import TwistStamped +from control_msgs.msg import JointJog + +# Define key codes +KEYCODE_RIGHT = 0x43 +KEYCODE_LEFT = 0x44 +KEYCODE_UP = 0x41 +KEYCODE_DOWN = 0x42 +KEYCODE_PERIOD = 0x2E +KEYCODE_SEMICOLON = 0x3B +KEYCODE_1 = 0x31 +KEYCODE_2 = 0x32 +KEYCODE_3 = 0x33 +KEYCODE_4 = 0x34 +KEYCODE_5 = 0x35 +KEYCODE_6 = 0x36 +KEYCODE_7 = 0x37 +KEYCODE_Q = 0x71 +KEYCODE_R = 0x72 +KEYCODE_J = 0x6A +KEYCODE_T = 0x74 +KEYCODE_W = 0x77 +KEYCODE_E = 0x65 + +# Constants used in the Servo Teleop demo +TWIST_TOPIC = "/servo_node/delta_twist_cmds" +JOINT_TOPIC = "/servo_node/delta_joint_cmds" +ROS_QUEUE_SIZE = 10 +PLANNING_FRAME_ID = "world" +EE_FRAME_ID = "tool0" + + +class KeyboardReader: + def __init__(self): + self.file_descriptor = sys.stdin.fileno() + self.old_settings = termios.tcgetattr(self.file_descriptor) + tty.setraw(self.file_descriptor) + + def read_one(self): + return sys.stdin.read(1) + + def shutdown(self): + termios.tcsetattr(self.file_descriptor, termios.TCSADRAIN, self.old_settings) + + +class KeyboardServo: + def __init__(self): + self.joint_vel_cmd = 1.0 + self.command_frame_id = PLANNING_FRAME_ID + self.node = rclpy.create_node("servo_keyboard_input") + self.twist_pub = self.node.create_publisher(TwistStamped, TWIST_TOPIC, ROS_QUEUE_SIZE) + self.joint_pub = self.node.create_publisher(JointJog, JOINT_TOPIC, ROS_QUEUE_SIZE) + self.switch_input = self.node.create_client(ServoCommandType, "servo_node/switch_command_type") + self.request = ServoCommandType.Request() + + def spin(self): + rclpy.spin(self.node) + + def key_loop(self): + publish_twist = False + publish_joint = False + print("Reading from keyboard") + print("---------------------------") + print("All commands are in the planning frame") + print("Use arrow keys and the '.' and ';' keys to Cartesian jog") + print("Use 1|2|3|4|5|6|7 keys to joint jog. 'r' to reverse the direction of jogging.") + print("Use 'j' to select joint jog.") + print("Use 't' to select twist") + print("Use 'w' and 'e' to switch between sending command in planning frame or end effector frame") + print("'Q' to quit.") + + while True: + c = input() + + twist_msg = TwistStamped() + joint_msg = JointJog() + joint_msg.joint_names = [ + "shoulder_lift_joint", + "shoulder_pan_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_2_joint", + "finger_joint", + ] + joint_msg.velocities = [0.0] * 7 + + # Use read key-press + if c == '\x1b': # ANSI escape sequence + c = input() + if c == '[': + c = input() + if c == 'A': + twist_msg.twist.linear.x = 0.5 # UP + publish_twist = True + elif c == 'B': + twist_msg.twist.linear.x = -0.5 # DOWN + publish_twist = True + elif c == 'C': + twist_msg.twist.linear.y = 0.5 # RIGHT + publish_twist = True + elif c == 'D': + twist_msg.twist.linear.y = -0.5 # LEFT + publish_twist = True + elif ord(c) == KEYCODE_PERIOD: + twist_msg.twist.linear.z = -0.5 # '.' + publish_twist = True + elif ord(c) == KEYCODE_SEMICOLON: + twist_msg.twist.linear.z = 0.5 # ';' + publish_twist = True + elif ord(c) == KEYCODE_1: + joint_msg.velocities[0] = self.joint_vel_cmd # '1' + publish_joint = True + elif ord(c) == KEYCODE_2: + joint_msg.velocities[1] = self.joint_vel_cmd # '2' + publish_joint = True + elif ord(c) == KEYCODE_3: + joint_msg.velocities[2] = self.joint_vel_cmd # '3' + publish_joint = True + elif ord(c) == KEYCODE_4: + joint_msg.velocities[3] = self.joint_vel_cmd # '4' + publish_joint = True + elif ord(c) == KEYCODE_5: + joint_msg.velocities[4] = self.joint_vel_cmd # '5' + publish_joint = True + elif ord(c) == KEYCODE_6: + joint_msg.velocities[5] = self.joint_vel_cmd # '6' + publish_joint = True + elif ord(c) == KEYCODE_7: + joint_msg.velocities[6] = self.joint_vel_cmd # '7' + publish_joint = True + elif ord(c) == KEYCODE_R: + self.joint_vel_cmd *= -1 # 'r' + elif ord(c) == KEYCODE_J: + self.request.command_type = ServoCommandType.Request.JOINT_JOG # 'j' + if self.switch_input.wait_for_service(timeout_sec=1): + result = self.switch_input.call(self.request) + if result.success: + print("Switched to input type: JointJog") + else: + print("Could not switch input to: JointJog") + elif ord(c) == KEYCODE_T: + self.request.command_type = ServoCommandType.Request.TWIST # 't' + if self.switch_input.wait_for_service(timeout_sec=1): + result = self.switch_input.call(self.request) + if result.success: + print("Switched to input type: Twist") + else: + print("Could not switch input to: Twist") + elif ord(c) == KEYCODE_W: + print(f"Command frame set to: {PLANNING_FRAME_ID}") # 'w' + self.command_frame_id = PLANNING_FRAME_ID + elif ord(c) == KEYCODE_E: + print(f"Command frame set to: {EE_FRAME_ID}") # 'e' + self.command_frame_id = EE_FRAME_ID + elif ord(c) == KEYCODE_Q: + print("Quit") # 'Q' + return 0 + + # If a key requiring a publish was pressed, publish the message now + if publish_twist: + twist_msg.header.stamp = self.node.get_clock().now().to_msg() + twist_msg.header.frame_id = self.command_frame_id + self.twist_pub.publish(twist_msg) + publish_twist = False + elif publish_joint: + joint_msg.header.stamp = self.node.get_clock().now().to_msg() + joint_msg.header.frame_id = PLANNING_FRAME_ID + self.joint_pub.publish(joint_msg) + publish_joint = False + + return 0 + + +def quit_handler(sig, frame): + input_reader.shutdown() + rclpy.shutdown() + sys.exit(0) + + +if __name__ == "__main__": + signal.signal(signal.SIGINT, quit_handler) + rclpy.init(args=sys.argv) + input_reader = KeyboardReader() + servo_keyboard = KeyboardServo() + threading.Thread(target=servo_keyboard.spin).start() + servo_keyboard.key_loop() diff --git a/src/ur_robotiq_servo/setup.py b/src/ur_robotiq_servo/setup.py index d91e499..1df7211 100644 --- a/src/ur_robotiq_servo/setup.py +++ b/src/ur_robotiq_servo/setup.py @@ -24,7 +24,9 @@ setup( tests_require=['pytest'], entry_points={ 'console_scripts': [ - 'ps5_servo = ur_robotiq_servo.ps5_control:main' + 'ps5_servo = ur_robotiq_servo.ps5_control:main', + 'kb_servo = ur_robotiq_servo.kb_control:main', + # 'servo_kb_input = ur_robotiq_servo.servo_keyboard_input:main', ], }, ) diff --git a/src/ur_robotiq_servo/ur_robotiq_servo/kb_control.py b/src/ur_robotiq_servo/ur_robotiq_servo/kb_control.py new file mode 100644 index 0000000..bb7c843 --- /dev/null +++ b/src/ur_robotiq_servo/ur_robotiq_servo/kb_control.py @@ -0,0 +1,142 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import Joy, JointState +from geometry_msgs.msg import TwistStamped +from control_msgs.msg import JointJog +from std_srvs.srv import Trigger +from std_msgs.msg import Float64MultiArray, MultiArrayDimension, MultiArrayLayout +import keyboard + +class KBControllerNode(Node): + def __init__(self): + super().__init__('kb_controller_node') + # States + self.mode = 'twist' # Initialize mode to 'twist'. Alternatives: 'twist', 'joint' + self.last_button_state = 0 # Track the last state of the toggle button to detect presses + self.last_finger_joint_angle = 0.0 + + # Parameters + self.linear_speed_multiplier = self.declare_parameter('linear_speed_multiplier', 1.0) + self.linear_speed_multiplier = self.get_parameter('linear_speed_multiplier').get_parameter_value().double_value + self.get_logger().info(f"Linear speed multiplier: {self.linear_speed_multiplier}") + + self.use_fake_hardware = self.declare_parameter('use_fake_hardware', False) + self.use_fake_hardware = self.get_parameter('use_fake_hardware').get_parameter_value().bool_value + self.get_logger().info(f"Use fake hardware: {self.use_fake_hardware}") + + self.gripper_speed_multiplier = self.declare_parameter('gripper_speed_multiplier', 1.0) + self.gripper_speed_multiplier = (self.get_parameter('gripper_speed_multiplier') + .get_parameter_value().double_value) + self.get_logger().info(f"Gripper speed multiplier: {self.gripper_speed_multiplier}") + + self.gripper_lower_limit = self.declare_parameter('gripper_lower_limit', 1.0) + self.gripper_lower_limit = (self.get_parameter('gripper_lower_limit') + .get_parameter_value().double_value) + self.get_logger().info(f"Gripper lower limit: {self.gripper_lower_limit}") + + self.gripper_upper_limit = self.declare_parameter('gripper_upper_limit', 1.0) + self.gripper_upper_limit = (self.get_parameter('gripper_upper_limit') + .get_parameter_value().double_value) + self.get_logger().info(f"Gripper upper limit: {self.gripper_upper_limit}") + # Subscriber and Publisher + self.joint_state_sub = self.create_subscription( + JointState, + '/joint_states', + self.joint_state_callback, + 10) + + self.joy_sub = self.create_subscription( + Joy, + '/joy', + self.keyboard_callback, + 10) + + self.twist_pub = self.create_publisher( + TwistStamped, + '/servo_node/delta_twist_cmds', + 10) + + self.joint_pub = self.create_publisher( + JointJog, + '/servo_node/delta_joint_cmds', + 10) + + self.gripper_cmd_pub = self.create_publisher( + Float64MultiArray, + '/forward_gripper_position_controller/commands', + 10) + + # Services + self.servo_client = self.create_client(Trigger, '/servo_node/start_servo') + + srv_msg = Trigger.Request() + while not self.servo_client.wait_for_service(timeout_sec=1.0): + self.get_logger().info('/servo_node/start_servo service not available, waiting again...') + + self.call_start_servo() + + self.get_logger().info('kb_servo_node started!') + + def call_start_servo(self): + request = Trigger.Request() + future = self.servo_client.call_async(request) + rclpy.spin_until_future_complete(self, future) + response = future.result() + if response.success: + self.get_logger().info(f'Successfully called start_servo: {response.message}') + else: + self.get_logger().info('Failed to call start_servo') + + def joint_state_callback(self, msg): + try: + index = msg.name.index('finger_joint') + self.last_finger_joint_angle = msg.position[index] + except ValueError: + self.get_logger().error('finger_joint not found in /joint_states msg') + + def keyboard_callback(self, event): + # Process keyboard events + print("Key pressed") + if event.event_type == keyboard.KEY_DOWN: + # Handle key presses + if event.name == 't': + # Toggle mode between twist and joint control + self.mode = 'joint' if self.mode == 'twist' else 'twist' + self.get_logger().info(f'Mode switched to: {self.mode}') + elif event.name == 'w': + # Move forward + self.publish_twist(1.0 * self.linear_speed_multiplier) # Adjust speed as needed + self.get_logger().info('Moving forward') + elif event.name == 's': + # Move backward + self.publish_twist(-1.0 * self.linear_speed_multiplier) # Adjust speed as needed + self.get_logger().info('Moving backward') + elif event.event_type == keyboard.KEY_UP: + # Handle key releases + if event.name == 'w' or event.name == 's': + # Stop moving + self.publish_twist(0.0) + self.get_logger().info('Stopped moving') + + def publish_twist(self, linear_speed): + twist_msg = TwistStamped() + twist_msg.header.frame_id = 'tool0' + twist_msg.header.stamp = self.get_clock().now().to_msg() + twist_msg.twist.linear.x = linear_speed + twist_msg.twist.linear.y = 0.0 # Adjust as needed + twist_msg.twist.linear.z = 0.0 # Adjust as needed + twist_msg.twist.angular.x = 0.0 + twist_msg.twist.angular.y = 0.0 + twist_msg.twist.angular.z = 0.0 + self.twist_pub.publish(twist_msg) + +def main(args=None): + rclpy.init(args=args) + node = KBControllerNode() + rclpy.spin(node) + node.destroy_node() + rclpy.shutdown() + + +if __name__ == '__main__': + main() \ No newline at end of file From 08bec7a499b6927e73d4f0cf63c0ebfbaa876f58 Mon Sep 17 00:00:00 2001 From: Niko Feith Date: Tue, 16 Apr 2024 08:42:24 +0000 Subject: [PATCH 7/7] =?UTF-8?q?Dateien=20hochladen=20nach=20=E2=80=9Esrc/u?= =?UTF-8?q?r=5Frobotiq=5Fdescription/config=E2=80=9C?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../config/ur_robotiq_calibration.yaml | 44 +++++++++++++++++++ 1 file changed, 44 insertions(+) create mode 100644 src/ur_robotiq_description/config/ur_robotiq_calibration.yaml diff --git a/src/ur_robotiq_description/config/ur_robotiq_calibration.yaml b/src/ur_robotiq_description/config/ur_robotiq_calibration.yaml new file mode 100644 index 0000000..526dffb --- /dev/null +++ b/src/ur_robotiq_description/config/ur_robotiq_calibration.yaml @@ -0,0 +1,44 @@ +kinematics: + shoulder: + x: 0 + y: 0 + z: 0.1519161231793138 + roll: -0 + pitch: 0 + yaw: -8.3688888041777445e-08 + upper_arm: + x: -9.8255319984929057e-05 + y: 0 + z: 0 + roll: 1.5713473529266058 + pitch: 0 + yaw: 4.5372573280303068e-06 + forearm: + x: -0.24361145101605494 + y: 0 + z: 0 + roll: 3.138959939545904 + pitch: 3.141417818890341 + yaw: 3.1415861917625292 + wrist_1: + x: -0.21328351386382516 + y: -0.00058695912107010507 + z: 0.13086135281159214 + roll: 0.0044853210843803608 + pitch: 0.00071590512460444685 + yaw: -2.0286176909971606e-06 + wrist_2: + x: 9.0086536104922084e-05 + y: -0.085314349728116412 + z: -6.1195228207677147e-05 + roll: 1.5715136178239859 + pitch: 0 + yaw: 1.3915445690113049e-06 + wrist_3: + x: -0.00014076969524819022 + y: 0.092309359181826575 + z: -0.00011628039579221433 + roll: 1.5695366459205147 + pitch: 3.1415926535897931 + yaw: 3.1415925648453848 + hash: calib_2535041911463403862 \ No newline at end of file