diff --git a/CMakeLists.txt b/CMakeLists.txt index 1381a35..1d9b0c8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,7 +18,7 @@ find_package(catkin REQUIRED COMPONENTS ## Uncomment this if the package has a setup.py. This macro ensures ## modules and global scripts declared therein get installed ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() +catkin_python_setup() ################################################ ## Declare ROS messages, services and actions ## @@ -153,33 +153,11 @@ include_directories( # all install targets should use catkin DESTINATION variables # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html -## Mark executable scripts (Python etc.) for installation -## in contrast to setup.py, you can choose the destination -# install(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables and/or libraries for installation -# install(TARGETS weiss_gripper_ieg76_030 weiss_gripper_ieg76_030_node -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) +catkin_install_python(PROGRAMS src/weiss_gripper_ieg76/driver.py + src/weiss_gripper_ieg76/test_client.py + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) -## Mark cpp header files for installation -# install(DIRECTORY include/${PROJECT_NAME}/ -# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -# FILES_MATCHING PATTERN "*.h" -# PATTERN ".svn" EXCLUDE -# ) - -## Mark other files for installation (e.g. launch and bag files, etc.) -# install(FILES -# # myfile1 -# # myfile2 -# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -# ) +install(DIRECTORY launch urdf DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) ############# ## Testing ## diff --git a/README.md b/README.md index bb71045..efcbf6b 100644 --- a/README.md +++ b/README.md @@ -1,5 +1,10 @@ # weiss_gripper_ieg76 -This is the ROS package of the driver for the [Weiss Robotics gripper, model IEG 76-030](https://www.weiss-robotics.com/en/produkte/gripping-systems/integration-line-en/ieg-en/). +This is the ROS package of the driver for several Weiss Robotics grippers: + +* [IEG 76-030](https://weiss-robotics.com/servo-electric/ieg-series/) +* [IEG 55-020](https://weiss-robotics.com/servo-electric/ieg-series/) +* [CRG 200-085](https://weiss-robotics.com/servo-electric/crg-series/) +* [CRG 30-050](https://weiss-robotics.com/servo-electric/crg-series/) 1. [Device Configurator Windows](#device-configurator-windows) 1. [Installation](#installation) diff --git a/package.xml b/package.xml index c19b7f6..2cd5593 100644 --- a/package.xml +++ b/package.xml @@ -1,13 +1,11 @@ - + weiss_gripper_ieg76 0.0.0 - This is the ros package of the driver for the Weiss Robotics gripper, model IEG 76-030. + This is the ros package of the driver for the Weiss Robotics gripper, model IEG + 55-020/76-030 as well as CRG 30-050/200-085. - - - - lth-vd + ipa-lth @@ -19,33 +17,32 @@ - + http://www.ipa.fraunhofer.de - - - - + lth-vd + ipa-pgt + + catkin + - + message_generation - - + - message_runtime + message_runtime + - catkin - roscpp - rospy - std_msgs - roscpp - rospy - std_msgs + + roscpp + rospy + std_msgs + python-transitions diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..5ed219f --- /dev/null +++ b/setup.py @@ -0,0 +1,13 @@ +## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD + +from distutils.core import setup +from catkin_pkg.python_setup import generate_distutils_setup + +# fetch values from package.xml +setup_args = generate_distutils_setup( + packages=['weiss_gripper_ieg76'], + package_dir={'': 'src'}, + requires=['rospy'] +) + +setup(**setup_args) diff --git a/src/driver_logic.py b/src/driver_logic.py deleted file mode 100644 index 83a0b89..0000000 --- a/src/driver_logic.py +++ /dev/null @@ -1,314 +0,0 @@ -#!/usr/bin/env python -# import roslib -# import struct -# import time -# import serial -import rospy -import numpy as np -import threading -# import diagnostic_updater -# from serial import SerialException -# from std_srvs.srv import Trigger, TriggerResponse -# from weiss_gripper_ieg76.srv import * -# from sensor_msgs.msg import JointState -# from diagnostic_msgs.msg import DiagnosticStatus -import transitions -from transitions.extensions import LockedHierarchicalMachine as Machine - - -class DriverLogic(object): - states = [ - 'not_initialized', 'fault', 'other_fault', - - {'name': 'st', 'children':[ - 'inactive', 'open', 'closed', 'holding'] - }, - - {'name': 'op', 'on_exit': 'operation_finished', 'children':[ - {'name': 'referencing', 'on_enter': 'exec_referencing'}, - {'name': 'closing_before_opening', 'on_enter': 'exec_closing_before_opening'}, - {'name': 'opening', 'on_enter': 'exec_opening'}, - {'name': 'opening_before_close', 'on_enter': 'exec_opening_before_closing'}, - {'name': 'closing', 'on_enter': 'exec_closing'}, - {'name': 'opening_before_grasp', 'on_enter': 'exec_opening_before_closing'}, - {'name': 'grasping', 'on_enter': 'exec_grasping'} ] - } - ] - - transitions = [ - # uninitialized, fault and other_fault state - ['on_inactive', ['not_initialized', 'fault', 'other_fault'], 'st_inactive'], - ['on_open', ['not_initialized', 'fault', 'other_fault'], 'st_open'], - ['on_closed', ['not_initialized', 'fault', 'other_fault'], 'st_closed'], - ['on_holding', ['not_initialized', 'fault', 'other_fault'], 'st_holding'], - ['on_fault', ['not_initialized', 'other_fault'], 'fault'], - ['on_other_fault', ['not_initialized', 'fault'], 'other_fault'], - ['on_not_initialized', ['fault', 'other_fault'], 'not_initialized'], - ['do_reference', 'not_initialized', 'op_referencing'], - - # inactive state - ['do_open', 'st_inactive', 'op_opening'], - ['do_close', 'st_inactive', 'op_closing'], - { 'trigger': 'do_grasp', 'source': 'st_inactive', 'dest': 'op_grasping', 'conditions': 'can_grasp'}, - - # open state - ['do_reference', 'st_open', 'op_referencing'], - ['do_open', 'st_open', 'op_closing_before_opening'], - ['do_close', 'st_open', 'op_closing'], - { 'trigger': 'do_grasp', 'source': 'st_open', 'dest': 'op_grasping', 'conditions': 'can_grasp'}, - - # closed state - ['do_reference', 'st_closed', 'op_referencing'], - ['do_open', 'st_closed', 'op_opening'], - ['do_close', 'st_closed', 'op_opening_before_close'], - { 'trigger': 'do_grasp', 'source': 'st_closed', 'dest': 'op_opening_before_grasp', 'conditions': 'can_grasp'}, - - # holding state - ['do_reference', 'st_holding', 'op_referencing'], - { 'trigger': 'do_open', 'source': 'st_holding', 'dest': 'op_opening', 'conditions': 'can_move_while_holding'}, - { 'trigger': 'do_close', 'source': 'st_holding', 'dest': 'op_opening_before_close', 'conditions': 'can_move_while_holding'}, - - # referencing state - { 'trigger': 'on_inactive', 'source': 'op_referencing', 'dest': 'st_inactive', 'before': 'operation_successful'}, - - # opening state - { 'trigger': 'on_open', 'source': 'op_opening', 'dest': 'st_open', 'before': 'operation_successful'}, - { 'trigger': 'on_holding', 'source': 'op_opening', 'dest': 'st_holding', 'before': 'claws_blocked'}, # opening failed because blocked by object - - # closing state - { 'trigger': 'on_closed', 'source': 'op_closing', 'dest': 'st_closed', 'before': 'operation_successful'}, - { 'trigger': 'on_holding', 'source': 'op_closing', 'dest': 'st_holding', 'before': 'claws_blocked'}, # failed because blocked by object - - # grasping state - { 'trigger': 'on_holding', 'source': 'op_grasping', 'dest': 'st_holding', 'before': 'operation_successful'}, - { 'trigger': 'on_closed', 'source': 'op_grasping', 'dest': 'st_closed', 'before': 'no_object'}, # failed because no object to grasp - - # closing_before_opening state - ['on_closed', 'op_closing_before_opening', 'op_opening'], - { 'trigger': 'on_holding', 'source': 'op_closing_before_opening', 'dest': 'st_holding', 'before': 'claws_blocked'}, # failed because blocked by object - - # opening_before_closing state - ['on_open', 'op_opening_before_close', 'op_closing'], - { 'trigger': 'on_holding', 'source': 'op_opening_before_close', 'dest': 'st_holding', 'before': 'claws_blocked'}, # failed because blocked by object - - ['on_open', 'op_opening_before_grasp', 'op_grasping'], - - # default transitions for unexpected state changes - { 'trigger': 'on_inactive', 'source': '*', 'dest': 'st_inactive', 'before': 'unexpected_state_change'}, - { 'trigger': 'on_open', 'source': '*', 'dest': 'st_open', 'before': 'unexpected_state_change'}, - { 'trigger': 'on_closed', 'source': '*', 'dest': 'st_closed', 'before': 'unexpected_state_change'}, - { 'trigger': 'on_holding', 'source': '*', 'dest': 'st_holding', 'before': 'unexpected_state_change'}, - { 'trigger': 'on_fault', 'source': '*', 'dest': 'fault', 'before': 'unexpected_state_change'}, - { 'trigger': 'on_other_fault', 'source': '*', 'dest': 'other_fault', 'before': 'unexpected_state_change'}, - { 'trigger': 'on_not_initialized', 'source': '*', 'dest': 'not_initialized', 'before': 'unexpected_state_change'} - ] - - def __init__(self, serial_port_comm): - self.state_machine_context = threading.RLock() - - self.machine = Machine(model=self, states=DriverLogic.states, transitions=DriverLogic.transitions, - initial='not_initialized', machine_context=[self.state_machine_context]) - self.serial_port_comm = serial_port_comm - serial_port_comm.add_flags_observer(self) - self.gripper_pos = None - - self.opening_pos = 29.5 # default gripper values - self.closing_pos = 0.5 - - self.service_sync = threading.Lock() - self.operation_params = None - self.operation_response = None - self.async_operation_finished = threading.Condition() - self.no_spurious_wakeup = False - - self.old_flag_signaled = {"OPEN_SIGNALED":False, "CLOSED_SIGNALED":False, "HOLDING_SIGNALED":False, - "FAULT_SIGNALED":False, "IDLE_SIGNALED":False, "OTHER_FAULT_SIGNALED":False, - "NOT_INITIALIZED_SIGNALED":True} - - def service_called(self, transition, params, trigger_response): - with self.service_sync: # only one service call allowed at a time - self.no_spurious_wakeup = False - self.operation_params = params - self.operation_response = trigger_response - with self.async_operation_finished: # needed if result of service only known at later stage - try: - trigger_response.success = False # only successful if explicitly set - if self.trigger(transition): - while not self.no_spurious_wakeup: # make sure, the wait finishes correctly - self.async_operation_finished.wait() # needed if result of service known at later stage - except transitions.core.MachineError as err: # not a valid action in the current state - trigger_response.success = False - trigger_response.message = self.get_err_msg(self.state) + trigger_response.message - finally: - if trigger_response.success: - trigger_response.message = "succeeded. " + trigger_response.message - else: - trigger_response.message = "failed. " + trigger_response.message - self.operation_params = None - self.operation_response = None - - def operation_finished(self): - with self.async_operation_finished: - self.no_spurious_wakeup = True - self.async_operation_finished.notify() - - def operation_successful(self): - self.operation_response.success = True - - def claws_blocked(self): - self.operation_response.success = False - self.operation_response.message += 'Claws are blocked.' - - def no_object(self): - self.operation_response.success = False - self.operation_response.message += 'No object to grasp.' - - def unexpected_state_change(self): - if self.operation_response is not None: - self.operation_response.success = False - self.operation_response.message += 'Unexpected state change.' - rospy.logerr('Unexpected state change.') - - def get_err_msg(self, state): - operation_err_msg_from_state = {'not_initialized': 'Reference the gripper before usage. ', - 'fault': 'Fault state. Ack the error before proceeding. ', - 'other_fault': 'Temperature error or maintenance required. No operation possible. '} - - if state in operation_err_msg_from_state: - return operation_err_msg_from_state[state] - else: - return "Operation not allowed from current state ({}). ".format(state) - - def update_flags(self, new_flags_dict): - thr = threading.Thread(target=self.update_flags_thread, args=(new_flags_dict,)) - thr.start() - - def update_flags_thread(self, new_flags_dict): - self.gripper_pos = new_flags_dict["POS"] - - # the observer function should be non-blocking - # -> make sure the state machine is not blocked by a service call - if not self.state_machine_context.acquire(blocking=False): - return - - if new_flags_dict["TEMPFAULT_FLAG"] == 1 or new_flags_dict["MAINT_FLAG"]: #or new_flags_dict["TEMPWARN_FLAG"]: - if not self.old_flag_signaled["OTHER_FAULT_SIGNALED"]: - self.set_signaled_flag("OTHER_FAULT_SIGNALED") - self.to_other_fault() - elif new_flags_dict["FAULT_FLAG"] == 1: - if not self.old_flag_signaled["FAULT_SIGNALED"]: - self.set_signaled_flag("FAULT_SIGNALED") - self.to_fault() - elif new_flags_dict["IDLE_FLAG"] == 1: - if not self.old_flag_signaled["IDLE_SIGNALED"]: - self.set_signaled_flag("IDLE_SIGNALED") - self.on_inactive() - elif new_flags_dict["OPEN_FLAG"] == 1: - if not self.old_flag_signaled["OPEN_SIGNALED"]: - self.set_signaled_flag("OPEN_SIGNALED") - self.on_open() - elif new_flags_dict["CLOSED_FLAG"] == 1: - if not self.old_flag_signaled["CLOSED_SIGNALED"]: - self.set_signaled_flag("CLOSED_SIGNALED") - self.on_closed() - elif new_flags_dict["HOLDING_FLAG"] == 1: - if not self.old_flag_signaled["HOLDING_SIGNALED"]: - self.set_signaled_flag("HOLDING_SIGNALED") - self.on_holding() - elif not self.check_if_referenced(new_flags_dict): - if not self.old_flag_signaled["NOT_INITIALIZED_SIGNALED"]: - self.set_signaled_flag("NOT_INITIALIZED_SIGNALED") - self.on_not_initialized() - - self.state_machine_context.release() - - def set_signaled_flag(self, flag): - rospy.loginfo("signaled flag: {}".format(flag)) - self.old_flag_signaled["OTHER_FAULT_SIGNALED"] = False - self.old_flag_signaled["FAULT_SIGNALED"] = False - self.old_flag_signaled["IDLE_SIGNALED"] = False - self.old_flag_signaled["OPEN_SIGNALED"] = False - self.old_flag_signaled["CLOSED_SIGNALED"] = False - self.old_flag_signaled["HOLDING_SIGNALED"] = False - self.old_flag_signaled["NOT_INITIALIZED_SIGNALED"] = False - self.old_flag_signaled[flag] = True - - def check_if_referenced(self, flags_dict): - #when the gripper is not referenced all flags are 0 - gripper_referenced = False - - if (flags_dict["IDLE_FLAG"] == 1 or flags_dict["OPEN_FLAG"] == 1 or - flags_dict["CLOSED_FLAG"] == 1 or flags_dict["HOLDING_FLAG"] == 1 or - flags_dict["FAULT_FLAG"] == 1 or flags_dict["TEMPFAULT_FLAG"] == 1 or - flags_dict["TEMPWARN_FLAG"] == 1 or flags_dict["MAINT_FLAG"] == 1): - gripper_referenced = True - - return gripper_referenced - - def can_move_while_holding(self): - outer_grip = self.opening_pos > self.closing_pos - if outer_grip and self.operation_params.position > self.gripper_pos: - return True # if currently outer grip -> only allow movement to outer side - elif not outer_grip and self.operation_params.position < self.gripper_pos: - return True # if currently inner grip -> only allow movement to inner side - else: - self.operation_response.success = False - self.operation_response.message += 'Moving into the grasped object is not possible. Move in the other direction. Current pos: {}, commanded pos: {}'.format(self.gripper_pos, self.operation_params.position) - return False - - def can_grasp(self): - least_movement = 0.5 # margin found by trial-and-error - if np.abs(self.operation_params.position - self.gripper_pos) >= least_movement: - return True - else: - self.operation_response.success = False - self.operation_response.message += 'Grasping range too little. Move by at least {}mm.. Current pos: {}, commanded pos: {}'.format(least_movement, self.gripper_pos, self.operation_params.position) - return False - - def exec_referencing(self): - rospy.loginfo('State: {}'.format(self.state)) - self.serial_port_comm.send_command_synced("reference") - - def set_positions(self, opening_pos, closing_pos): - pos_set = False - while not pos_set: - pos_set = self.serial_port_comm.set_opening_pos(opening_pos) - pos_set = pos_set and self.serial_port_comm.set_closing_pos(closing_pos) - self.opening_pos = opening_pos - self.closing_pos = closing_pos - rospy.loginfo("Opening pos: {}, closing pos: {}".format(opening_pos, closing_pos)) - - def exec_opening(self): - rospy.loginfo('Trying to set opening positions...') - rospy.loginfo('State: {}'.format(self.state)) - self.set_positions(opening_pos=self.operation_params.position, closing_pos=self.gripper_pos) - rospy.loginfo('Opening positions set.') - self.serial_port_comm.send_command_synced("open") - - def exec_closing(self): - rospy.loginfo('Trying to set closing positions...') - rospy.loginfo('State: {}'.format(self.state)) - self.set_positions(opening_pos=self.gripper_pos, closing_pos=self.operation_params.position) - rospy.loginfo('Closing positions set.') - self.serial_port_comm.send_command_synced("close") - - def exec_grasping(self): - rospy.loginfo('Trying to set grasping positions...') - rospy.loginfo('State: {}'.format(self.state)) - self.set_positions(opening_pos=self.gripper_pos, closing_pos=self.operation_params.position) - rospy.loginfo('Grasping positions set.') - self.serial_port_comm.send_command_synced("close") - - def exec_opening_before_closing(self): - rospy.loginfo('Trying to set opening_before_closing positions...') - rospy.loginfo('State: {}'.format(self.state)) - self.set_positions(opening_pos=self.gripper_pos, closing_pos=self.gripper_pos) - rospy.loginfo('Prepening positions set.') - self.serial_port_comm.send_command_synced("open") - - def exec_closing_before_opening(self): - rospy.loginfo('Trying to set closing_before_opening positions...') - rospy.loginfo('State: {}'.format(self.state)) - self.set_positions(opening_pos=self.gripper_pos, closing_pos=self.gripper_pos) - rospy.loginfo('Closing_before_opening positions set.') - self.serial_port_comm.send_command_synced("close") diff --git a/src/state_publisher.py b/src/state_publisher.py deleted file mode 100644 index 982d84e..0000000 --- a/src/state_publisher.py +++ /dev/null @@ -1,87 +0,0 @@ -#!/usr/bin/env python -# import roslib -# import struct -# import time -# import serial -import rospy -import threading -import diagnostic_updater -# from serial import SerialException -# from std_srvs.srv import Trigger, TriggerResponse -# from weiss_gripper_ieg76.srv import * -from sensor_msgs.msg import JointState -from diagnostic_msgs.msg import DiagnosticStatus - - -class StatesPublisher(threading.Thread): - def __init__(self, loop_time, serial_port_comm): - threading.Thread.__init__(self) - self.loop_time = loop_time - self.driver_shutdown = False - - self.current_flags_sync = threading.Lock() - self.current_flags_dict = None - self.current_flags_updated = False - serial_port_comm.add_flags_observer(self) - - self.joint_state_msg = JointState() - self.joint_state_msg.name = [] - self.joint_state_msg.name.append("gripper_claws") - self.joint_states_publisher = rospy.Publisher('joint_states', JointState, queue_size=10) - - self.updater = diagnostic_updater.Updater() - self.updater.setHardwareID("Weiss Robotics Gripper IEG 76-030 V1.02 SN 000106") - self.updater.add("Position and flags updater", self.produce_diagnostics) - freq_bounds = {'min':0.5, 'max':2} - # It publishes the messages and simultaneously makes diagnostics for the topic "joint_states" using a FrequencyStatus and TimeStampStatus - self.pub_freq_time_diag = diagnostic_updater.DiagnosedPublisher(self.joint_states_publisher, - self.updater, - diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.1, 10), - diagnostic_updater.TimeStampStatusParam()) - - def shutdown(self): - self.driver_shutdown = True - - def update_flags(self, new_flags_dict): - with self.current_flags_sync: - self.current_flags_dict = new_flags_dict - self.current_flags_updated = True - - def run(self): - rospy.logdebug("StatesPublisher.run()") - while not self.driver_shutdown: - with self.current_flags_sync: - if self.current_flags_updated: - self.updater.update() - rospy.logdebug("StatesPublisher publishing states...") - self.publish_states() - rospy.logdebug("StatesPublisher states published.") - #self.updater.force_update() - self.current_flags_updated = False - rospy.sleep(self.loop_time) - rospy.logdebug("states_publisher_thread done.") - - def produce_diagnostics(self, stat): - if self.current_flags_dict["FAULT_FLAG"] == True: - stat.summary(DiagnosticStatus.ERROR, "The fault bit of the gripper is 1.") - else: - stat.summary(DiagnosticStatus.OK, "The fault bit of the gripper is 0.") - stat.add("Position", self.current_flags_dict["POS"]) - stat.add("Idle Flag", self.current_flags_dict["IDLE_FLAG"]) - stat.add("Open Flag", self.current_flags_dict["OPEN_FLAG"]) - stat.add("Closed Flag", self.current_flags_dict["CLOSED_FLAG"]) - stat.add("Holding Flag", self.current_flags_dict["HOLDING_FLAG"]) - stat.add("Error Flag", self.current_flags_dict["FAULT_FLAG"]) - stat.add("Temperature Error Flag", self.current_flags_dict["TEMPFAULT_FLAG"]) - stat.add("Temperature Warning Flag", self.current_flags_dict["TEMPWARN_FLAG"]) - stat.add("Maintenance Flag", self.current_flags_dict["MAINT_FLAG"]) - return stat - - def publish_states(self): - self.joint_state_msg.header.stamp = rospy.Time.now() - self.joint_state_msg.position = [] - self.joint_state_msg.position.append(self.current_flags_dict["POS"]) - try: - self.pub_freq_time_diag.publish(self.joint_state_msg) - except: - rospy.logerr("\nClosed topics.") diff --git a/src/test_client.py b/src/test_client.py deleted file mode 100755 index 598569a..0000000 --- a/src/test_client.py +++ /dev/null @@ -1,232 +0,0 @@ -#!/usr/bin/env python -import roslib; -import sys -import rospy -import math -from std_srvs.srv import Trigger -from weiss_gripper_ieg76.srv import * - -def send_reference_request(): - rospy.wait_for_service('reference') - try: - reference = rospy.ServiceProxy('reference', Trigger) - resp = reference() - if resp.success == True: - print "Success: " + resp.message - else: - print "Failure: " + resp.message - except rospy.ServiceException, e: - print "Service call failed: %s"%e - -def send_close_request(grasp_config_no): - rospy.wait_for_service('close_jaws') - try: - close_jaws = rospy.ServiceProxy('close_jaws', ConfigTrigger) - resp = close_jaws(grasp_config_no) - if resp.success == True: - print "Success: " + resp.message - else: - print "Failure: " + resp.message - except rospy.ServiceException, e: - print "Service call failed: %s"%e - -def send_grasp_object_request(grasp_config_no): - rospy.wait_for_service('grasp_object') - try: - grasp_object = rospy.ServiceProxy('grasp_object', ConfigTrigger) - resp = grasp_object(grasp_config_no) - if resp.success == True: - print "Success: " + resp.message - else: - print "Failure: " + resp.message - except rospy.ServiceException, e: - print "Service call failed: %s"%e - -def send_open_request(grasp_config_no): - rospy.wait_for_service('open_jaws') - try: - open_jaws = rospy.ServiceProxy('open_jaws', ConfigTrigger) - resp = open_jaws(grasp_config_no) - if resp.success == True: - print "Success: " + resp.message - else: - print "Failure: " + resp.message - except rospy.ServiceException, e: - print "Service call failed: %s"%e - -def send_ack_error_request(): - rospy.wait_for_service('ack_error') - try: - ack_error = rospy.ServiceProxy('ack_error', Trigger) - resp = ack_error() - if resp.success == True: - print "Success: " + resp.message - else: - print "Failure: " + resp.message - except rospy.ServiceException, e: - print "Service call failed: %s"%e - -def send_ack_ref_error_request(): - rospy.wait_for_service('ack_ref_error') - try: - ack_ref_error = rospy.ServiceProxy('ack_ref_error', Trigger) - resp = ack_ref_error() - if resp.success == True: - print "Success: " + resp.message - else: - print "Failure: " + resp.message - except rospy.ServiceException, e: - print "Service call failed: %s"%e - -def get_all_param_request(grasp_config_no): - rospy.wait_for_service('get_all_param') - try: - get_all_param = rospy.ServiceProxy('get_all_param', GetAllParam) - resp = get_all_param(grasp_config_no) - if resp.success == True: - print "Success: " + resp.message - print "grasping_force: " + str(resp.grasping_force) + " [%]" - print "opening_position: " + str(round(resp.opening_position, 2)) + " [mm]" - print "closing_position: " + str(round(resp.closing_position, 2)) + " [mm]" - else: - print "Failure: " + resp.message - except rospy.ServiceException, e: - print "Service call failed: %s"%e - -def set_all_param_request(grasp_config_no, grasping_force, opening_position, closing_position): - rospy.wait_for_service('set_all_param') - try: - set_all_param = rospy.ServiceProxy('set_all_param', SetAllParam) - resp = set_all_param(grasp_config_no, grasping_force, opening_position, closing_position) - if resp.success == True: - print "Success: " + resp.message - else: - print "Failure: " + resp.message - except rospy.ServiceException, e: - print "Service call failed: %s"%e - -def set_grasping_force_request(grasp_config_no, grasping_force): - rospy.wait_for_service('set_grasping_force') - try: - set_grasping_force = rospy.ServiceProxy('set_grasping_force', SetGraspingForce) - resp = set_grasping_force(grasp_config_no, grasping_force) - if resp.success == True: - print "Success: " + resp.message - else: - print "Failure: " + resp.message - except rospy.ServiceException, e: - print "Service call failed: %s"%e - -def set_opening_pos_request(grasp_config_no, opening_position): - rospy.wait_for_service('set_opening_pos') - try: - set_opening_pos = rospy.ServiceProxy('set_opening_pos', SetOpeningPos) - resp = set_opening_pos(grasp_config_no, opening_position) - if resp.success == True: - print "Success: " + resp.message - else: - print "Failure: " + resp.message - except rospy.ServiceException, e: - print "Service call failed: %s"%e - -def set_closing_pos_request(grasp_config_no, closing_position): - rospy.wait_for_service('set_closing_pos') - try: - set_closing_pos = rospy.ServiceProxy('set_closing_pos', SetClosingPos) - resp = set_closing_pos(grasp_config_no, closing_position) - if resp.success == True: - print "Success: " + resp.message - else: - print "Failure: " + resp.message - except rospy.ServiceException, e: - print "Service call failed: %s"%e - -def usage(): - print "-------- Commands Weiss Robotics Gripper ieg76 --------" - print "1. Reference" - print "2. Open jaws" - print "3. Close jaws" - print "4. Grasp object" - print "5. Ack error" - print "6. Ack reference error" - print "7. Select grasp configuration" - print "8. Get all the grasp configuration's param" - print "9. Set all the grasp configuration's param" - print "10. Set the grasping force" - print "11. Set the opening position" - print "12. Set the closing position" - print "13. Exit" - -if __name__ == "__main__": - selected_cmd = usage() - grasp_config_no = 0 - while True: - selected_cmd = input("Select a command to send: ") - if selected_cmd == 1: - print "Sending reference request..." - send_reference_request() - elif selected_cmd == 2: - print "Sending open jaws request..." - send_open_request(grasp_config_no) - elif selected_cmd == 3: - print "Sending close jaws request..." - send_close_request(grasp_config_no) - elif selected_cmd == 4: - print "Sending grasp object request..." - send_grasp_object_request(grasp_config_no) - elif selected_cmd == 5: - print "Sending acknowledge error request..." - send_ack_error_request() - elif selected_cmd == 6: - print "Sending acknowledge error request..." - send_ack_ref_error_request() - elif selected_cmd == 7: - print "Current grasp configuration number: " + str(grasp_config_no) - input_data = input("Enter new grasp configuration no = ") - if input_data >=0 and input_data <=3: - grasp_config_no = input_data - else: - print "Unknown grasp config no " + str(input_data) + " given. Valid grasp configuration numbers are 0, 1, 2, 3." - elif selected_cmd == 8: - print "Getting all the parameters (grasping force, opening position, closing position) of grasp configuration number " + str(grasp_config_no) + "..." - get_all_param_request(grasp_config_no) - elif selected_cmd == 9: - print "Setting all the parameters (grasping force, opening position, closing position) of grasp configuration number " + str(grasp_config_no) + "..." - input_data_grasping_force = input("Enter the grasping force (0..100%) of grasp config no " + str(grasp_config_no) + " = ") - input_data_opening_position = input("Enter the opening position of grasp config no " + str(grasp_config_no) + " = ") - input_data_closing_position = input("Enter the closing position of grasp config no " + str(grasp_config_no) + " = ") - if input_data_grasping_force <0 or input_data_grasping_force >100: - print "Value error " + str(input_data_grasping_force) + ". Valid grasping force values are 0..100%." - elif input_data_opening_position <0.0 and input_data_opening_position >30.0: - print "Value error " + str(input_data_opening_position) + ". Valid opening position values are 0.0..30.0 mm." - elif input_data_closing_position <0.0 and input_data_closing_position >30.0: - print "Value error " + str(input_data_closing_position) + ". Valid closing position values are 0.0..30.0 mm." - else: - print "Setting all the param (grasping force " + str(input_data_grasping_force) + " [%], opening position " + str(input_data_opening_position) + " [mm], closing position " + str(input_data_closing_position) + " [mm]) of grasp config " + str(grasp_config_no) + "." - set_all_param_request(grasp_config_no, input_data_grasping_force, input_data_opening_position, input_data_closing_position) - elif selected_cmd == 10: - input_data = input("Enter the grasping force (0..100%) of grasp config no " + str(grasp_config_no) + " = ") - if input_data >=0 and input_data <=100: - print "Setting the grasping force of grasp configuration number " + str(grasp_config_no) + "..." - set_grasping_force_request(grasp_config_no, input_data) - else: - print "Value error " + str(input_data) + ". Valid grasping force values are 0..100%." - elif selected_cmd == 11: - input_data = input("Enter the opening position of grasp config no " + str(grasp_config_no) + " = ") - if input_data >=0.0 and input_data <=30.0: - print "Setting the opening position of grasp configuration number " + str(grasp_config_no) + "..." - set_opening_pos_request(grasp_config_no, input_data) - else: - print "Value error " + str(input_data) + ". Valid opening position values are 0.0..30.0 mm." - elif selected_cmd == 12: - input_data = input("Enter the closing position of grasp config no " + str(grasp_config_no) + " = ") - if input_data >=0.0 and input_data <=30.0: - print "Setting the closing position of grasp configuration number " + str(grasp_config_no) + "..." - set_closing_pos_request(grasp_config_no, input_data) - else: - print "Value error " + str(input_data) + ". Valid closing position values are 0.0..30.0 mm." - elif selected_cmd == 13: - print "Exiting the test_client..." - sys.exit(0) - else: - print "Unknown option entered." \ No newline at end of file diff --git a/src/weiss_gripper_ieg76/__init__.py b/src/weiss_gripper_ieg76/__init__.py new file mode 100644 index 0000000..e69de29 diff --git a/src/driver.py b/src/weiss_gripper_ieg76/driver.py similarity index 61% rename from src/driver.py rename to src/weiss_gripper_ieg76/driver.py index 656a12b..f82e3e2 100755 --- a/src/driver.py +++ b/src/weiss_gripper_ieg76/driver.py @@ -2,9 +2,9 @@ import rospy from std_srvs.srv import Trigger, TriggerResponse from weiss_gripper_ieg76.srv import Move, MoveResponse, SetForce, SetForceResponse -from serial_comm import SerialPortComm -from driver_logic import DriverLogic -from state_publisher import StatesPublisher +from weiss_gripper_ieg76.serial_comm import SerialPortComm +from weiss_gripper_ieg76.driver_logic import DriverLogic +from weiss_gripper_ieg76.state_publisher import StatesPublisher class Driver(object): @@ -17,6 +17,19 @@ def __init__(self): self.states_publisher_thread = StatesPublisher(0.8, self.serial_port_comm) rospy.on_shutdown(self.shutdown_handler) + def set_max_pos(self, gripper_info): + product_id = gripper_info["product_id"] + if product_id == "CRG 200-085": + self._max_pos = 85 + elif product_id == "CRG 30-050": + self._max_pos = 50 + elif product_id == "IEG 76-030": + self._max_pos = 30 + elif product_id == "IEG 55-020": + self._max_pos = 20 + else: + raise RuntimeError("Gripper type '{}' not yet supported!".format(product_id)) + def log_reply(self, reply): if reply.success: rospy.loginfo(reply.message) @@ -24,7 +37,7 @@ def log_reply(self, reply): rospy.logerr(reply.message) def check_position(self, pos): - return 0 <= pos <= 30 + return 0 <= pos <= self._max_pos def check_force(self, force): return 0 <= force <= 100 @@ -37,12 +50,19 @@ def handle_reference(self, req): self.log_reply(reply) return reply + def relative_to_absolute(self, req): + if req.relative: + if self.driver_logic.gripper_pos is None: + raise ValueError("Relative position demanded, but gripper position was not received yet.") + req.position = self.driver_logic.gripper_pos + req.position + def handle_open(self, req): rospy.loginfo("Opening") reply = MoveResponse() + self.relative_to_absolute(req) if not self.check_position(req.position): reply.success = False - reply.message = 'Opening failed. Position must be 0.0(mm) <= position <= 30.0(mm).' + reply.message = 'Opening failed. Position must be 0.0(mm) <= position <= {}.0(mm).'.format(self._max_pos) else: self.driver_logic.service_called(transition="do_open", params=req, trigger_response=reply) reply.message = 'Opening ' + reply.message @@ -52,9 +72,10 @@ def handle_open(self, req): def handle_close(self, req): rospy.loginfo("Closing") reply = MoveResponse() + self.relative_to_absolute(req) if not self.check_position(req.position): reply.success = False - reply.message = 'Closing failed. Position must be 0.0(mm) <= position <= 30.0(mm).' + reply.message = 'Closing failed. Position must be 0.0(mm) <= position <= {}.0(mm).'.format(self._max_pos) else: self.driver_logic.service_called(transition="do_close", params=req, trigger_response=reply) reply.message = 'Closing ' + reply.message @@ -64,9 +85,10 @@ def handle_close(self, req): def handle_grasp(self, req): rospy.loginfo("Grasping") reply = MoveResponse() + self.relative_to_absolute(req) if not self.check_position(req.position): reply.success = False - reply.message = 'Grasping failed. Position must be 0.0(mm) <= position <= 30.0(mm).' + reply.message = 'Grasping failed. Position must be 0.0(mm) <= position <= {}.0(mm).'.format(self._max_pos) else: self.driver_logic.service_called(transition="do_grasp", params=req, trigger_response=reply) reply.message = 'Grasping ' + reply.message @@ -88,6 +110,22 @@ def handle_set_force(self, req): self.log_reply(reply) return reply + def handle_ack(self, req): + rospy.loginfo("Ack error") + reply = TriggerResponse() + self.driver_logic.service_called(transition="do_ack", params=req, trigger_response=reply) + reply.message = 'Ack error ' + reply.message + self.log_reply(reply) + return reply + + def handle_deactivate(self, req): + rospy.loginfo("Deactivate gripper") + reply = TriggerResponse() + self.driver_logic.service_called(transition="do_deactivate", params=req, trigger_response=reply) + reply.message = 'Deactivating ' + reply.message + self.log_reply(reply) + return reply + def shutdown_handler(self): self.states_publisher_thread.shutdown() self.serial_port_comm.shutdown() @@ -102,6 +140,18 @@ def run(self): self.states_publisher_thread.start() rospy.logdebug("Threads started.") + rospy.loginfo('Receiving info about gripper') + self.serial_port_comm.get_gripper_info() + print(self.serial_port_comm.gripper_info) + serial_no = rospy.get_param("~serial_no", None) + if not serial_no is None and serial_no != int(self.serial_port_comm.gripper_info['serial_no']): + rospy.logerr('Serial no is {}, but requested is {}'.format(self.serial_port_comm.gripper_info['serial_no'], serial_no)) + return + + # update parameters based on gripper_info + self.set_max_pos(self.serial_port_comm.gripper_info) + self.states_publisher_thread.set_gripper_info(self.serial_port_comm.gripper_info) + grasp_force = rospy.get_param("~grasping_force", 100) rospy.loginfo('Setting force to {}%...'.format(grasp_force)) while True: @@ -113,8 +163,9 @@ def run(self): serv_ref = rospy.Service('~open', Move, self.handle_open) serv_ref = rospy.Service('~close', Move, self.handle_close) serv_ref = rospy.Service('~grasp', Move, self.handle_grasp) - # serv_ref = rospy.Service('ack', Trigger, self.handle_ack) + serv_ref = rospy.Service('~ack', Trigger, self.handle_ack) serv_ref = rospy.Service('~set_force', SetForce, self.handle_set_force) + serv_ref = rospy.Service('~deactivate', Trigger, self.handle_deactivate) rospy.loginfo("Ready to receive requests.") diff --git a/src/weiss_gripper_ieg76/driver_logic.py b/src/weiss_gripper_ieg76/driver_logic.py new file mode 100644 index 0000000..7a9a073 --- /dev/null +++ b/src/weiss_gripper_ieg76/driver_logic.py @@ -0,0 +1,380 @@ +import rospy +import numpy as np +import threading +import transitions +from transitions.extensions import LockedHierarchicalMachine as Machine + + +class DriverLogic(object): + states = [ + 'not_initialized', 'fault', 'ref_fault', 'other_fault', + + {'name': 'st', 'children':[ + 'inactive', 'open', 'closed', 'holding'] + }, + + {'name': 'op', 'on_enter': 'operation_started', 'on_exit': 'operation_finished', 'children':[ + {'name': 'deactivating_before_activate', 'on_enter': 'exec_deactivating'}, + {'name': 'activating', 'on_enter': 'exec_activating'}, + {'name': 'deactivating', 'on_enter': 'exec_deactivating'}, + {'name': 'referencing', 'on_enter': 'exec_referencing'}, + {'name': 'opening_before_referencing', 'on_enter': 'exec_opening_before_referencing'}, + {'name': 'wait_for_referenced'}, + {'name': 'closing_before_opening', 'on_enter': 'exec_closing_before_opening'}, + {'name': 'opening', 'on_enter': 'exec_opening'}, + {'name': 'wait_for_opening'}, + {'name': 'opening_before_close', 'on_enter': 'exec_opening_before_closing'}, + {'name': 'closing', 'on_enter': 'exec_closing'}, + {'name': 'opening_before_grasp', 'on_enter': 'exec_opening_before_closing'}, + {'name': 'grasping', 'on_enter': 'exec_grasping'} ] + } + ] + + transitions = [ + # uninitialized, fault, ref_fault and other_fault state + ['on_inactive', ['not_initialized', 'fault', 'ref_fault', 'other_fault'], 'st_inactive'], + ['on_open', ['not_initialized', 'fault', 'ref_fault', 'other_fault'], 'st_open'], + ['on_closed', ['not_initialized', 'fault', 'ref_fault', 'other_fault'], 'st_closed'], + ['on_holding', ['not_initialized', 'fault', 'ref_fault', 'other_fault'], 'st_holding'], + ['on_fault', ['not_initialized', 'other_fault'], 'fault'], + ['on_other_fault', ['not_initialized', 'fault', 'ref_fault'], 'other_fault'], + ['on_not_initialized', ['fault', 'ref_fault', 'other_fault'], 'not_initialized'], + + ['do_reference', 'not_initialized', 'op_referencing'], + ['do_ack', 'fault', 'op_deactivating_before_activate'], + ['do_ack', 'ref_fault', 'op_deactivating'], + + # inactive state + ['do_reference', 'st_inactive', 'op_opening_before_referencing'], + { 'trigger': 'do_deactivate', 'source': 'st_inactive', 'dest': '=', 'before': 'operation_successful', 'after': 'operation_finished'}, + ['do_open', 'st_inactive', 'op_opening'], + ['do_close', 'st_inactive', 'op_closing'], + { 'trigger': 'do_grasp', 'source': 'st_inactive', 'dest': 'op_grasping', 'conditions': 'can_grasp'}, + + # open state + ['do_reference', 'st_open', 'op_referencing'], + ['do_deactivate', 'st_open', 'op_deactivating'], + ['do_open', 'st_open', 'op_closing_before_opening'], + ['do_close', 'st_open', 'op_closing'], + { 'trigger': 'do_grasp', 'source': 'st_open', 'dest': 'op_grasping', 'conditions': 'can_grasp'}, + + # closed state + ['do_reference', 'st_closed', 'op_referencing'], + ['do_deactivate', 'st_closed', 'op_deactivating'], + ['do_open', 'st_closed', 'op_opening'], + ['do_close', 'st_closed', 'op_opening_before_close'], + { 'trigger': 'do_grasp', 'source': 'st_closed', 'dest': 'op_opening_before_grasp', 'conditions': 'can_grasp'}, + + # holding state + ['do_reference', 'st_holding', 'op_referencing'], + ['do_deactivate', 'st_holding', 'op_deactivating'], + { 'trigger': 'do_open', 'source': 'st_holding', 'dest': 'op_opening', 'conditions': 'can_move_while_holding'}, + { 'trigger': 'do_close', 'source': 'st_holding', 'dest': 'op_opening_before_close', 'conditions': 'can_move_while_holding'}, + + # deactivating_before_activate state (part of do_ack) + ['on_inactive', 'op_deactivating_before_activate', 'op_activating'], + + # activating state (part of do_ack) + { 'trigger': 'on_open', 'source': 'op_activating', 'dest': 'st_open', 'before': 'operation_successful'}, + + # deactivating state (part of do_ack for referencing errors) + { 'trigger': 'on_inactive', 'source': 'op_deactivating', 'dest': 'st_inactive', 'before': 'operation_successful'}, + + # referencing state + { 'trigger': 'on_inactive', 'source': 'op_referencing', 'dest': 'st_inactive', 'before': 'operation_successful'}, + { 'trigger': 'on_not_initialized', 'source': 'op_referencing', 'dest': 'op_wait_for_referenced'}, + { 'trigger': 'on_fault', 'source': 'op_referencing', 'dest': 'ref_fault', 'before': 'unexpected_state_change'}, + + # wait_for_referenced state + { 'trigger': 'on_inactive', 'source': 'op_wait_for_referenced', 'dest': 'st_inactive', 'before': 'operation_successful'}, + { 'trigger': 'on_fault', 'source': 'op_wait_for_referenced', 'dest': 'ref_fault', 'before': 'unexpected_state_change'}, + + # opening state + { 'trigger': 'on_open', 'source': 'op_opening', 'dest': 'st_open', 'before': 'operation_successful'}, + { 'trigger': 'on_closed', 'source': 'op_opening', 'dest': 'op_wait_for_opening'}, + { 'trigger': 'on_holding', 'source': 'op_opening', 'dest': 'st_holding', 'before': 'claws_blocked'}, # opening failed because blocked by object + + # closing state + { 'trigger': 'on_closed', 'source': 'op_closing', 'dest': 'st_closed', 'before': 'operation_successful'}, + { 'trigger': 'on_holding', 'source': 'op_closing', 'dest': 'st_holding', 'before': 'claws_blocked'}, # failed because blocked by object + + # grasping state + { 'trigger': 'on_holding', 'source': 'op_grasping', 'dest': 'st_holding', 'before': 'operation_successful'}, + { 'trigger': 'on_closed', 'source': 'op_grasping', 'dest': 'st_closed', 'before': 'no_object'}, # failed because no object to grasp + + # wait_for_opening state + { 'trigger': 'on_open', 'source': 'op_wait_for_opening', 'dest': 'st_open', 'before': 'operation_successful'}, + + # closing_before_opening state + ['on_closed', 'op_closing_before_opening', 'op_opening'], + { 'trigger': 'on_holding', 'source': 'op_closing_before_opening', 'dest': 'st_holding', 'before': 'claws_blocked'}, # failed because blocked by object + + # opening_before_closing state + ['on_open', 'op_opening_before_close', 'op_closing'], + { 'trigger': 'on_holding', 'source': 'op_opening_before_close', 'dest': 'st_holding', 'before': 'claws_blocked'}, # failed because blocked by object + + ['on_open', 'op_opening_before_grasp', 'op_grasping'], + + # opening_before_reference state + ['on_open', 'op_opening_before_referencing', 'op_referencing'], + { 'trigger': 'on_holding', 'source': 'op_opening_before_referencing', 'dest': 'st_holding', 'before': 'claws_blocked'}, # failed because blocked by object + + # default transitions for unexpected state changes + { 'trigger': 'on_inactive', 'source': '*', 'dest': 'st_inactive', 'before': 'unexpected_state_change'}, + { 'trigger': 'on_open', 'source': '*', 'dest': 'st_open', 'before': 'unexpected_state_change'}, + { 'trigger': 'on_closed', 'source': '*', 'dest': 'st_closed', 'before': 'unexpected_state_change'}, + { 'trigger': 'on_holding', 'source': '*', 'dest': 'st_holding', 'before': 'unexpected_state_change'}, + { 'trigger': 'on_fault', 'source': '*', 'dest': 'fault', 'before': 'unexpected_state_change'}, + { 'trigger': 'on_other_fault', 'source': '*', 'dest': 'other_fault', 'before': 'unexpected_state_change'}, + { 'trigger': 'on_not_initialized', 'source': '*', 'dest': 'not_initialized', 'before': 'unexpected_state_change'} + ] + + def __init__(self, serial_port_comm): + self.state_machine_context = threading.RLock() + + self.machine = Machine(model=self, states=DriverLogic.states, transitions=DriverLogic.transitions, + initial='not_initialized', machine_context=[self.state_machine_context]) + self.serial_port_comm = serial_port_comm + serial_port_comm.add_flags_observer(self) + self.gripper_pos = None + + self.opening_pos = 29.5 # default gripper values + self.closing_pos = 0.5 + + self.service_sync = threading.Lock() + self.operation_params = None + self.operation_response = None + self.async_operation_finished = threading.Condition() + self.no_spurious_wakeup = False + + self.op_timeout_timer = None + + self.old_flag_signaled = {"OPEN_SIGNALED":False, "CLOSED_SIGNALED":False, "HOLDING_SIGNALED":False, + "FAULT_SIGNALED":False, "IDLE_SIGNALED":False, "OTHER_FAULT_SIGNALED":False, + "NOT_INITIALIZED_SIGNALED":True} + + def service_called(self, transition, params, trigger_response): + with self.service_sync: # only one service call allowed at a time + self.no_spurious_wakeup = False + self.operation_params = params + self.operation_response = trigger_response + with self.async_operation_finished: # needed if result of service only known at later stage + try: + trigger_response.success = False # only successful if explicitly set + if self.trigger(transition): + while not self.no_spurious_wakeup: # make sure, the wait finishes correctly + self.async_operation_finished.wait() # needed if result of service known at later stage + except transitions.core.MachineError as err: # not a valid action in the current state + trigger_response.success = False + trigger_response.message = self.get_err_msg(self.state) + trigger_response.message + finally: + if trigger_response.success: + trigger_response.message = "succeeded. " + trigger_response.message + else: + trigger_response.message = "failed. " + trigger_response.message + self.operation_params = None + self.operation_response = None + + def operation_started(self): + self.op_timeout_timer = rospy.Timer(rospy.Duration(3.0), self.op_timeout, oneshot=True) + + def operation_finished(self): + if self.op_timeout_timer: + self.op_timeout_timer.shutdown() + with self.async_operation_finished: + self.no_spurious_wakeup = True + self.async_operation_finished.notify() + + def operation_successful(self): + self.operation_response.success = True + + def claws_blocked(self): + self.operation_response.success = False + self.operation_response.message += 'Claws are blocked.' + + def no_object(self): + self.operation_response.success = False + self.operation_response.message += 'No object to grasp.' + + def unexpected_state_change(self): + if self.operation_response is not None: + self.operation_response.success = False + self.operation_response.message += 'Unexpected state change.' + rospy.logerr('Unexpected state change.') + + def op_timeout(self, *params): + # an operation timed out -> leave the operation state and go to the currently active gripper state + self.operation_response.success = False + self.operation_response.message += 'Operation timed out.' + rospy.logerr('Operation timed out.') + + self.state_machine_context.acquire(blocking=True) + + self.old_flag_signaled["OTHER_FAULT_SIGNALED"] = False + self.old_flag_signaled["FAULT_SIGNALED"] = False + self.old_flag_signaled["IDLE_SIGNALED"] = False + self.old_flag_signaled["OPEN_SIGNALED"] = False + self.old_flag_signaled["CLOSED_SIGNALED"] = False + self.old_flag_signaled["HOLDING_SIGNALED"] = False + self.old_flag_signaled["NOT_INITIALIZED_SIGNALED"] = False + + self.state_machine_context.release() + + def get_err_msg(self, state): + operation_err_msg_from_state = {'not_initialized': 'Reference the gripper before usage. ', + 'fault': 'Fault state. Ack the error before proceeding. ', + 'other_fault': 'Temperature error or maintenance required. No operation possible. '} + + if state in operation_err_msg_from_state: + return operation_err_msg_from_state[state] + else: + return "Operation not allowed from current state ({}). ".format(state) + + def update_flags(self, new_flags_dict): + thr = threading.Thread(target=self.update_flags_thread, args=(new_flags_dict,)) + thr.start() + + def update_flags_thread(self, new_flags_dict): + self.gripper_pos = new_flags_dict["POS"] + + # the observer function should be non-blocking + # -> make sure the state machine is not blocked by a service call + if not self.state_machine_context.acquire(blocking=False): + return + + if new_flags_dict["TEMPFAULT_FLAG"] == 1 or new_flags_dict["MAINT_FLAG"]: #or new_flags_dict["TEMPWARN_FLAG"]: + if not self.old_flag_signaled["OTHER_FAULT_SIGNALED"]: + self.set_signaled_flag("OTHER_FAULT_SIGNALED") + self.to_other_fault() + elif new_flags_dict["FAULT_FLAG"] == 1: + if not self.old_flag_signaled["FAULT_SIGNALED"]: + self.set_signaled_flag("FAULT_SIGNALED") + self.to_fault() + elif new_flags_dict["IDLE_FLAG"] == 1: + if not self.old_flag_signaled["IDLE_SIGNALED"]: + self.set_signaled_flag("IDLE_SIGNALED") + self.on_inactive() + elif new_flags_dict["OPEN_FLAG"] == 1: + if not self.old_flag_signaled["OPEN_SIGNALED"]: + self.set_signaled_flag("OPEN_SIGNALED") + self.on_open() + elif new_flags_dict["CLOSED_FLAG"] == 1: + if not self.old_flag_signaled["CLOSED_SIGNALED"]: + self.set_signaled_flag("CLOSED_SIGNALED") + self.on_closed() + elif new_flags_dict["HOLDING_FLAG"] == 1: + if not self.old_flag_signaled["HOLDING_SIGNALED"]: + self.set_signaled_flag("HOLDING_SIGNALED") + self.on_holding() + elif not self.check_if_referenced(new_flags_dict): + if not self.old_flag_signaled["NOT_INITIALIZED_SIGNALED"]: + self.set_signaled_flag("NOT_INITIALIZED_SIGNALED") + self.on_not_initialized() + + self.state_machine_context.release() + + def set_signaled_flag(self, flag): + rospy.loginfo("signaled flag: {}".format(flag)) + self.old_flag_signaled["OTHER_FAULT_SIGNALED"] = False + self.old_flag_signaled["FAULT_SIGNALED"] = False + self.old_flag_signaled["IDLE_SIGNALED"] = False + self.old_flag_signaled["OPEN_SIGNALED"] = False + self.old_flag_signaled["CLOSED_SIGNALED"] = False + self.old_flag_signaled["HOLDING_SIGNALED"] = False + self.old_flag_signaled["NOT_INITIALIZED_SIGNALED"] = False + self.old_flag_signaled[flag] = True + + def check_if_referenced(self, flags_dict): + #when the gripper is not referenced all flags are 0 + gripper_referenced = False + + if (flags_dict["IDLE_FLAG"] == 1 or flags_dict["OPEN_FLAG"] == 1 or + flags_dict["CLOSED_FLAG"] == 1 or flags_dict["HOLDING_FLAG"] == 1 or + flags_dict["FAULT_FLAG"] == 1 or flags_dict["TEMPFAULT_FLAG"] == 1 or + flags_dict["TEMPWARN_FLAG"] == 1 or flags_dict["MAINT_FLAG"] == 1): + gripper_referenced = True + + return gripper_referenced + + def can_move_while_holding(self): + outer_grip = self.opening_pos > self.closing_pos + if outer_grip and self.operation_params.position > self.gripper_pos: + return True # if currently outer grip -> only allow movement to outer side + elif not outer_grip and self.operation_params.position < self.gripper_pos: + return True # if currently inner grip -> only allow movement to inner side + else: + self.operation_response.success = False + self.operation_response.message += 'Moving into the grasped object is not possible. Move in the other direction. Current pos: {}, commanded pos: {}'.format(self.gripper_pos, self.operation_params.position) + return False + + def can_grasp(self): + least_movement = 0.5 # margin found by trial-and-error + if np.abs(self.operation_params.position - self.gripper_pos) >= least_movement: + return True + else: + self.operation_response.success = False + self.operation_response.message += 'Grasping range too little. Move by at least {}mm.. Current pos: {}, commanded pos: {}'.format(least_movement, self.gripper_pos, self.operation_params.position) + return False + + def exec_activating(self): + rospy.loginfo('State: {}'.format(self.state)) + self.serial_port_comm.send_command_synced("activate") + + def exec_deactivating(self): + rospy.loginfo('State: {}'.format(self.state)) + self.serial_port_comm.send_command_synced("deactivate") + + def exec_referencing(self): + rospy.loginfo('State: {}'.format(self.state)) + self.serial_port_comm.send_command_synced("reference") + + def set_positions(self, opening_pos, closing_pos): + pos_set = False + while not pos_set: + pos_set = self.serial_port_comm.set_opening_pos(opening_pos) + pos_set = pos_set and self.serial_port_comm.set_closing_pos(closing_pos) + self.opening_pos = opening_pos + self.closing_pos = closing_pos + rospy.loginfo("Opening pos: {}, closing pos: {}".format(opening_pos, closing_pos)) + + def exec_opening(self): + rospy.loginfo('Trying to set opening positions...') + rospy.loginfo('State: {}'.format(self.state)) + self.set_positions(opening_pos=self.operation_params.position, closing_pos=self.gripper_pos) + rospy.loginfo('Opening positions set.') + self.serial_port_comm.send_command_synced("open") + + def exec_closing(self): + rospy.loginfo('Trying to set closing positions...') + rospy.loginfo('State: {}'.format(self.state)) + self.set_positions(opening_pos=self.gripper_pos, closing_pos=self.operation_params.position) + rospy.loginfo('Closing positions set.') + self.serial_port_comm.send_command_synced("close") + + def exec_grasping(self): + rospy.loginfo('Trying to set grasping positions...') + rospy.loginfo('State: {}'.format(self.state)) + self.set_positions(opening_pos=self.gripper_pos, closing_pos=self.operation_params.position) + rospy.loginfo('Grasping positions set.') + self.serial_port_comm.send_command_synced("close") + + def exec_opening_before_closing(self): + rospy.loginfo('Trying to set opening_before_closing positions...') + rospy.loginfo('State: {}'.format(self.state)) + self.set_positions(opening_pos=self.gripper_pos, closing_pos=self.gripper_pos) + rospy.loginfo('Preopening positions set.') + self.serial_port_comm.send_command_synced("open") + + def exec_closing_before_opening(self): + rospy.loginfo('Trying to set closing_before_opening positions...') + rospy.loginfo('State: {}'.format(self.state)) + self.set_positions(opening_pos=self.gripper_pos, closing_pos=self.gripper_pos) + rospy.loginfo('Closing_before_opening positions set.') + self.serial_port_comm.send_command_synced("close") + + def exec_opening_before_referencing(self): + rospy.loginfo('Trying to set opening_before_referencing positions...') + rospy.loginfo('State: {}'.format(self.state)) + self.set_positions(opening_pos=self.gripper_pos, closing_pos=self.gripper_pos) + rospy.loginfo('Preopening positions set.') + self.serial_port_comm.send_command_synced("open") diff --git a/src/serial_comm.py b/src/weiss_gripper_ieg76/serial_comm.py similarity index 81% rename from src/serial_comm.py rename to src/weiss_gripper_ieg76/serial_comm.py index 6097bec..b5ad90e 100644 --- a/src/serial_comm.py +++ b/src/weiss_gripper_ieg76/serial_comm.py @@ -1,16 +1,9 @@ -#!/usr/bin/env python -# import roslib import struct import time import serial import rospy import threading -# import diagnostic_updater from serial import SerialException -# from std_srvs.srv import Trigger, TriggerResponse -# from weiss_gripper_ieg76.srv import * -# from sensor_msgs.msg import JointState -# from diagnostic_msgs.msg import DiagnosticStatus def create_send_payload(command, grasping_force = 100, opening_position = 29.50, closing_position = 0.5): @@ -46,7 +39,11 @@ def create_send_payload(command, grasping_force = 100, opening_position = 29.50, closing_pos_hex_byte1 = closing_pos_hex[-2:] send_cmd = "SETPARAM(" + str(grasp_index) + ", 1, [" + closing_pos_hex_byte0[0:2] + "," + closing_pos_hex_byte1[0:2] + "])\n" else: - cmd_dict = {"query":"ID?\n", "activate":"PDOUT=[02,00]\n", "operate":"OPERATE()\n", "reference":"PDOUT=[07,00]\n", "deactivate":"PDOUT=[00,00]\n", "fallback":"FALLBACK(1)\n", "mode":"MODE?\n", "restart":"RESTART()\n", "reset":"PDOUT=[00,00]\n"} + cmd_dict = {"query":"ID?\n", "activate":"PDOUT=[02,00]\n", "operate":"OPERATE()\n", "reference":"PDOUT=[06,00]\n", + "deactivate":"PDOUT=[00,00]\n", "fallback":"FALLBACK(1)\n", "mode":"MODE?\n", "restart":"RESTART()\n", + "reset":"PDOUT=[00,00]\n", "get_vendor_name":"GETPARAM(16, 0)\n", "get_vendor_text":"GETPARAM(17, 0)\n", + "get_product_name":"GETPARAM(18, 0)\n", "get_product_id":"GETPARAM(19, 0)\n", "get_product_text":"GETPARAM(20, 0)\n", + "get_serial_no":"GETPARAM(21, 0)\n", "get_device_status":"GETPARAM(36, 0)\n", "get_detailed_device_status":"GETPARAM(37, 0)\n"} # Query if command is known if not(command in cmd_dict): rospy.logerr("Command not recognized") @@ -74,6 +71,8 @@ def __init__(self, serial_port, serial_timeout): self.set_single_param_msg_length = 19 self.input_data_unavailable = 0 + self.gripper_info = {} + self.flags_observers = [] self.current_flags_dict = {"POS":0.0, "OPEN_FLAG":0b0, "CLOSED_FLAG":0b0, "HOLDING_FLAG":0b0, "FAULT_FLAG":0b0, "IDLE_FLAG":0b0, "TEMPFAULT_FLAG":0b0, "TEMPWARN_FLAG":0b0, "MAINT_FLAG":0b0} self.grasp_config = {"grasp_config_no": 0, "grasping_force": 100, "closing_position": 0.5, "opening_position": 29.5, "fresh": False} @@ -82,20 +81,25 @@ def __init__(self, serial_port, serial_timeout): self.serial_write_sync = threading.Lock() self.driver_shutdown = False - self.open_serial_port(serial_port, serial_timeout) - self.initialize_gripper() + self.serial_port = serial_port + self.serial_timeout = serial_timeout self.changing_settings = threading.Lock() # only one setting at a time self.setting_changed_successfully = False self.setting_changed = threading.Condition() + self.getting_settings = threading.Lock() # only one setting at a time + self.setting_received = threading.Condition() + threading.Thread.__init__(self) def open_serial_port(self, serial_port, serial_timeout): self.serial.port = serial_port self.serial.timeout = serial_timeout + retries = 3 while not(self.driver_shutdown): try: + retries -= 1 self.serial.open() self.serial.flushInput() self.serial.flushOutput() @@ -105,6 +109,8 @@ def open_serial_port(self, serial_port, serial_timeout): rospy.logerr("\terror opening serial port %s : %s", serial_port, e) rospy.loginfo("Retrying to open the serial port %s...", serial_port) time.sleep(1) + if retries == 0: + raise rospy.loginfo("Serial port %s opened: %s", self.serial.port, self.serial.isOpen()) @@ -133,7 +139,7 @@ def initialize_gripper(self): self.send_command("reset") time.sleep(0.5) self.input_data_unavailable = 0 - rospy.loginfo("Ready to receive requests.") + rospy.loginfo("Gripper initialized.") except Exception as e: rospy.logerr("Error reading from the serial port while (re)connect: %s", e) @@ -178,6 +184,23 @@ def change_setting(self, command, grasping_force = 100, opening_position = 29.50 self.setting_changed.wait(3.0) return self.setting_changed_successfully + def get_setting(self, command): + with self.getting_settings: # only get one setting at a time + with self.setting_received: + self.send_command_synced(command) + self.setting_received.wait(10.0) + + def get_gripper_info(self): + self.gripper_info = {} + self.get_setting("get_vendor_name") + self.get_setting("get_vendor_text") + self.get_setting("get_product_name") + self.get_setting("get_product_id") + self.get_setting("get_product_text") + self.get_setting("get_serial_no") + # self.get_setting("get_device_status") + # self.get_setting("get_detailed_device_status") + def set_force(self, grasping_force = 100): return self.change_setting("set_grasping_force", grasping_force = grasping_force) @@ -208,6 +231,29 @@ def ack_set_param(self, read_data_hexstr): self.setting_changed_successfully = True self.setting_changed.notify() + def extract_param(self, read_data_hexstr): + # example: "GETPARAM[17][0]=[4d,65,63,68,61,74,72,6f,6e,69,63,73,20,69,6e,20,41,75,74,6f,6d,61,74,69,6f,6e]" + # or: "ERR GETPARAM[16][1] 13" + setting_num = read_data_hexstr[9:11] + info = ''.join(chr(int(x,16)) for x in read_data_hexstr[read_data_hexstr.find('='):][2:-1].split(',')) + # print setting_num, info + if setting_num == "16": + self.gripper_info['vendor_name'] = info + elif setting_num == "17": + self.gripper_info['vendor_text'] = info + elif setting_num == "18": + self.gripper_info['product_name'] = info + elif setting_num == "19": + self.gripper_info['product_id'] = info + elif setting_num == "20": + self.gripper_info['product_text'] = info + elif setting_num == "21": + self.gripper_info['serial_no'] = info + else: + print("setting num {} not expected".format(setting_num)) + with self.setting_received: + self.setting_received.notify() + def extract_flags(self, read_data_hexstr): #the data read from the serial port is @PDIN=[BYTE0,BYTE1,BYTE2,BYTE3] (see pag.20 in user manual) @@ -259,13 +305,19 @@ def reconnect_serial_port(self): def parse_incoming_data_block(self, read_data_hexstr): incoming_msgs = read_data_hexstr.splitlines()#returns a list of incoming messages without the line break "\n" for msg in incoming_msgs: - if len(msg) == self.flags_msg_length: + if msg.startswith("GETPARAM["): + self.extract_param(msg) + elif len(msg) == self.flags_msg_length: self.extract_flags(msg) elif len(msg) == self.set_single_param_msg_length: self.ack_set_param(msg) def run(self): rospy.logdebug("SerialPortComm.run()") + + self.open_serial_port(self.serial_port, self.serial_timeout) + self.initialize_gripper() + #read from port connection_errors_no = 0 incoming_bytes_no = 0 @@ -290,7 +342,7 @@ def run(self): self.input_data_unavailable = 0 input_data = self.serial.read(self.serial.inWaiting()) data_str = input_data.decode('ascii') #read the bytes and convert from binary array to ASCII - if (incoming_bytes_no <> 22): + if (incoming_bytes_no != 22): rospy.logdebug("incoming_bytes_no = %d: %s", incoming_bytes_no, data_str) self.parse_incoming_data_block(data_str) else: diff --git a/src/weiss_gripper_ieg76/state_publisher.py b/src/weiss_gripper_ieg76/state_publisher.py new file mode 100644 index 0000000..641223d --- /dev/null +++ b/src/weiss_gripper_ieg76/state_publisher.py @@ -0,0 +1,85 @@ +import rospy +import threading +import diagnostic_updater +from sensor_msgs.msg import JointState +from diagnostic_msgs.msg import DiagnosticStatus + + +class StatesPublisher(threading.Thread): + def __init__(self, loop_time, serial_port_comm): + threading.Thread.__init__(self) + self.loop_time = loop_time + self.driver_shutdown = False + + self.current_flags_sync = threading.Lock() + self.current_flags_dict = None + self.current_flags_updated = False + serial_port_comm.add_flags_observer(self) + + self.joint_state_msg = JointState() + self.joint_state_msg.name = [] + self.joint_state_msg.name.append("ur5_weiss_ieg76_jaw_position") # TODO use parameter + self.joint_states_publisher = rospy.Publisher('~joint_states', JointState, queue_size=10) + + self.updater = diagnostic_updater.Updater() + self.updater.setHardwareID("Weiss Robotics Gripper ??? SN ???") + self.updater.add("Position and flags updater", self.produce_diagnostics) + freq_bounds = {'min': 0.5, 'max': 2} + # It publishes the messages and simultaneously makes diagnostics for the topic "joint_states" using a FrequencyStatus and TimeStampStatus + self.pub_freq_time_diag = diagnostic_updater.DiagnosedPublisher(self.joint_states_publisher, + self.updater, + diagnostic_updater.FrequencyStatusParam(freq_bounds, 0.1, 10), + diagnostic_updater.TimeStampStatusParam()) + + def set_gripper_info(self, gripper_info): + self.updater.setHardwareID("Weiss Robotics Gripper {} SN {}".format(gripper_info["product_id"], gripper_info["serial_no"])) + + def shutdown(self): + self.driver_shutdown = True + + def update_flags(self, new_flags_dict): + with self.current_flags_sync: + self.current_flags_dict = new_flags_dict + self.current_flags_updated = True + + def run(self): + rospy.logdebug("StatesPublisher.run()") + while not self.driver_shutdown: + with self.current_flags_sync: + if self.current_flags_updated: + self.updater.update() + rospy.logdebug("StatesPublisher publishing states...") + self.publish_states() + rospy.logdebug("StatesPublisher states published.") + #self.updater.force_update() + self.current_flags_updated = False + rospy.sleep(self.loop_time) + rospy.logdebug("states_publisher_thread done.") + + def produce_diagnostics(self, stat): + if self.current_flags_dict["FAULT_FLAG"] == True: + stat.summary(DiagnosticStatus.ERROR, "The fault bit of the gripper is 1.") + else: + stat.summary(DiagnosticStatus.OK, "The fault bit of the gripper is 0.") + stat.add("Position", self.current_flags_dict["POS"]) + stat.add("Idle Flag", self.current_flags_dict["IDLE_FLAG"]) + stat.add("Open Flag", self.current_flags_dict["OPEN_FLAG"]) + stat.add("Closed Flag", self.current_flags_dict["CLOSED_FLAG"]) + stat.add("Holding Flag", self.current_flags_dict["HOLDING_FLAG"]) + stat.add("Error Flag", self.current_flags_dict["FAULT_FLAG"]) + stat.add("Temperature Error Flag", self.current_flags_dict["TEMPFAULT_FLAG"]) + stat.add("Temperature Warning Flag", + self.current_flags_dict["TEMPWARN_FLAG"]) + stat.add("Maintenance Flag", self.current_flags_dict["MAINT_FLAG"]) + return stat + + def publish_states(self): + self.joint_state_msg.header.stamp = rospy.Time.now() + self.joint_state_msg.position = [] + MM_TO_M = 0.001 + joint_state = self.current_flags_dict["POS"] * MM_TO_M / 2.0 # joint state is half the stroke ("POS") + self.joint_state_msg.position.append(joint_state) + try: + self.pub_freq_time_diag.publish(self.joint_state_msg) + except: + rospy.logerr("\nClosed topics.") diff --git a/src/weiss_gripper_ieg76/test_client.py b/src/weiss_gripper_ieg76/test_client.py new file mode 100755 index 0000000..d050f38 --- /dev/null +++ b/src/weiss_gripper_ieg76/test_client.py @@ -0,0 +1,232 @@ +#!/usr/bin/env python +import roslib; +import sys +import rospy +import math +from std_srvs.srv import Trigger +from weiss_gripper_ieg76.srv import * + +def send_reference_request(): + rospy.wait_for_service('reference') + try: + reference = rospy.ServiceProxy('reference', Trigger) + resp = reference() + if resp.success == True: + print("Success: " + resp.message) + else: + print("Failure: " + resp.message) + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + +def send_close_request(grasp_config_no): + rospy.wait_for_service('close_jaws') + try: + close_jaws = rospy.ServiceProxy('close_jaws', ConfigTrigger) + resp = close_jaws(grasp_config_no) + if resp.success == True: + print("Success: " + resp.message) + else: + print("Failure: " + resp.message) + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + +def send_grasp_object_request(grasp_config_no): + rospy.wait_for_service('grasp_object') + try: + grasp_object = rospy.ServiceProxy('grasp_object', ConfigTrigger) + resp = grasp_object(grasp_config_no) + if resp.success == True: + print("Success: " + resp.message) + else: + print("Failure: " + resp.message) + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + +def send_open_request(grasp_config_no): + rospy.wait_for_service('open_jaws') + try: + open_jaws = rospy.ServiceProxy('open_jaws', ConfigTrigger) + resp = open_jaws(grasp_config_no) + if resp.success == True: + print("Success: " + resp.message) + else: + print("Failure: " + resp.message) + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + +def send_ack_error_request(): + rospy.wait_for_service('ack_error') + try: + ack_error = rospy.ServiceProxy('ack_error', Trigger) + resp = ack_error() + if resp.success == True: + print("Success: " + resp.message) + else: + print("Failure: " + resp.message) + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + +def send_ack_ref_error_request(): + rospy.wait_for_service('ack_ref_error') + try: + ack_ref_error = rospy.ServiceProxy('ack_ref_error', Trigger) + resp = ack_ref_error() + if resp.success == True: + print("Success: " + resp.message) + else: + print("Failure: " + resp.message) + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + +def get_all_param_request(grasp_config_no): + rospy.wait_for_service('get_all_param') + try: + get_all_param = rospy.ServiceProxy('get_all_param', GetAllParam) + resp = get_all_param(grasp_config_no) + if resp.success == True: + print("Success: " + resp.message) + print("grasping_force: " + str(resp.grasping_force) + " [%]") + print("opening_position: " + str(round(resp.opening_position, 2)) + " [mm]") + print("closing_position: " + str(round(resp.closing_position, 2)) + " [mm]") + else: + print("Failure: " + resp.message) + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + +def set_all_param_request(grasp_config_no, grasping_force, opening_position, closing_position): + rospy.wait_for_service('set_all_param') + try: + set_all_param = rospy.ServiceProxy('set_all_param', SetAllParam) + resp = set_all_param(grasp_config_no, grasping_force, opening_position, closing_position) + if resp.success == True: + print("Success: " + resp.message) + else: + print("Failure: " + resp.message) + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + +def set_grasping_force_request(grasp_config_no, grasping_force): + rospy.wait_for_service('set_grasping_force') + try: + set_grasping_force = rospy.ServiceProxy('set_grasping_force', SetGraspingForce) + resp = set_grasping_force(grasp_config_no, grasping_force) + if resp.success == True: + print("Success: " + resp.message) + else: + print("Failure: " + resp.message) + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + +def set_opening_pos_request(grasp_config_no, opening_position): + rospy.wait_for_service('set_opening_pos') + try: + set_opening_pos = rospy.ServiceProxy('set_opening_pos', SetOpeningPos) + resp = set_opening_pos(grasp_config_no, opening_position) + if resp.success == True: + print("Success: " + resp.message) + else: + print("Failure: " + resp.message) + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + +def set_closing_pos_request(grasp_config_no, closing_position): + rospy.wait_for_service('set_closing_pos') + try: + set_closing_pos = rospy.ServiceProxy('set_closing_pos', SetClosingPos) + resp = set_closing_pos(grasp_config_no, closing_position) + if resp.success == True: + print("Success: " + resp.message) + else: + print("Failure: " + resp.message) + except rospy.ServiceException as e: + print("Service call failed: %s"%e) + +def usage(): + print("-------- Commands Weiss Robotics Gripper ieg76 --------") + print("1. Reference") + print("2. Open jaws") + print("3. Close jaws") + print("4. Grasp object") + print("5. Ack error") + print("6. Ack reference error") + print("7. Select grasp configuration") + print("8. Get all the grasp configuration's param") + print("9. Set all the grasp configuration's param") + print("10. Set the grasping force") + print("11. Set the opening position") + print("12. Set the closing position") + print("13. Exit") + +if __name__ == "__main__": + selected_cmd = usage() + grasp_config_no = 0 + while True: + selected_cmd = eval(input("Select a command to send: ")) + if selected_cmd == 1: + print("Sending reference request...") + send_reference_request() + elif selected_cmd == 2: + print("Sending open jaws request...") + send_open_request(grasp_config_no) + elif selected_cmd == 3: + print("Sending close jaws request...") + send_close_request(grasp_config_no) + elif selected_cmd == 4: + print("Sending grasp object request...") + send_grasp_object_request(grasp_config_no) + elif selected_cmd == 5: + print("Sending acknowledge error request...") + send_ack_error_request() + elif selected_cmd == 6: + print("Sending acknowledge error request...") + send_ack_ref_error_request() + elif selected_cmd == 7: + print("Current grasp configuration number: " + str(grasp_config_no)) + input_data = eval(input("Enter new grasp configuration no = ")) + if input_data >=0 and input_data <=3: + grasp_config_no = input_data + else: + print("Unknown grasp config no " + str(input_data) + " given. Valid grasp configuration numbers are 0, 1, 2, 3.") + elif selected_cmd == 8: + print("Getting all the parameters (grasping force, opening position, closing position) of grasp configuration number " + str(grasp_config_no) + "...") + get_all_param_request(grasp_config_no) + elif selected_cmd == 9: + print("Setting all the parameters (grasping force, opening position, closing position) of grasp configuration number " + str(grasp_config_no) + "...") + input_data_grasping_force = eval(input("Enter the grasping force (0..100%) of grasp config no " + str(grasp_config_no) + " = ")) + input_data_opening_position = eval(input("Enter the opening position of grasp config no " + str(grasp_config_no) + " = ")) + input_data_closing_position = eval(input("Enter the closing position of grasp config no " + str(grasp_config_no) + " = ")) + if input_data_grasping_force <0 or input_data_grasping_force >100: + print("Value error " + str(input_data_grasping_force) + ". Valid grasping force values are 0..100%.") + elif input_data_opening_position <0.0 and input_data_opening_position >30.0: + print("Value error " + str(input_data_opening_position) + ". Valid opening position values are 0.0..30.0 mm.") + elif input_data_closing_position <0.0 and input_data_closing_position >30.0: + print("Value error " + str(input_data_closing_position) + ". Valid closing position values are 0.0..30.0 mm.") + else: + print("Setting all the param (grasping force " + str(input_data_grasping_force) + " [%], opening position " + str(input_data_opening_position) + " [mm], closing position " + str(input_data_closing_position) + " [mm]) of grasp config " + str(grasp_config_no) + ".") + set_all_param_request(grasp_config_no, input_data_grasping_force, input_data_opening_position, input_data_closing_position) + elif selected_cmd == 10: + input_data = eval(input("Enter the grasping force (0..100%) of grasp config no " + str(grasp_config_no) + " = ")) + if input_data >=0 and input_data <=100: + print("Setting the grasping force of grasp configuration number " + str(grasp_config_no) + "...") + set_grasping_force_request(grasp_config_no, input_data) + else: + print("Value error " + str(input_data) + ". Valid grasping force values are 0..100%.") + elif selected_cmd == 11: + input_data = eval(input("Enter the opening position of grasp config no " + str(grasp_config_no) + " = ")) + if input_data >=0.0 and input_data <=30.0: + print("Setting the opening position of grasp configuration number " + str(grasp_config_no) + "...") + set_opening_pos_request(grasp_config_no, input_data) + else: + print("Value error " + str(input_data) + ". Valid opening position values are 0.0..30.0 mm.") + elif selected_cmd == 12: + input_data = eval(input("Enter the closing position of grasp config no " + str(grasp_config_no) + " = ")) + if input_data >=0.0 and input_data <=30.0: + print("Setting the closing position of grasp configuration number " + str(grasp_config_no) + "...") + set_closing_pos_request(grasp_config_no, input_data) + else: + print("Value error " + str(input_data) + ". Valid closing position values are 0.0..30.0 mm.") + elif selected_cmd == 13: + print("Exiting the test_client...") + sys.exit(0) + else: + print("Unknown option entered.") \ No newline at end of file diff --git a/srv/Move.srv b/srv/Move.srv index 59225ad..8dc5c59 100644 --- a/srv/Move.srv +++ b/srv/Move.srv @@ -1,4 +1,5 @@ float32 position +bool relative --- bool success string message diff --git a/todo b/todo index d25ccda..73372f1 100644 --- a/todo +++ b/todo @@ -1,6 +1,4 @@ -1. Allow reference from inactive state -2. Remove old msgs -3. Track state changes in reference (first not_initialized, then inactive) 4. Add grasp service with tolerances -5. Add timeout to detect problems with an operation 6. Add dedicated thread for driver_logic flags observer +10. Find USB port that is fitting for the given serial id +11. Change name to reflect all supported grippers \ No newline at end of file diff --git a/urdf/ieg55.urdf.xacro b/urdf/ieg55.urdf.xacro index f29b50a..924bd46 100644 --- a/urdf/ieg55.urdf.xacro +++ b/urdf/ieg55.urdf.xacro @@ -23,26 +23,7 @@ - - - - - - - - - - - - - - - - - - - - + diff --git a/urdf/ieg55_adapter.urdf.xacro b/urdf/ieg55_adapter.urdf.xacro index a1ba452..d084766 100644 --- a/urdf/ieg55_adapter.urdf.xacro +++ b/urdf/ieg55_adapter.urdf.xacro @@ -29,7 +29,7 @@ - + diff --git a/urdf/ieg55_with_joints.urdf.xacro b/urdf/ieg55_with_joints.urdf.xacro new file mode 100644 index 0000000..42fadc5 --- /dev/null +++ b/urdf/ieg55_with_joints.urdf.xacro @@ -0,0 +1,53 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/urdf/ieg76.urdf.xacro b/urdf/ieg76.urdf.xacro index a9957b4..2d99f08 100644 --- a/urdf/ieg76.urdf.xacro +++ b/urdf/ieg76.urdf.xacro @@ -2,7 +2,7 @@ - +