build complete
now launch debugging
This commit is contained in:
parent
1be83c8f93
commit
a59a248969
@ -5,7 +5,5 @@
|
||||
<mapping directory="$PROJECT_DIR$/src/Universal_Robots_ROS2_Description" vcs="Git" />
|
||||
<mapping directory="$PROJECT_DIR$/src/Universal_Robots_ROS2_Driver" vcs="Git" />
|
||||
<mapping directory="$PROJECT_DIR$/src/cartesian_controllers" vcs="Git" />
|
||||
<mapping directory="$PROJECT_DIR$/src/ros2_robotiq_gripper" vcs="Git" />
|
||||
<mapping directory="$PROJECT_DIR$/src/serial" vcs="Git" />
|
||||
</component>
|
||||
</project>
|
||||
@ -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(<dependency> 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()
|
||||
|
||||
@ -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
|
||||
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
|
||||
@ -9,6 +9,20 @@
|
||||
|
||||
<buildtool_depend>ament_cmake</buildtool_depend>
|
||||
|
||||
<exec_depend>joint_state_publisher_gui</exec_depend>
|
||||
<exec_depend>launch</exec_depend>
|
||||
<exec_depend>launch_ros</exec_depend>
|
||||
<exec_depend>robot_state_publisher</exec_depend>
|
||||
<exec_depend>rviz2</exec_depend>
|
||||
<exec_depend>urdf</exec_depend>
|
||||
<exec_depend>xacro</exec_depend>
|
||||
|
||||
<exec_depend>ros2_control</exec_depend>
|
||||
<exec_depend>ros2_controllers</exec_depend>
|
||||
|
||||
<exec_depend>gripper_controllers</exec_depend>
|
||||
<exec_depend>joint_state_broadcaster</exec_depend>
|
||||
|
||||
<test_depend>ament_lint_auto</test_depend>
|
||||
<test_depend>ament_lint_common</test_depend>
|
||||
|
||||
|
||||
@ -13,7 +13,7 @@
|
||||
<!-- Plugins -->
|
||||
<hardware>
|
||||
<xacro:if value="${use_fake_hardware}">
|
||||
<plugin>mock_components/GenericSystem</plugin>
|
||||
<plugin>robotiq_2f_interface/RobotiqGripperHardwareInterface</plugin>
|
||||
<param name="fake_sensor_commands">${fake_sensor_commands}</param>
|
||||
<param name="state_following_offset">0.0</param>
|
||||
</xacro:if>
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
|
||||
<xacro:include filename="$(find robotiq_2f_description)/config/robotiq_2f_140_config.yaml" />
|
||||
<xacro:macro name="robotiq_gripper" params="
|
||||
name
|
||||
prefix
|
||||
@ -20,7 +21,13 @@
|
||||
use_fake_hardware="${use_fake_hardware}"
|
||||
fake_sensor_commands="${fake_sensor_commands}"
|
||||
robot_ip="${robot_ip}"
|
||||
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}"/>
|
||||
</xacro:if>
|
||||
|
||||
<!-- this is a temporary link to rotate the 2f-140 gripper to match the 2f-85 -->
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
<?xml version="1.0"?>
|
||||
<robot xmlns:xacro="http://wiki.ros.org/xacro" name="robotiq_gripper">
|
||||
<xacro:include filename="$(find robotiq_2f_description)/config/robotiq_2f_85_config.yaml" />
|
||||
<xacro:macro name="robotiq_gripper" params="
|
||||
name
|
||||
prefix
|
||||
@ -20,7 +21,13 @@
|
||||
use_fake_hardware="${use_fake_hardware}"
|
||||
fake_sensor_commands="${fake_sensor_commands}"
|
||||
robot_ip="${robot_ip}"/>
|
||||
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}"/>
|
||||
</xacro:if>
|
||||
|
||||
<link name="${prefix}robotiq_85_base_link">
|
||||
|
||||
@ -1,6 +1,6 @@
|
||||
<library path="robotiq_driver_plugin">
|
||||
<class name="robotiq_2f_interface/RobotiqGripperHardwareInterface"
|
||||
type="robtiq_2f_interface::RobotiqGripperHardwareInterface"
|
||||
type="robotiq::RobotiqGripperHardwareInterface"
|
||||
base_class_type="hardware_interface::SystemInterface">
|
||||
<description>
|
||||
ROS2 controller for the Robotiq gripper.
|
||||
|
||||
@ -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();
|
||||
}
|
||||
|
||||
@ -237,7 +237,7 @@ private:
|
||||
|
||||
// Smart pointer to manage the socket descriptor with the custom deleter
|
||||
std::unique_ptr<int, SocketDeleter> 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
|
||||
|
||||
|
||||
@ -10,13 +10,17 @@
|
||||
#include <vector>
|
||||
#include <atomic>
|
||||
|
||||
#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 <hardware_interface/actuator_interface.hpp>
|
||||
#include <hardware_interface/handle.hpp>
|
||||
#include <hardware_interface/hardware_info.hpp>
|
||||
#include <hardware_interface/system_interface.hpp>
|
||||
#include <hardware_interface/types/hardware_interface_type_values.hpp>
|
||||
|
||||
#include <hardware_interface/types/hardware_interface_return_values.hpp>
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
|
||||
#include <stdexcept>
|
||||
@ -48,11 +52,12 @@ protected: // Likely changes the access to protected from private
|
||||
// Members for driver interaction
|
||||
bool use_mock_hardware_;
|
||||
std::unique_ptr<SocketAdapterBase> socket_adapter_;
|
||||
void RobotiqGripperHardwareInterface::configureAdapter(bool useMock);
|
||||
void configureAdapter(bool useMock);
|
||||
|
||||
// Background communication thread
|
||||
std::thread communication_thread_;
|
||||
std::atomic<bool> communication_thread_is_running_;
|
||||
std::mutex resource_mutex_;
|
||||
void background_task();
|
||||
|
||||
static constexpr double NO_NEW_CMD_ = std::numeric_limits<double>::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
|
||||
|
||||
|
||||
@ -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_
|
||||
@ -12,7 +12,7 @@ Robotiq2fSocketAdapter::~Robotiq2fSocketAdapter() {
|
||||
|
||||
// Connection and disconnection
|
||||
bool Robotiq2fSocketAdapter::connect(const std::string& hostname, int port) {
|
||||
std::lock_guard<std::mutex> lock(socketMutex_);
|
||||
std::lock_guard<std::mutex> 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<std::mutex> lock(socketMutex_);
|
||||
std::lock_guard<std::mutex> 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<std::mutex> lock(socketMutex_);
|
||||
std::lock_guard<std::mutex> 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<std::mutex> lock(socketMutex_);
|
||||
std::lock_guard<std::mutex> 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<std::string, dou
|
||||
for (const auto& [variable, value] : variableMap) {
|
||||
std::stringstream ss;
|
||||
ss << value;
|
||||
cmd += " " + commandVariables.at(variable) + " " + ss.str();
|
||||
cmd += " " + variable + " " + ss.str();
|
||||
}
|
||||
cmd += "\n";
|
||||
|
||||
// Send the command and receive the response
|
||||
std::lock_guard<std::mutex> lock(socketMutex_);
|
||||
std::lock_guard<std::mutex> 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<std::mutex> lock(socketMutex_);
|
||||
std::lock_guard<std::mutex> 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<std::mutex> lock(socketMutex_);
|
||||
std::lock_guard<std::mutex> lock(socket_mutex_);
|
||||
if (*sockfd_ < 0) {
|
||||
throw std::runtime_error("Cannot get variables on a disconnected socket.");
|
||||
return -1;
|
||||
|
||||
@ -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<std::mutex> 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<std::mutex> 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<uint8_t>(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<uint8_t>(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)
|
||||
|
||||
Loading…
Reference in New Issue
Block a user