diff --git a/README.md b/README.md index 2dfe824..9ba622f 100644 --- a/README.md +++ b/README.md @@ -133,6 +133,16 @@ source ~/.bashrc rosrun uarm pump_node.py off ``` +- `gripper_node.py` is the node which can control the pump on or off. + + Use this node in the **monitor-mode** of `uarm_core.py` node + ```bash + # gripper on + rosrun uarm gripper_node.py on + # gripper off + rosrun uarm gripper_node.py off + ``` + - `report_angles_node.py` is the node which will report current angles. Use this node in the **monitor-mode** of `uarm_core.py` node diff --git a/scripts/gripper_node.py b/scripts/gripper_node.py new file mode 100755 index 0000000..14969f8 --- /dev/null +++ b/scripts/gripper_node.py @@ -0,0 +1,59 @@ +#!/usr/bin/env python + +''' +# File Name : gripper_node.py +# Author : Kiru Park, Mainly refered from Joey Song's pump_node.py +# Version : V1.0 +# Date : 24 May, 2017 +# Modified Date : 24 May, 2017 +# Description : This documents is for uarm ROS Library and ROS package +''' + +# All libraries needed to import +# Import system library + +import rospy +import sys +from std_msgs.msg import String + +# raise error +def raiseError(): + print 'ERROR: Input Incorrect' + print 'ERROR: Input off / low / 0 or on / high / 1' + +# main exection function +def execute(): + + # define publisher and its topic + pub = rospy.Publisher('gripper_str_control',String,queue_size = 10) + rospy.init_node('gripper_node',anonymous = True) + rate = rospy.Rate(10) + + # process different inputs + if len(sys.argv) == 2: + data_input = sys.argv[1] + + # control pump on + if data_input.lower() == 'on' or data_input == '1' or data_input.lower() == 'high': + pub.publish('on') + + # control pump off + elif data_input.lower() == 'off' or data_input == '0' or data_input.lower() == 'low': + pub.publish('off') + else: + raiseError() + else: + raiseError() + + rate.sleep() + + +# main function +if __name__ == '__main__': + try: + execute() + except: + print '==========================================' + print 'ERROR: exectuion error' + raiseError() + pass diff --git a/scripts/uarm_core.py b/scripts/uarm_core.py index f727a13..1999e7f 100755 --- a/scripts/uarm_core.py +++ b/scripts/uarm_core.py @@ -13,7 +13,7 @@ ''' -# All libraries needed to import +# All libraries needed to import # Import system library import sys import time @@ -49,7 +49,7 @@ def readCurrentAngles(): ra['s2'] = uarm.get_servo_angle(1) ra['s3'] = uarm.get_servo_angle(2) ra['s4'] = uarm.get_servo_angle(3) - + print 'Four Servo Angles: %2.2f, %2.2f, %2.2f and %2.2f degrees.' %(ra['s1'], ra['s2'],ra['s3'],ra['s4']) rospy.set_param('servo_1',ra['s1']) @@ -66,8 +66,8 @@ def readStopperStatus(): else: print 'Stopper is not actived' rospy.set_param('stopper_status','LOW') - - + + # Write single angle def writeSingleAngle(num, angle): # limits of each servo @@ -94,10 +94,10 @@ def writeSingleAngle(num, angle): angle = 0 else: print "Input incorrect: wrong servo number" - + uarm.set_servo_angle(num-1, angle) - - + + # Write coordinate: x y z and speed def writeCoordinate(coordinate): x = float(coordinate[1]) @@ -108,21 +108,21 @@ def writeCoordinate(coordinate): elif len(coordinate) == 5: speed = float(coordinate[4]) - + else: print "Input incorrect: wrong number of arguments" - + if y > 0: y = -y - + uarm.set_position(x, y, z, speed) - + # Main connect function def connectFcn(): global failed_number - global connectionStatus + global connectionStatus connectionStatus = 0 global uarm global controlFcnLoop @@ -147,7 +147,7 @@ def connectFcn(): if len(sys.argv) == 2: failed_number = 21 uarm = pyuarm.get_uarm() - + uarm.connect() connectionStatus = 1 print 'Connected' @@ -160,6 +160,7 @@ def connectFcn(): if sys.argv[2] == 'l': failed_number = 23 uarm = pyuarm.get_uarm() + uarm.connect() controlFcnLoop = False print 'Connected' else: @@ -171,6 +172,7 @@ def connectFcn(): elif len(sys.argv) == 4: failed_number = 23 uarm = pyuarm.get_uarm() + uarm.connect() connectionStatus = 1 print 'Connected' @@ -181,7 +183,7 @@ def connectFcn(): return 25 return 23 - else: + else: # 2 means input argument is wrong return 24 #if sys.argv[1] == 'd' @@ -209,6 +211,7 @@ def controlFcn(): print 'mt x y z speed - move to a point with speed(mm/s)' print ' for example: mt 0 12 12 10' print 'pp 1/0 - pump on/off' + print 'gp 1/0 - gripper on/off' print 'ss - status of stopper' print 'at - attach uarm' print 'de - detach uarm' @@ -217,21 +220,21 @@ def controlFcn(): print '==================================================================' print ' ' - elif commands == 'l': + elif commands == 'l': print 'Exit: Break the control fuction loop' break; - - elif commands == 'e': + + elif commands == 'e': print 'Detach all servos and exit the program' uarm.set_servo_detach() sys.exit(0) elif len(commands) == 0: print 'len is 0' - + else: commands_split = commands.split() - + # Detach if commands_split[0] == 'detach'or commands_split[0] == 'de': if len(commands_split) == 1: @@ -239,7 +242,7 @@ def controlFcn(): else: print 'no other commands should be input' pass - + # Attach if commands_split[0] == 'attach'or commands_split[0] == 'at': if len(commands_split) == 1: @@ -271,6 +274,20 @@ def controlFcn(): print 'Incorrect inputs, should input 1 / 0 / HIGH / LOW / ON / OFF' pass + elif commands_split[0] == 'gripper' or commands_split[0] == 'gp': + if len(commands_split) == 1: + print 'Status should be input' + elif len(commands_split) >2: + print 'Too many inputs' + else: + if commands_split[1] == '1' or commands_split[1].lower() == 'high' or commands_split[1].lower() == 'on': + uarm.set_gripper(1) + elif commands_split[1] == '0' or commands_split[1].lower() == 'low'or commands_split[1].lower() == 'off': + uarm.set_gripper(0) + else: + print 'Incorrect inputs, should input 1 / 0 / HIGH / LOW / ON / OFF' + pass + # Current Coordinates elif commands_split[0] == 'currentCoords' or commands_split[0] == 'cc': if len(commands_split) == 1: @@ -278,7 +295,7 @@ def controlFcn(): else: print 'no other commands should be input' pass - + # Write Single Angles elif commands_split[0] == 'writeSingleAngles' or commands_split[0] == 'wsa': if len(commands_split) == 3: @@ -315,17 +332,17 @@ def controlFcn(): else: pass - + # pump control function once received data from topic def pumpCallack(data): data_input = data.data if data_input == 0: - uarm.pump_control(0) + uarm.set_pump(0) print 'Pump: Off' elif data_input == 1: - uarm.pump_control(1) + uarm.set_pump(1) print 'Pump: On' else: pass @@ -336,17 +353,46 @@ def pumpStrCallack(data): data_input = data.data print data_input - + if data_input.lower() == 'low' or data_input.lower() == 'off': - uarm.pump_control(0) + uarm.set_pump(0) print 'Pump: Off' elif data_input.lower() == 'on' or data_input.lower() == 'high': - uarm.pump_control(1) + uarm.set_pump(1) print 'Pump: On' else: pass +def gripperCallback(data): + + data_input = data.data + + if data_input == 0: + uarm.set_gripper(0) + print 'Gripper: Off' + elif data_input == 1: + uarm.set_gripper(1) + print 'Gripper: On' + else: + pass + + +# pump str control function once received data from topic +def gripperStrCallback(data): + + data_input = data.data + print data_input + + if data_input.lower() == 'low' or data_input.lower() == 'off': + uarm.set_gripper(0) + print 'Gripper: Off' + elif data_input.lower() == 'on' or data_input.lower() == 'high': + uarm.set_gripper(1) + print 'Gripper: On' + else: + pass + # angles control function once received data from topic def writeAnglesCallback(servos): @@ -370,7 +416,7 @@ def writeAnglesCallback(servos): # attach or detach uarm function once received data from topic def attchCallback(attachStatus): data_input = attachStatus.data - + if data_input.lower() == 'attach' : uarm.attach_all_servos() print 'uArm: Attach' @@ -389,7 +435,7 @@ def moveToCallback(coords): y = -y z = coords.z uarm.set_position(x, y, z, 2) - print 'Movement: Moved Once' + print 'Movement: Moved Once' # moveto functions once received data from topic @@ -405,7 +451,7 @@ def moveToTimeCallback(coordsAndT): else: uarm.set_position(x, y, z, 2) - print 'Movement: Moved Once' + print 'Movement: Moved Once' pass @@ -422,7 +468,7 @@ def moveToTimeAndS4Callback(coordsAndTS4): if s4 > 180: s4 = 180 if s4 <0 : s4 =0 uarm.set_position(x, y, z, s4, 0, time, 0, 0) - print 'Movement: Moved Once' + print 'Movement: Moved Once' pass @@ -474,17 +520,20 @@ def listener(): print '=======================================================' print ' Use rqt_graph to check the connection ' print '=======================================================' - + rospy.init_node('uarm_core',anonymous=True) rospy.Subscriber("uarm_status",String, attchCallback) rospy.Subscriber("pump_control",UInt8, pumpCallack) rospy.Subscriber("pump_str_control",String, pumpStrCallack) + rospy.Subscriber("gripper_control",UInt8, gripperCallback) + rospy.Subscriber("gripper_str_control",String, gripperStrCallback) + rospy.Subscriber("read_coords",Int32, currentCoordsCallback) rospy.Subscriber("read_angles",Int32, readAnglesCallback) rospy.Subscriber("stopper_status",Int32, stopperStatusCallback) - + rospy.Subscriber("write_angles",Angles, writeAnglesCallback) rospy.Subscriber("move_to",Coords, moveToCallback) rospy.Subscriber("move_to_time",CoordsWithTime, moveToTimeCallback) @@ -504,7 +553,7 @@ def processFailedNum(failed_number): if __name__ == '__main__': - + try: # Connect uarm first return_value = connectFcn() @@ -516,7 +565,7 @@ def processFailedNum(failed_number): if listenerFcn == True: listener() - except: + except: processFailedNum(failed_number) print 'ERROR: Execution Failed' pass @@ -524,6 +573,3 @@ def processFailedNum(failed_number): finally: print 'DONE: Program Stopped' pass - - -