From d2de869e867ad801fc38fb1ea98e872479f3ff8a Mon Sep 17 00:00:00 2001 From: Niko Date: Fri, 12 Apr 2024 17:07:41 +0200 Subject: [PATCH] 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)