Niko/UR_Robotiq
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

working keyboard example

Browse Source
This commit is contained in:
ligerfotis 2024-04-16 13:33:52 +02:00
parent 8b3c40bbe7
commit 7a3c6fcc2e
12 changed files with 20 additions and 767 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

148
src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py
View File

@ -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)

75
src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py
View File

@ -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)

234
src/robotiq_2f/robotiq_2f_description/rviz/view_urdf.rviz
View File

@ -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: <Fixed 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: <Fixed 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

9
src/robotiq_2f/robotiq_controllers/CHANGELOG.rst
View File

@ -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

84
src/robotiq_2f/robotiq_controllers/CMakeLists.txt
View File

@ -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()

7
src/robotiq_2f/robotiq_controllers/controller_plugins.xml
View File

@ -1,7 +0,0 @@
<library path="robotiq_controllers">
<class name="robotiq_controllers/RobotiqActivationController" type="robotiq_controllers::RobotiqActivationController" base_class_type="controller_interface::ControllerInterface">
<description>
This controller provides an interface to (re-)activate the Robotiq gripper.
</description>
</class>
</library>

64
src/robotiq_2f/robotiq_controllers/include/robotiq_controllers/robotiq_activation_controller.hpp
View File

@ -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<std_srvs::srv::Trigger>::SharedPtr reactivate_gripper_srv_;
};
} // namespace robotiq_controllers

23
src/robotiq_2f/robotiq_controllers/package.xml
View File

@ -1,23 +0,0 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>robotiq_controllers</name>
<version>0.0.1</version>
<description>Controllers for the Robotiq gripper.</description>
<maintainer email="alex.moriarty@picknik.ai">Alex Moriarty</maintainer>
<maintainer email="marq.rasmussen@picknik.ai">Marq Rasmussen</maintainer>
<license>BSD 3-Clause</license>
<author>Cory Crean</author>
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>controller_interface</depend>
<depend>std_srvs</depend>
<test_depend>ament_lint_auto</test_depend>
<test_depend>ament_lint_common</test_depend>
<export>
<build_type>ament_cmake</build_type>
</export>
</package>

123
src/robotiq_2f/robotiq_controllers/src/robotiq_activation_controller.cpp
View File

@ -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<std_srvs::srv::Trigger>(
"~/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)

1
src/ros2_robotiq_gripper Submodule

@ -0,0 +1 @@
Subproject commit b6136bdc866a929bfb096890ca61dde1335ffd30

1
src/serial Submodule

@ -0,0 +1 @@
Subproject commit d8d160678aa0b31cdf467c052b954fa287cc6cdf

18
src/servo_keyboard/src/servo_keyboard_input.cpp
View File

@ -43,6 +43,7 @@
#include <control_msgs/msg/joint_jog.hpp> #include <control_msgs/msg/joint_jog.hpp>
#include <controller_manager_msgs/srv/switch_controller.hpp> // Add necessary includes #include <controller_manager_msgs/srv/switch_controller.hpp> // Add necessary includes
#include <std_msgs/msg/float64_multi_array.hpp> // Add necessary includes #include <std_msgs/msg/float64_multi_array.hpp> // Add necessary includes
#include <sensor_msgs/msg/joint_state.hpp> // Add necessary include for JointState message
#include <signal.h> #include <signal.h>
#include <stdio.h> #include <stdio.h>
@ -132,6 +133,7 @@ private:
rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_; rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_pub_;
rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_; rclcpp::Publisher<control_msgs::msg::JointJog>::SharedPtr joint_pub_;
rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_cmd_pub_; rclcpp::Publisher<std_msgs::msg::Float64MultiArray>::SharedPtr gripper_cmd_pub_;
rclcpp::Subscription<sensor_msgs::msg::JointState>::SharedPtr joint_state_sub_;
std::string frame_to_publish_; std::string frame_to_publish_;
double joint_vel_cmd_; double joint_vel_cmd_;
@ -154,10 +156,26 @@ KeyboardServo::KeyboardServo()
twist_pub_ = nh_->create_publisher<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, ROS_QUEUE_SIZE); twist_pub_ = nh_->create_publisher<geometry_msgs::msg::TwistStamped>(TWIST_TOPIC, ROS_QUEUE_SIZE);
joint_pub_ = nh_->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, ROS_QUEUE_SIZE); joint_pub_ = nh_->create_publisher<control_msgs::msg::JointJog>(JOINT_TOPIC, ROS_QUEUE_SIZE);
gripper_cmd_pub_ = nh_->create_publisher<std_msgs::msg::Float64MultiArray>(GRIPPER_TOPIC, ROS_QUEUE_SIZE); gripper_cmd_pub_ = nh_->create_publisher<std_msgs::msg::Float64MultiArray>(GRIPPER_TOPIC, ROS_QUEUE_SIZE);
joint_state_sub_ = nh_->create_subscription<sensor_msgs::msg::JointState>(
"/joint_states", ROS_QUEUE_SIZE,
std::bind(&KeyboardServo::jointStateCallback, this, std::placeholders::_1));
} }
KeyboardReader input; 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) void KeyboardServo::publishGripperCommand(double finger_joint_angle)
{ {
auto msg = std::make_unique<std_msgs::msg::Float64MultiArray>(); auto msg = std::make_unique<std_msgs::msg::Float64MultiArray>();