From 7a3c6fcc2ed083e87496d4f3c37c1e8cef45c6a6 Mon Sep 17 00:00:00 2001 From: ligerfotis Date: Tue, 16 Apr 2024 13:33:52 +0200 Subject: [PATCH] working keyboard example --- .../launch/robotiq_control.launch.py | 148 ----------- .../launch/view_gripper.launch.py | 75 ------ .../rviz/view_urdf.rviz | 234 ------------------ .../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 --------- src/ros2_robotiq_gripper | 1 + src/serial | 1 + .../src/servo_keyboard_input.cpp | 18 ++ 12 files changed, 20 insertions(+), 767 deletions(-) delete mode 100644 src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py delete mode 100644 src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py delete mode 100644 src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz delete mode 100644 src/robotiq_2f/robotiq_controllers/CHANGELOG.rst delete mode 100644 src/robotiq_2f/robotiq_controllers/CMakeLists.txt delete mode 100644 src/robotiq_2f/robotiq_controllers/controller_plugins.xml delete mode 100644 src/robotiq_2f/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp delete mode 100644 src/robotiq_2f/robotiq_controllers/package.xml delete mode 100644 src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp create mode 160000 src/ros2_robotiq_gripper create mode 160000 src/serial 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 deleted file mode 100644 index 04a18dc..0000000 --- a/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py +++ /dev/null @@ -1,148 +0,0 @@ -# Copyright (c) 2022 PickNik, Inc. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions are met: -# -# * Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# -# * Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in the -# documentation and/or other materials provided with the distribution. -# -# * Neither the name of the {copyright_holder} nor the names of its -# contributors may be used to endorse or promote products derived from -# this software without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. - -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 deleted file mode 100644 index 5983666..0000000 --- a/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py +++ /dev/null @@ -1,75 +0,0 @@ -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 deleted file mode 100644 index c23ac44..0000000 --- a/src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz +++ /dev/null @@ -1,234 +0,0 @@ -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_controllers/CHANGELOG.rst b/src/robotiq_2f/robotiq_controllers/CHANGELOG.rst deleted file mode 100644 index 467d0a3..0000000 --- a/src/robotiq_2f/robotiq_controllers/CHANGELOG.rst +++ /dev/null @@ -1,9 +0,0 @@ -^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -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 deleted file mode 100644 index c4334bc..0000000 --- a/src/robotiq_2f/robotiq_controllers/CMakeLists.txt +++ /dev/null @@ -1,84 +0,0 @@ -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 deleted file mode 100644 index 9c5c8f5..0000000 --- a/src/robotiq_2f/robotiq_controllers/controller_plugins.xml +++ /dev/null @@ -1,7 +0,0 @@ - - - - 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 deleted file mode 100644 index d0dac29..0000000 --- a/src/robotiq_2f/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp +++ /dev/null @@ -1,64 +0,0 @@ -// Copyright (c) 2022 PickNik, Inc. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#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 deleted file mode 100644 index 562b784..0000000 --- a/src/robotiq_2f/robotiq_controllers/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - - 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 deleted file mode 100644 index aa63d21..0000000 --- a/src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp +++ /dev/null @@ -1,123 +0,0 @@ -// Copyright (c) 2022 PickNik, Inc. -// -// Redistribution and use in source and binary forms, with or without -// modification, are permitted provided that the following conditions are met: -// -// * Redistributions of source code must retain the above copyright -// notice, this list of conditions and the following disclaimer. -// -// * Redistributions in binary form must reproduce the above copyright -// notice, this list of conditions and the following disclaimer in the -// documentation and/or other materials provided with the distribution. -// -// * Neither the name of the {copyright_holder} nor the names of its -// contributors may be used to endorse or promote products derived from -// this software without specific prior written permission. -// -// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" -// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE -// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE -// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE -// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR -// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF -// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS -// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN -// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) -// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -// POSSIBILITY OF SUCH DAMAGE. - -#include "robotiq_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) diff --git a/src/ros2_robotiq_gripper b/src/ros2_robotiq_gripper new file mode 160000 index 0000000..b6136bd --- /dev/null +++ b/src/ros2_robotiq_gripper @@ -0,0 +1 @@ +Subproject commit b6136bdc866a929bfb096890ca61dde1335ffd30 diff --git a/src/serial b/src/serial new file mode 160000 index 0000000..d8d1606 --- /dev/null +++ b/src/serial @@ -0,0 +1 @@ +Subproject commit d8d160678aa0b31cdf467c052b954fa287cc6cdf diff --git a/src/servo_keyboard/src/servo_keyboard_input.cpp b/src/servo_keyboard/src/servo_keyboard_input.cpp index e29dc4a..04cee63 100644 --- a/src/servo_keyboard/src/servo_keyboard_input.cpp +++ b/src/servo_keyboard/src/servo_keyboard_input.cpp @@ -43,6 +43,7 @@ #include #include // Add necessary includes #include // Add necessary includes +#include // Add necessary include for JointState message #include #include @@ -132,6 +133,7 @@ private: rclcpp::Publisher::SharedPtr twist_pub_; rclcpp::Publisher::SharedPtr joint_pub_; rclcpp::Publisher::SharedPtr gripper_cmd_pub_; + rclcpp::Subscription::SharedPtr joint_state_sub_; std::string frame_to_publish_; double joint_vel_cmd_; @@ -154,10 +156,26 @@ KeyboardServo::KeyboardServo() 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); + joint_state_sub_ = nh_->create_subscription( + "/joint_states", ROS_QUEUE_SIZE, + std::bind(&KeyboardServo::jointStateCallback, this, std::placeholders::_1)); } KeyboardReader input; +// Define callback function for joint states subscriber +void jointStateCallback(const sensor_msgs::msg::JointState::SharedPtr msg) +{ + // Find index of the finger joint in the joint state message + auto it = std::find(msg->name.begin(), msg->name.end(), "finger_joint"); + if (it != msg->name.end()) + { + size_t index = std::distance(msg->name.begin(), it); + // Update last_finger_joint_angle_ with the corresponding value from the message + last_finger_joint_angle_ = msg->position[index]; + } +} + void KeyboardServo::publishGripperCommand(double finger_joint_angle) { auto msg = std::make_unique();