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/launch/robotiq_control.launch.py b/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py new file mode 100644 index 0000000..04a18dc --- /dev/null +++ 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 new file mode 100644 index 0000000..5983666 --- /dev/null +++ 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_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) diff --git a/src/servo_keyboard/package.xml b/src/servo_keyboard/package.xml index 9014e35..391b053 100644 --- a/src/servo_keyboard/package.xml +++ b/src/servo_keyboard/package.xml @@ -11,11 +11,17 @@ rclcpp +<<<<<<<< HEAD:src/servo_keyboard/package.xml control_msgs geometry_msgs controller_manager_msgs controller_manager_msgs +======== + rclcpp_lifecycle + hardware_interface + pluginlib +>>>>>>>> origin/main:src/robotiq_2f/robotiq_2f_interface/package.xml ament_lint_auto ament_lint_common