4444
4545import roslib ; roslib .load_manifest ('robotiq_3f_gripper_control' )
4646import rospy
47- from robotiq_3f_gripper_msgs .msg import SModelRobotOutput
47+ from robotiq_3f_gripper_msgs .msg import SModelRobotOutput
4848from time import sleep
4949
5050
5151def genCommand (char , command ):
52- """Update the command according to the character entered by the user."""
53-
52+ """Update the command according to the character entered by the user."""
53+
5454 if char == 'a' :
5555 command = SModelRobotOutput ()
5656 command .rACT = 1
@@ -70,52 +70,52 @@ def genCommand(char, command):
7070
7171 if char == 'b' :
7272 command .rMOD = 0
73-
73+
7474 if char == 'p' :
7575 command .rMOD = 1
76-
76+
7777 if char == 'w' :
7878 command .rMOD = 2
79-
79+
8080 if char == 's' :
8181 command .rMOD = 3
8282
8383 #If the command entered is a int, assign this value to rPRA
84- try :
84+ try :
8585 command .rPRA = int (char )
8686 if command .rPRA > 255 :
8787 command .rPRA = 255
8888 if command .rPRA < 0 :
8989 command .rPRA = 0
9090 except ValueError :
91- pass
92-
91+ pass
92+
9393 if char == 'f' :
9494 command .rSPA += 25
9595 if command .rSPA > 255 :
9696 command .rSPA = 255
97-
97+
9898 if char == 'l' :
9999 command .rSPA -= 25
100100 if command .rSPA < 0 :
101101 command .rSPA = 0
102102
103-
103+
104104 if char == 'i' :
105105 command .rFRA += 25
106106 if command .rFRA > 255 :
107107 command .rFRA = 255
108-
108+
109109 if char == 'd' :
110110 command .rFRA -= 25
111111 if command .rFRA < 0 :
112112 command .rFRA = 0
113113
114114 return command
115-
115+
116116
117117def askForCommand (command ):
118- """Ask the user for a command to send to the gripper."""
118+ """Ask the user for a command to send to the gripper."""
119119
120120 currentCommand = 'Simple S-Model Controller\n -----\n Current command:'
121121 currentCommand += ' rACT = ' + str (command .rACT )
@@ -156,7 +156,7 @@ def askForCommand(command):
156156 strAskForCommand += 'l: Slower\n '
157157 strAskForCommand += 'i: Increase force\n '
158158 strAskForCommand += 'd: Decrease force\n '
159-
159+
160160 strAskForCommand += '-->'
161161
162162 return raw_input (strAskForCommand )
@@ -166,19 +166,19 @@ def publisher():
166166
167167 rospy .init_node ('SModelSimpleController' )
168168
169- topic_name = rospy .get_param ('~topic' , '/UR_1/ SModelRobotOutput' )
169+ topic_name = rospy .get_param ('~topic' , 'SModelRobotOutput' )
170170 pub = rospy .Publisher (topic_name , SModelRobotOutput )
171171
172172 command = SModelRobotOutput ()
173173
174174 while not rospy .is_shutdown ():
175175
176- command = genCommand (askForCommand (command ), command )
177-
176+ command = genCommand (askForCommand (command ), command )
177+
178178 pub .publish (command )
179179
180180 rospy .sleep (0.1 )
181-
181+
182182
183183if __name__ == '__main__' :
184184 publisher ()
0 commit comments