added python interface
This commit is contained in:
qlin1806
56
scripts/example_gripper_control.py
Normal file
56
scripts/example_gripper_control.py
Normal file
@@ -0,0 +1,56 @@
|
||||
import rospy
|
||||
|
||||
from sas_robot_driver_franka import FrankaGripperInterface
|
||||
|
||||
|
||||
def main_loop():
|
||||
rate = rospy.Rate(10) # 10 Hz
|
||||
iteration_ = 0
|
||||
|
||||
hand1 = FrankaGripperInterface("/franka_hand_1")
|
||||
|
||||
while not hand1.is_enabled():
|
||||
rospy.loginfo("Waiting for gripper to be enabled...")
|
||||
rate.sleep()
|
||||
rospy.loginfo("Gripper enabled!")
|
||||
|
||||
rate = rospy.Rate(2) # 0.5 Hz
|
||||
|
||||
while not rospy.is_shutdown():
|
||||
rospy.loginfo("Main loop running...")
|
||||
|
||||
# Get the temperature of the gripper
|
||||
temperature = hand1.get_temperature()
|
||||
rospy.loginfo(f"Temperature: {temperature}")
|
||||
max_width = hand1.get_max_width()
|
||||
rospy.loginfo(f"Max width: {max_width}")
|
||||
width = hand1.get_width()
|
||||
rospy.loginfo(f"Width: {width}")
|
||||
is_grasped = hand1.is_grasped()
|
||||
rospy.loginfo(f"Is grasped: {is_grasped}")
|
||||
is_moving = hand1.is_moving()
|
||||
rospy.loginfo(f"Is moving: {is_moving}")
|
||||
is_grasping = hand1.is_grasping()
|
||||
rospy.loginfo(f"Is grasping: {is_grasping}")
|
||||
rospy.logwarn("calling move(0.01)")
|
||||
if not hand1.is_busy():
|
||||
hand1.grasp(0.01)
|
||||
else:
|
||||
rospy.logwarn("Gripper is busy. Waiting...")
|
||||
result_ready = hand1.is_done()
|
||||
if not result_ready:
|
||||
rospy.loginfo("Waiting for gripper to finish moving...")
|
||||
else:
|
||||
result = hand1.get_result()
|
||||
rospy.loginfo(f"Result: {result}")
|
||||
|
||||
|
||||
# Check if there is a response in the queue
|
||||
|
||||
iteration_ += 1
|
||||
rate.sleep()
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
rospy.init_node("example_gripper_control_node")
|
||||
main_loop()
|
||||
@@ -32,21 +32,21 @@
|
||||
#include <franka/exception.h>
|
||||
|
||||
|
||||
namespace qros {
|
||||
|
||||
namespace qros
|
||||
{
|
||||
//const static int SLAVE_MODE_JOINT_CONTROL;
|
||||
//const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
|
||||
|
||||
EffectorDriverFrankaHand::~EffectorDriverFrankaHand()
|
||||
{
|
||||
if(_is_connected())
|
||||
if (_is_connected())
|
||||
{
|
||||
disconnect();
|
||||
}
|
||||
}
|
||||
|
||||
EffectorDriverFrankaHand::EffectorDriverFrankaHand(
|
||||
const std::string &driver_node_prefix,
|
||||
const std::string& driver_node_prefix,
|
||||
const EffectorDriverFrankaHandConfiguration& configuration,
|
||||
ros::NodeHandle& node_handel,
|
||||
std::atomic_bool* break_loops):
|
||||
@@ -56,9 +56,12 @@ namespace qros {
|
||||
break_loops_(break_loops)
|
||||
{
|
||||
gripper_sptr_ = nullptr;
|
||||
grasp_server_ = node_handel_.advertiseService(driver_node_prefix_+"/grasp", &EffectorDriverFrankaHand::_grasp_srv_callback, this);
|
||||
move_server_ = node_handel_.advertiseService(driver_node_prefix_+"/move", &EffectorDriverFrankaHand::_move_srv_callback, this);
|
||||
gripper_status_publisher_ = node_handel_.advertise<sas_robot_driver_franka::GripperState>(driver_node_prefix_+"/gripper_status", 1);
|
||||
grasp_server_ = node_handel_.advertiseService(driver_node_prefix_ + "/grasp",
|
||||
&EffectorDriverFrankaHand::_grasp_srv_callback, this);
|
||||
move_server_ = node_handel_.advertiseService(driver_node_prefix_ + "/move",
|
||||
&EffectorDriverFrankaHand::_move_srv_callback, this);
|
||||
gripper_status_publisher_ = node_handel_.advertise<sas_robot_driver_franka::GripperState>(
|
||||
driver_node_prefix_ + "/gripper_status", 1);
|
||||
}
|
||||
|
||||
bool EffectorDriverFrankaHand::_is_connected() const
|
||||
@@ -66,25 +69,29 @@ namespace qros {
|
||||
#ifdef IN_TESTING
|
||||
return true;
|
||||
#endif
|
||||
if(gripper_sptr_ == nullptr) return false;
|
||||
if(!gripper_sptr_) return false;
|
||||
if (gripper_sptr_ == nullptr) return false;
|
||||
if (!gripper_sptr_) return false;
|
||||
else return true;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void EffectorDriverFrankaHand::start_control_loop()
|
||||
{
|
||||
|
||||
sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_ns);
|
||||
clock.init();
|
||||
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
|
||||
while(!(*break_loops_))
|
||||
ROS_INFO_STREAM(
|
||||
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
|
||||
while (!(*break_loops_))
|
||||
{
|
||||
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected.");
|
||||
if (!_is_connected()) throw std::runtime_error(
|
||||
"[" + ros::this_node::getName() +
|
||||
"]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected.");
|
||||
|
||||
if(!status_loop_running_){
|
||||
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_is_connected::Status loop is not running.");
|
||||
if (!status_loop_running_)
|
||||
{
|
||||
ROS_WARN_STREAM(
|
||||
"["+ ros::this_node::getName()+
|
||||
"]::[EffectorDriverFrankaHand]::_is_connected::Status loop is not running.");
|
||||
break_loops_->store(true);
|
||||
break;
|
||||
}
|
||||
@@ -92,25 +99,29 @@ namespace qros {
|
||||
clock.update_and_sleep();
|
||||
ros::spinOnce();
|
||||
}
|
||||
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Exiting control loop.");
|
||||
|
||||
ROS_INFO_STREAM(
|
||||
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Exiting control loop.");
|
||||
}
|
||||
|
||||
|
||||
void EffectorDriverFrankaHand::connect()
|
||||
{
|
||||
#ifdef IN_TESTING
|
||||
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::connect::In testing mode. to DUMMY");
|
||||
ROS_WARN_STREAM(
|
||||
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::connect::In testing mode. to DUMMY");
|
||||
return;
|
||||
#endif
|
||||
gripper_sptr_ = std::make_shared<franka::Gripper>(configuration_.robot_ip);
|
||||
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::connect::Could not connect to the robot.");
|
||||
|
||||
if (!_is_connected()) throw std::runtime_error(
|
||||
"[" + ros::this_node::getName() +
|
||||
"]::[EffectorDriverFrankaHand]::connect::Could not connect to the robot.");
|
||||
}
|
||||
|
||||
void EffectorDriverFrankaHand::disconnect() noexcept
|
||||
{
|
||||
#ifdef IN_TESTING
|
||||
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnect::In testing mode. from DUMMY");
|
||||
ROS_WARN_STREAM(
|
||||
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnect::In testing mode. from DUMMY");
|
||||
return;
|
||||
#endif
|
||||
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnecting...");
|
||||
@@ -124,25 +135,29 @@ namespace qros {
|
||||
void EffectorDriverFrankaHand::gripper_homing()
|
||||
{
|
||||
#ifdef IN_TESTING
|
||||
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::In testing mode.");
|
||||
ROS_WARN_STREAM(
|
||||
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::In testing mode.");
|
||||
return;
|
||||
#endif
|
||||
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::Robot is not connected.");
|
||||
if (!_is_connected()) throw std::runtime_error(
|
||||
"[" + ros::this_node::getName() + "]::[EffectorDriverFrankaHand]::gripper_homing::Robot is not connected.");
|
||||
auto ret = gripper_sptr_->homing();
|
||||
if(!ret)
|
||||
if (!ret)
|
||||
{
|
||||
throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
|
||||
throw std::runtime_error(
|
||||
"[" + ros::this_node::getName() +
|
||||
"]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
|
||||
}
|
||||
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
|
||||
}
|
||||
|
||||
void EffectorDriverFrankaHand::initialize()
|
||||
{
|
||||
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::initialize::Robot is not connected.");
|
||||
if (!_is_connected()) throw std::runtime_error(
|
||||
"[" + ros::this_node::getName() + "]::[EffectorDriverFrankaHand]::initialize::Robot is not connected.");
|
||||
gripper_homing();
|
||||
// start gripper status loop
|
||||
status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this);
|
||||
|
||||
}
|
||||
|
||||
void EffectorDriverFrankaHand::deinitialize()
|
||||
@@ -151,10 +166,10 @@ namespace qros {
|
||||
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::deinitialize::In testing mode.");
|
||||
return;
|
||||
#endif
|
||||
if(_is_connected())
|
||||
if (_is_connected())
|
||||
{
|
||||
franka::GripperState gripper_state = gripper_sptr_->readOnce();
|
||||
if(gripper_state.is_grasped)
|
||||
if (gripper_state.is_grasped)
|
||||
{
|
||||
gripper_sptr_->stop();
|
||||
}
|
||||
@@ -162,24 +177,31 @@ namespace qros {
|
||||
}
|
||||
|
||||
|
||||
bool EffectorDriverFrankaHand::_grasp_srv_callback(sas_robot_driver_franka::Grasp::Request& req, sas_robot_driver_franka::Grasp::Response& res)
|
||||
bool EffectorDriverFrankaHand::_grasp_srv_callback(sas_robot_driver_franka::Grasp::Request& req,
|
||||
sas_robot_driver_franka::Grasp::Response& res)
|
||||
{
|
||||
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping...");
|
||||
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std::to_string(req.width));
|
||||
ROS_INFO_STREAM(
|
||||
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping...");
|
||||
ROS_INFO_STREAM(
|
||||
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std::
|
||||
to_string(req.width));
|
||||
auto force = req.force;
|
||||
auto speed = req.speed;
|
||||
auto epsilon_inner = req.epsilon_inner;
|
||||
auto epsilon_outer = req.epsilon_outer;
|
||||
if(force <= 0.0) force = configuration_.default_force;
|
||||
if(speed<= 0.0) speed = configuration_.default_speed;
|
||||
if(epsilon_inner <= 0.0) epsilon_inner = configuration_.default_epsilon_inner;
|
||||
if(epsilon_outer <= 0.0) epsilon_outer = configuration_.default_epsilon_outer;
|
||||
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std::to_string(force) + " speed: " + std::to_string(speed));
|
||||
if (force <= 0.0) force = configuration_.default_force;
|
||||
if (speed <= 0.0) speed = configuration_.default_speed;
|
||||
if (epsilon_inner <= 0.0) epsilon_inner = configuration_.default_epsilon_inner;
|
||||
if (epsilon_outer <= 0.0) epsilon_outer = configuration_.default_epsilon_outer;
|
||||
ROS_INFO_STREAM(
|
||||
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std::
|
||||
to_string(force) + " speed: " + std::to_string(speed));
|
||||
bool ret = false;
|
||||
bool function_ret = true;
|
||||
gripper_in_use_.lock();
|
||||
#ifdef IN_TESTING
|
||||
ret = true;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
|
||||
#else
|
||||
try
|
||||
{
|
||||
@@ -200,18 +222,21 @@ namespace qros {
|
||||
}
|
||||
|
||||
|
||||
|
||||
bool EffectorDriverFrankaHand::_move_srv_callback(sas_robot_driver_franka::Move::Request& req, sas_robot_driver_franka::Move::Response& res)
|
||||
bool EffectorDriverFrankaHand::_move_srv_callback(sas_robot_driver_franka::Move::Request& req,
|
||||
sas_robot_driver_franka::Move::Response& res)
|
||||
{
|
||||
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Moving...");
|
||||
auto speed = req.speed;
|
||||
if (speed <= 0.0) speed = configuration_.default_speed;
|
||||
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string(speed) + " Width: " + std::to_string(req.width));
|
||||
ROS_INFO_STREAM(
|
||||
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string
|
||||
(speed) + " Width: " + std::to_string(req.width));
|
||||
bool ret = false;
|
||||
bool function_ret = true;
|
||||
gripper_in_use_.lock();
|
||||
#ifdef IN_TESTING
|
||||
ret = true;
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
|
||||
#else
|
||||
try
|
||||
{
|
||||
@@ -236,24 +261,36 @@ namespace qros {
|
||||
{
|
||||
status_loop_running_ = true;
|
||||
sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_ns);
|
||||
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.");
|
||||
ROS_INFO_STREAM(
|
||||
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.")
|
||||
;
|
||||
clock.init();
|
||||
try
|
||||
{
|
||||
while (status_loop_running_)
|
||||
{
|
||||
bool should_unlock = false;
|
||||
if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Robot is not connected.");
|
||||
if (!_is_connected()) throw std::runtime_error(
|
||||
"[" + ros::this_node::getName() +
|
||||
"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Robot is not connected.");
|
||||
try
|
||||
{
|
||||
#ifdef IN_TESTING
|
||||
ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
|
||||
throw std::runtime_error("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
|
||||
ROS_WARN_STREAM(
|
||||
"["+ ros::this_node::getName()+
|
||||
"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
|
||||
throw std::runtime_error(
|
||||
"[" + ros::this_node::getName() +
|
||||
"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
|
||||
#endif
|
||||
#ifdef BLOCK_READ_IN_USED
|
||||
gripper_in_use_.lock();
|
||||
should_unlock = true;
|
||||
#endif
|
||||
franka::GripperState gripper_state = gripper_sptr_->readOnce();
|
||||
#ifdef BLOCK_READ_IN_USED
|
||||
gripper_in_use_.unlock();
|
||||
#endif
|
||||
sas_robot_driver_franka::GripperState msg;
|
||||
msg.width = static_cast<float>(gripper_state.width);
|
||||
msg.max_width = static_cast<float>(gripper_state.max_width);
|
||||
@@ -261,10 +298,16 @@ namespace qros {
|
||||
msg.temperature = gripper_state.temperature;
|
||||
msg.duration_ms = gripper_state.time.toMSec();
|
||||
gripper_status_publisher_.publish(msg);
|
||||
}catch(...)
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
#ifdef BLOCK_READ_IN_USED
|
||||
if (should_unlock) gripper_in_use_.unlock();
|
||||
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Could not read gripper state. Unavailable.");
|
||||
#endif
|
||||
ROS_INFO_STREAM(
|
||||
"["+ ros::this_node::getName()+
|
||||
"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Could not read gripper state. Unavailable.")
|
||||
;
|
||||
sas_robot_driver_franka::GripperState msg;
|
||||
msg.width = 0.0;
|
||||
gripper_status_publisher_.publish(msg);
|
||||
@@ -272,21 +315,23 @@ namespace qros {
|
||||
|
||||
clock.update_and_sleep();
|
||||
}
|
||||
status_loop_running_=false;
|
||||
}catch (std::exception& e)
|
||||
status_loop_running_ = false;
|
||||
}
|
||||
catch (std::exception& e)
|
||||
{
|
||||
ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + e.what());
|
||||
status_loop_running_=false;
|
||||
ROS_ERROR_STREAM(
|
||||
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + e.
|
||||
what());
|
||||
status_loop_running_ = false;
|
||||
}
|
||||
catch (...)
|
||||
{
|
||||
ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::Unknown exception.");
|
||||
status_loop_running_=false;
|
||||
ROS_ERROR_STREAM(
|
||||
"["+ ros::this_node::getName()+
|
||||
"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::Unknown exception.");
|
||||
status_loop_running_ = false;
|
||||
}
|
||||
ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exiting status loop.");
|
||||
ROS_INFO_STREAM(
|
||||
"["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exiting status loop.");
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
} // qros
|
||||
@@ -38,6 +38,7 @@
|
||||
#include <thread>
|
||||
#include <mutex>
|
||||
|
||||
// #define BLOCK_READ_IN_USED
|
||||
// #define IN_TESTING
|
||||
|
||||
// #include <sas_robot_driver/sas_robot_driver.h>
|
||||
|
||||
@@ -1,3 +1,4 @@
|
||||
"""
|
||||
"""
|
||||
from _qros_robot_dynamic import RobotDynamicsInterface, RobotDynamicsProvider
|
||||
from .franka_gripper_interface import FrankaGripperInterface
|
||||
182
src/sas_robot_driver_franka/franka_gripper_interface.py
Normal file
182
src/sas_robot_driver_franka/franka_gripper_interface.py
Normal file
@@ -0,0 +1,182 @@
|
||||
import rospy
|
||||
from sas_robot_driver_franka.srv import Move, MoveRequest, MoveResponse, Grasp, GraspRequest, GraspResponse
|
||||
from sas_robot_driver_franka.msg import GripperState
|
||||
import os
|
||||
import threading
|
||||
from queue import Queue
|
||||
import struct
|
||||
|
||||
MOVE_TOPIC_SUFFIX = "move"
|
||||
GRASP_TOPIC_SUFFIX = "grasp"
|
||||
STATUS_TOPIC_SUFFIX = "gripper_status"
|
||||
|
||||
|
||||
class FrankaGripperInterface:
|
||||
"""
|
||||
Non blocking interface to control the Franka gripper
|
||||
"""
|
||||
|
||||
def __init__(self, robot_prefix):
|
||||
self.robot_prefix = robot_prefix
|
||||
self.move_service = rospy.ServiceProxy(os.path.join(robot_prefix, MOVE_TOPIC_SUFFIX), Move)
|
||||
self._moving = False
|
||||
self.grasp_service = rospy.ServiceProxy(os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX), Grasp)
|
||||
self._grasping = False
|
||||
self.status_subscriber = rospy.Subscriber(os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX), GripperState,
|
||||
self._status_callback)
|
||||
|
||||
self.result_queue = Queue()
|
||||
|
||||
# gripper state
|
||||
self.state_width = None
|
||||
self.state_max_width = None
|
||||
self.state_temperature = None
|
||||
self.state_is_grasped = None
|
||||
|
||||
def _is_service_ready(self, service):
|
||||
try:
|
||||
rospy.wait_for_service(service, timeout=0.1)
|
||||
return True
|
||||
except rospy.ROSException:
|
||||
return False
|
||||
|
||||
def is_enabled(self):
|
||||
if self.state_width is None:
|
||||
return False
|
||||
if not self._is_service_ready(self.move_service.resolved_name):
|
||||
return False
|
||||
if not self._is_service_ready(self.grasp_service.resolved_name):
|
||||
return False
|
||||
return True
|
||||
|
||||
def _status_callback(self, msg: GripperState):
|
||||
self.state_width = msg.width
|
||||
self.state_max_width = msg.max_width
|
||||
self.state_temperature = msg.temperature
|
||||
self.state_is_grasped = msg.is_grasped
|
||||
|
||||
def move(self, width, speed=0):
|
||||
"""
|
||||
Move the gripper to a specific width
|
||||
:param width: width in meters
|
||||
:param speed: speed in meters per second
|
||||
:return: None
|
||||
"""
|
||||
if self.is_busy():
|
||||
raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
|
||||
threading.Thread(target=self._move, args=(width, speed)).start()
|
||||
|
||||
def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0):
|
||||
"""
|
||||
Grasp an object with the gripper
|
||||
:param width:
|
||||
:param force:
|
||||
:param speed:
|
||||
:param epsilon_inner:
|
||||
:param epsilon_outer:
|
||||
:return:
|
||||
"""
|
||||
if self.is_busy():
|
||||
raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
|
||||
threading.Thread(target=self._grasp, args=(width, force, speed, epsilon_inner, epsilon_outer)).start()
|
||||
|
||||
def get_max_width(self):
|
||||
""" Get the maximum width of the gripper """
|
||||
if not self.is_enabled():
|
||||
raise Exception("Gripper is not enabled")
|
||||
return self.state_max_width
|
||||
|
||||
def get_width(self):
|
||||
""" Get the current width of the gripper """
|
||||
if not self.is_enabled():
|
||||
raise Exception("Gripper is not enabled")
|
||||
return self.state_width
|
||||
|
||||
def get_temperature(self):
|
||||
""" Get the temperature of the gripper """
|
||||
if not self.is_enabled():
|
||||
raise Exception("Gripper is not enabled")
|
||||
return self.state_temperature
|
||||
|
||||
def is_grasped(self):
|
||||
""" Check if an object is grasped """
|
||||
if not self.is_enabled():
|
||||
raise Exception("Gripper is not enabled")
|
||||
return self.state_is_grasped
|
||||
|
||||
def is_moving(self):
|
||||
""" Check if the gripper is currently moving """
|
||||
return self._moving
|
||||
|
||||
def is_grasping(self):
|
||||
""" Check if the gripper is currently grasping """
|
||||
return self._grasping
|
||||
|
||||
def is_busy(self):
|
||||
""" Check if the gripper is currently moving or grasping """
|
||||
return self._moving or self._grasping
|
||||
|
||||
def is_done(self):
|
||||
""" Check if the gripper is done moving or grasping """
|
||||
if not self.is_busy():
|
||||
rospy.logwarn("Gripper is not moving or grasping")
|
||||
return False
|
||||
else:
|
||||
if self.result_queue.empty():
|
||||
return False
|
||||
else:
|
||||
return True
|
||||
|
||||
def get_result(self):
|
||||
"""
|
||||
Get the result of the last action
|
||||
:return:
|
||||
"""
|
||||
if not self.result_queue.empty():
|
||||
response = self.result_queue.get()
|
||||
self._moving = False
|
||||
self._grasping = False
|
||||
return response.success
|
||||
else:
|
||||
raise Exception("No result available")
|
||||
|
||||
def wait_until_done(self):
|
||||
"""
|
||||
Wait until the gripper is done moving or grasping
|
||||
:return: success
|
||||
"""
|
||||
if not self.is_enabled():
|
||||
raise Exception("Gripper is not enabled")
|
||||
if not self._moving and not self._grasping:
|
||||
raise Exception("Gripper is not moving or grasping")
|
||||
while self._moving or self._grasping:
|
||||
rospy.sleep(0.1)
|
||||
if not self.result_queue.empty():
|
||||
response = self.result_queue.get()
|
||||
if isinstance(response, MoveResponse):
|
||||
self._moving = False
|
||||
elif isinstance(response, GraspResponse):
|
||||
self._grasping = False
|
||||
else:
|
||||
raise Exception("Invalid response type")
|
||||
break
|
||||
return response.success
|
||||
|
||||
def _move(self, width, speed):
|
||||
self._moving = True
|
||||
request = MoveRequest()
|
||||
request.width = width
|
||||
request.speed = speed
|
||||
response = self.move_service(request)
|
||||
self.result_queue.put(response)
|
||||
|
||||
def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer):
|
||||
self._grasping = True
|
||||
request = GraspRequest()
|
||||
request.width = width
|
||||
request.force = force
|
||||
request.speed = speed
|
||||
request.epsilon_inner = epsilon_inner
|
||||
request.epsilon_outer = epsilon_outer
|
||||
response = self.grasp_service(request)
|
||||
self.result_queue.put(response)
|
||||
Reference in New Issue
Block a user