diff --git a/.idea/vcs.xml b/.idea/vcs.xml
index 588e956..4dc5475 100644
--- a/.idea/vcs.xml
+++ b/.idea/vcs.xml
@@ -5,7 +5,5 @@
-
-
\ No newline at end of file
diff --git a/src/robotiq_2f/robotiq_2f_description/CMakeLists.txt b/src/robotiq_2f/robotiq_2f_description/CMakeLists.txt
index 20cbfc6..a4d9266 100644
--- a/src/robotiq_2f/robotiq_2f_description/CMakeLists.txt
+++ b/src/robotiq_2f/robotiq_2f_description/CMakeLists.txt
@@ -5,22 +5,16 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()
-# find dependencies
find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package( REQUIRED)
-
-if(BUILD_TESTING)
- find_package(ament_lint_auto REQUIRED)
- # the following line skips the linter which checks for copyrights
- # comment the line when a copyright and license is added to all source files
- set(ament_cmake_copyright_FOUND TRUE)
- # the following line skips cpplint (only works in a git repo)
- # comment the line when this package is in a git repo and when
- # a copyright and license is added to all source files
- set(ament_cmake_cpplint_FOUND TRUE)
- ament_lint_auto_find_test_dependencies()
-endif()
+install(
+ DIRECTORY
+ config
+ launch
+ meshes
+ rviz
+ urdf
+ DESTINATION
+ share/${PROJECT_NAME}
+)
ament_package()
diff --git a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml
index ce06c22..b38c13c 100644
--- a/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml
+++ b/src/robotiq_2f/robotiq_2f_description/config/robotiq_2f_140_config.yaml
@@ -1,9 +1,8 @@
-robotiq_2f_140_gripper:
- ros__parameters:
- # Physical limits
- min_position: 0.0 # Meters (fully closed)
- max_position: 0.14 # Meters (fully open)
- min_speed: 0.02 # Meters per second
- max_speed: 0.15 # Meters per second
- min_force: 20.0 # Newtons
- max_force: 235.0 # Newtons
\ No newline at end of file
+robotiq_2f_gripper:
+ # Physical limits
+ min_position: 0.0 # Meters (fully closed)
+ max_position: 0.14 # Meters (fully open)
+ min_speed: 0.02 # Meters per second
+ max_speed: 0.15 # Meters per second
+ min_force: 20.0 # Newtons
+ max_force: 235.0 # Newtons
\ No newline at end of file
diff --git a/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py b/src/robotiq_2f/robotiq_2f_description/launch/robotiq_control.launch.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py b/src/robotiq_2f/robotiq_2f_description/launch/view_gripper.launch.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/robotiq_2f/robotiq_2f_description/package.xml b/src/robotiq_2f/robotiq_2f_description/package.xml
index 4118eee..59e3d99 100644
--- a/src/robotiq_2f/robotiq_2f_description/package.xml
+++ b/src/robotiq_2f/robotiq_2f_description/package.xml
@@ -9,6 +9,20 @@
ament_cmake
+ joint_state_publisher_gui
+ launch
+ launch_ros
+ robot_state_publisher
+ rviz2
+ urdf
+ xacro
+
+ ros2_control
+ ros2_controllers
+
+ gripper_controllers
+ joint_state_broadcaster
+
ament_lint_auto
ament_lint_common
diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro
index ee21c97..1eaaf26 100644
--- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro
+++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140.ros2_control.xacro
@@ -13,7 +13,7 @@
- mock_components/GenericSystem
+ robotiq_2f_interface/RobotiqGripperHardwareInterface
${fake_sensor_commands}
0.0
diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro
index c9509e6..dc70eee 100644
--- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro
+++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_140_macro.urdf.xacro
@@ -1,5 +1,6 @@
+
+ robot_port="${robot_port}"
+ min_position="${robotiq_2f_gripper.min_position}"
+ max_position="${robotiq_2f_gripper.max_position}"
+ min_speed="${robotiq_2f_gripper.min_speed}"
+ max_speed="${robotiq_2f_gripper.max_speed}"
+ min_force="${robotiq_2f_gripper.min_force}"
+ max_force="${robotiq_2f_gripper.max_force}"/>
diff --git a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro
index b4bc13c..90ba2c1 100644
--- a/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro
+++ b/src/robotiq_2f/robotiq_2f_description/urdf/robotiq_2f_85_macro.urdf.xacro
@@ -1,5 +1,6 @@
+
- robot_port="${robot_port}"/>
+ robot_port="${robot_port}"
+ min_position="${robotiq_2f_gripper.min_position}"
+ max_position="${robotiq_2f_gripper.max_position}"
+ min_speed="${robotiq_2f_gripper.min_speed}"
+ max_speed="${robotiq_2f_gripper.max_speed}"
+ min_force="${robotiq_2f_gripper.min_force}"
+ max_force="${robotiq_2f_gripper.max_force}"/>
diff --git a/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml b/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml
index 5245a01..0788a67 100644
--- a/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml
+++ b/src/robotiq_2f/robotiq_2f_interface/hardware_interface_plugin.xml
@@ -1,6 +1,6 @@
ROS2 controller for the Robotiq gripper.
diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp
index d2b7fe4..cd19292 100644
--- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp
+++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp
@@ -30,15 +30,6 @@ public:
isConnected.store(false);
}
- int getGripperPosition() override {
- return gripperPosition.load();
- }
-
- int getGripperForce() override {
- // Assuming this is supposed to mimic `force()`
- return gripperForce.load();
- }
-
int force() override {
return gripperForce.load();
}
diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp
index 394da7b..52020f4 100644
--- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp
+++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/Robotiq2fSocketAdapter.hpp
@@ -237,7 +237,7 @@ private:
// Smart pointer to manage the socket descriptor with the custom deleter
std::unique_ptr sockfd_;
- std::mutex socketMutex_; // Mutex for socket access synchronization
+ std::mutex socket_mutex_; // Mutex for socket access synchronization
// bounds of the encoded gripper states
int min_position_ = 0;
@@ -351,6 +351,8 @@ private:
// Constants buffer sizes
const size_t BUFFER_SIZE = 1024; // Adjust as necessary
+ const int MAX_RETRIES = 5;
+ const int RETRY_DELAY_MS = 20;
};
} // end namespace
diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp
index ee9394c..7054954 100644
--- a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp
+++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/hardware_interface.hpp
@@ -10,13 +10,17 @@
#include
#include
+#include "robotiq_2f_interface/visibility_control.hpp"
+
#include "robotiq_2f_interface/SocketAdapterBase.hpp"
#include "robotiq_2f_interface/Robotiq2fSocketAdapter.hpp"
#include "robotiq_2f_interface/MockRobotiq2fSocketAdapter.hpp"
-#include
+#include
+#include
+#include
#include
-
+#include
#include
#include
@@ -48,11 +52,12 @@ protected: // Likely changes the access to protected from private
// Members for driver interaction
bool use_mock_hardware_;
std::unique_ptr socket_adapter_;
- void RobotiqGripperHardwareInterface::configureAdapter(bool useMock);
+ void configureAdapter(bool useMock);
// Background communication thread
std::thread communication_thread_;
std::atomic communication_thread_is_running_;
+ std::mutex resource_mutex_;
void background_task();
static constexpr double NO_NEW_CMD_ = std::numeric_limits::quiet_NaN();
@@ -97,10 +102,10 @@ protected: // Likely changes the access to protected from private
double max_force_;
// loop time
- constexpr auto gripperCommsLoopPeriod = std::chrono::milliseconds{ 10 };
+ const std::chrono::milliseconds gripperCommsLoopPeriod = std::chrono::milliseconds{ 10 };
// Logger
- const auto logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
+ const rclcpp::Logger logger_ = rclcpp::get_logger("RobotiqGripperHardwareInterface");
};
} // end namespace
diff --git a/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/visibility_control.hpp b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/visibility_control.hpp
new file mode 100644
index 0000000..a04ea16
--- /dev/null
+++ b/src/robotiq_2f/robotiq_2f_interface/include/robotiq_2f_interface/visibility_control.hpp
@@ -0,0 +1,56 @@
+// Copyright 2017 Open Source Robotics Foundation, Inc.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+// http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+/* This header must be included by all rclcpp headers which declare symbols
+ * which are defined in the rclcpp library. When not building the rclcpp
+ * library, i.e. when using the headers in other package's code, the contents
+ * of this header change the visibility of certain symbols which the rclcpp
+ * library cannot have, but the consuming code must have inorder to link.
+ */
+
+#ifndef ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_
+#define ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_
+
+// This logic was borrowed (then namespaced) from the examples on the gcc wiki:
+// https://gcc.gnu.org/wiki/Visibility
+
+#if defined _WIN32 || defined __CYGWIN__
+#ifdef __GNUC__
+#define ROBOTIQ_DRIVER_EXPORT __attribute__((dllexport))
+#define ROBOTIQ_DRIVER_IMPORT __attribute__((dllimport))
+#else
+#define ROBOTIQ_DRIVER_EXPORT __declspec(dllexport)
+#define ROBOTIQ_DRIVER_IMPORT __declspec(dllimport)
+#endif
+#ifdef ROBOTIQ_DRIVER_BUILDING_DLL
+#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_EXPORT
+#else
+#define ROBOTIQ_DRIVER_PUBLIC ROBOTIQ_DRIVER_IMPORT
+#endif
+#define ROBOTIQ_DRIVER_PUBLIC_TYPE ROBOTIQ_DRIVER_PUBLIC
+#define ROBOTIQ_DRIVER_LOCAL
+#else
+#define ROBOTIQ_DRIVER_EXPORT __attribute__((visibility("default")))
+#define ROBOTIQ_DRIVER_IMPORT
+#if __GNUC__ >= 4
+#define ROBOTIQ_DRIVER_PUBLIC __attribute__((visibility("default")))
+#define ROBOTIQ_DRIVER_LOCAL __attribute__((visibility("hidden")))
+#else
+#define ROBOTIQ_DRIVER_PUBLIC
+#define ROBOTIQ_DRIVER_LOCAL
+#endif
+#define ROBOTIQ_DRIVER_PUBLIC_TYPE
+#endif
+
+#endif // ROBOTIQ_DRIVER__VISIBILITY_CONTROL_HPP_
\ No newline at end of file
diff --git a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp
index 1228fae..f21a245 100644
--- a/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp
+++ b/src/robotiq_2f/robotiq_2f_interface/src/Robotiq2fSocketAdapter.cpp
@@ -12,7 +12,7 @@ Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
// Connection and disconnection
bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
- std::lock_guard lock(socketMutex_);
+ std::lock_guard lock(socket_mutex_);
if (*sockfd_ >= 0) {
std::cerr << "Already connected. Disconnecting to reconnect.\n";
disconnect();
@@ -48,7 +48,7 @@ bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
}
void Robotiq2fSocketAdapter::disconnect() {
- std::lock_guard lock(socketMutex_);
+ std::lock_guard lock(socket_mutex_);
if (*sockfd_ >= 0) {
if (close(*sockfd_) == -1) {
std::cerr << "Error closing socket: " << strerror(errno) << "\n";
@@ -61,7 +61,7 @@ void Robotiq2fSocketAdapter::disconnect() {
// Utility methods
bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
- std::lock_guard lock(socketMutex_);
+ std::lock_guard lock(socket_mutex_);
if (*sockfd_ < 0) {
std::cerr << "Attempted to send command on a disconnected socket.\n";
return false;
@@ -77,7 +77,7 @@ bool Robotiq2fSocketAdapter::sendCommand(const std::string& cmd) {
}
std::string Robotiq2fSocketAdapter::receiveResponse(int timeout_ms) {
- std::lock_guard lock(socketMutex_);
+ std::lock_guard lock(socket_mutex_);
if (*sockfd_ < 0) {
std::cerr << "Attempted to receive response on a disconnected socket.\n";
return "";
@@ -162,12 +162,12 @@ bool Robotiq2fSocketAdapter::setGripperVariables(const std::map lock(socketMutex_);
+ std::lock_guard lock(socket_mutex_);
if (*sockfd_ < 0) {
throw std::runtime_error("Cannot set variables on a disconnected socket.");
return false;
@@ -190,9 +190,9 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou
std::stringstream ss;
ss << value; // Convert the value to a string
- std::string cmd = SET_COMMAND + " " + commandVariables.at(variable) + " " + ss.str() + "\n";
+ std::string cmd = SET_COMMAND + " " + variable + " " + ss.str() + "\n";
- std::lock_guard lock(socketMutex_);
+ std::lock_guard lock(socket_mutex_);
if (*sockfd_ < 0) {
throw std::runtime_error("Cannot set variables on a disconnected socket.");
return false;
@@ -208,9 +208,9 @@ bool Robotiq2fSocketAdapter::setGripperVariable(const std::string& variable, dou
}
int Robotiq2fSocketAdapter::getGripperVariable(const std::string& variable) {
- std::string cmd = GET_COMMAND + " " + commandVariables.at(variable) + "\n";
+ std::string cmd = GET_COMMAND + " " + variable + "\n";
- std::lock_guard lock(socketMutex_);
+ std::lock_guard lock(socket_mutex_);
if (*sockfd_ < 0) {
throw std::runtime_error("Cannot get variables on a disconnected socket.");
return -1;
diff --git a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp
index d6b3365..ed19dcb 100644
--- a/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp
+++ b/src/robotiq_2f/robotiq_2f_interface/src/hardware_interface.cpp
@@ -9,14 +9,14 @@ RobotiqGripperHardwareInterface::RobotiqGripperHardwareInterface()
RobotiqGripperHardwareInterface::~RobotiqGripperHardwareInterface()
{
- if (communication_thread_is_running_.load())
- {
- communication_thread_is_running_.store(false);
- if (communication_thread_.joinable())
+ if (communication_thread_is_running_.load())
{
- communication_thread_.join();
+ communication_thread_is_running_.store(false);
+ if (communication_thread_.joinable())
+ {
+ communication_thread_.join();
+ }
}
- }
}
hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(const hardware_interface::HardwareInfo& info)
@@ -28,25 +28,17 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
return CallbackReturn::ERROR;
}
- // Loading paramters from Hardware info and .yaml file
- auto node = this->get_node();
- node->declare_parameter("min_position", 0.0);
- node->declare_parameter("max_position", 0.14);
- node->declare_parameter("min_speed", 0.02);
- node->declare_parameter("max_speed", 0.15);
- node->declare_parameter("min_force", 20.0);
- node->declare_parameter("max_force", 235.0);
try
{
- robot_ip_ = info_.hardware_parameters.at("robot_ip");
- robot_port_ = std::stod(info_.hardware_parameters.at("robot_port"));
+ robot_ip_ = info_.hardware_parameters.at("robot_ip");
+ robot_port_ = std::stod(info_.hardware_parameters.at("robot_port"));
use_mock_hardware_ = std::stod(info_.hardware_parameters.at("use_fake_hardware"));
- node->get_parameter("min_position", min_position_);
- node->get_parameter("max_position", max_position_);
- node->get_parameter("min_speed", min_speed_);
- node->get_parameter("max_speed", max_speed_);
- node->get_parameter("min_force", min_force_);
- node->get_parameter("max_force", max_force_);
+ min_position_ = std::stod(info_.hardware_parameters.at("min_position"));
+ max_position_ = std::stod(info_.hardware_parameters.at("max_position"));
+ min_speed_ = std::stod(info_.hardware_parameters.at("min_speed"));
+ max_speed_ = std::stod(info_.hardware_parameters.at("max_speed"));
+ min_force_ = std::stod(info_.hardware_parameters.at("min_force"));
+ max_force_ = std::stod(info_.hardware_parameters.at("max_force"));
}
catch (const std::exception& e)
{
@@ -101,7 +93,7 @@ hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_init(cons
return CallbackReturn::SUCCESS;
}
-hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& /*previous_state*/)
+hardware_interface::CallbackReturn RobotiqGripperHardwareInterface::on_configure(const rclcpp_lifecycle::State& previous_state)
{
RCLCPP_DEBUG(logger_, "on_configure");
try
@@ -305,41 +297,42 @@ hardware_interface::return_type RobotiqGripperHardwareInterface::write(const rcl
void RobotiqGripperHardwareInterface::background_task()
{
- while (communication_thread_is_running_.load())
- {
- std::lock_guard guard(resource_mutex);
- try
+ while (communication_thread_is_running_.load())
{
- // Re-activate the gripper
- // This can be used, for example, to re-run the auto-calibration.
- if (reactivate_gripper_async_cmd_.load())
- {
- socket_adapter_->deactivate();
- socket_adapter_->activate();
- reactivate_gripper_async_cmd_.store(false);
- reactivate_gripper_async_response_.store(true);
- }
+ std::lock_guard guard(resource_mutex_);
+ try
+ {
+ // Re-activate the gripper
+ // This can be used, for example, to re-run the auto-calibration.
+ if (reactivate_gripper_async_cmd_.load())
+ {
+ socket_adapter_->deactivate();
+ socket_adapter_->activate();
+ reactivate_gripper_async_cmd_.store(false);
+ reactivate_gripper_async_response_.store(true);
+ }
- // Write the latest command to the gripper.
- int scaled_position = write_command_.load();
- int scaled_speed = write_speed_.load();
- int scaled_force = write_force_.load();
+ // Write the latest command to the gripper.
+ int scaled_position = write_command_.load();
+ int scaled_speed = write_speed_.load();
+ int scaled_force = write_force_.load();
- if (!socket_adapter_->move(scaled_position, scaled_speed, scaled_force)) {
- throw std::runtime_error("Failed to send move command to Robotiq gripper.");
- }
+ auto result = socket_adapter_->move(scaled_position, scaled_speed, scaled_force);
+ if (!std::get<0>(result)) {
+ throw std::runtime_error("Failed to send move command to Robotiq gripper.");
+ }
- // Read the state of the gripper.
- int raw_position = socket_adapter_->position();
- gripper_current_state_.store(convertRawToPosition(raw_position));
+ // Read the state of the gripper.
+ int raw_position = socket_adapter_->position();
+ gripper_current_state_.store(convertRawToPosition(raw_position));
+ }
+ catch (const std::exception& e)
+ {
+ RCLCPP_ERROR(logger_, "Error in background task: %s", e.what());
+ }
+
+ std::this_thread::sleep_for(gripperCommsLoopPeriod); // Sleep for a predefined period
}
- catch (const std::exception& e)
- {
- RCLCPP_ERROR(logger_, "Error in background task: %s", e.what());
- }
-
- std::this_thread::sleep_for(gripperCommsLoopPeriod); // Sleep for a predefined period
- }
}
@@ -364,7 +357,7 @@ double RobotiqGripperHardwareInterface::convertRawToPosition(int raw_position) {
return std::max(min_position_, std::min(scaled_position, max_position_));
}
-uint8_t RobotiqGripperHardwareInterface::convertPositionToRaw(double position) {
+int RobotiqGripperHardwareInterface::convertPositionToRaw(double position) {
if (std::isnan(min_position_) || std::isnan(max_position_) || min_position_ >= max_position_) {
throw std::runtime_error("Invalid gripper position limits: min_position_ or max_position_ are not configured correctly.");
}
@@ -372,7 +365,7 @@ uint8_t RobotiqGripperHardwareInterface::convertPositionToRaw(double position) {
return static_cast(std::clamp(scaled, 0.0, 255.0));
}
-uint8_t RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) {
+int RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) {
if (std::isnan(min_speed_) || std::isnan(max_speed_) || min_speed_ >= max_speed_) {
throw std::runtime_error("Invalid gripper speed limits: min_speed_ or max_speed_ are not configured correctly.");
}
@@ -380,7 +373,7 @@ uint8_t RobotiqGripperHardwareInterface::convertSpeedToRaw(double speed) {
return static_cast(std::clamp(scaled, 0.0, 255.0));
}
-uint8_t RobotiqGripperHardwareInterface::convertForceToRaw(double force) {
+int RobotiqGripperHardwareInterface::convertForceToRaw(double force) {
if (std::isnan(min_force_) || std::isnan(max_force_) || min_force_ >= max_force_) {
throw std::runtime_error("Invalid gripper force limits: min_force_ or max_force_ are not configured correctly.");
}
@@ -392,4 +385,4 @@ uint8_t RobotiqGripperHardwareInterface::convertForceToRaw(double force) {
#include "pluginlib/class_list_macros.hpp"
-PLUGINLIB_EXPORT_CLASS(robotiq_2f_interface::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface)
+PLUGINLIB_EXPORT_CLASS(robotiq::RobotiqGripperHardwareInterface, hardware_interface::SystemInterface)