44
44
45
45
import roslib ; roslib .load_manifest ('robotiq_3f_gripper_control' )
46
46
import rospy
47
- from robotiq_3f_gripper_msgs .msg import SModelRobotOutput
47
+ from robotiq_3f_gripper_msgs .msg import SModelRobotOutput
48
48
from time import sleep
49
49
50
50
51
51
def 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
+
54
54
if char == 'a' :
55
55
command = SModelRobotOutput ()
56
56
command .rACT = 1
@@ -70,52 +70,52 @@ def genCommand(char, command):
70
70
71
71
if char == 'b' :
72
72
command .rMOD = 0
73
-
73
+
74
74
if char == 'p' :
75
75
command .rMOD = 1
76
-
76
+
77
77
if char == 'w' :
78
78
command .rMOD = 2
79
-
79
+
80
80
if char == 's' :
81
81
command .rMOD = 3
82
82
83
83
#If the command entered is a int, assign this value to rPRA
84
- try :
84
+ try :
85
85
command .rPRA = int (char )
86
86
if command .rPRA > 255 :
87
87
command .rPRA = 255
88
88
if command .rPRA < 0 :
89
89
command .rPRA = 0
90
90
except ValueError :
91
- pass
92
-
91
+ pass
92
+
93
93
if char == 'f' :
94
94
command .rSPA += 25
95
95
if command .rSPA > 255 :
96
96
command .rSPA = 255
97
-
97
+
98
98
if char == 'l' :
99
99
command .rSPA -= 25
100
100
if command .rSPA < 0 :
101
101
command .rSPA = 0
102
102
103
-
103
+
104
104
if char == 'i' :
105
105
command .rFRA += 25
106
106
if command .rFRA > 255 :
107
107
command .rFRA = 255
108
-
108
+
109
109
if char == 'd' :
110
110
command .rFRA -= 25
111
111
if command .rFRA < 0 :
112
112
command .rFRA = 0
113
113
114
114
return command
115
-
115
+
116
116
117
117
def 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."""
119
119
120
120
currentCommand = 'Simple S-Model Controller\n -----\n Current command:'
121
121
currentCommand += ' rACT = ' + str (command .rACT )
@@ -156,7 +156,7 @@ def askForCommand(command):
156
156
strAskForCommand += 'l: Slower\n '
157
157
strAskForCommand += 'i: Increase force\n '
158
158
strAskForCommand += 'd: Decrease force\n '
159
-
159
+
160
160
strAskForCommand += '-->'
161
161
162
162
return raw_input (strAskForCommand )
@@ -166,19 +166,19 @@ def publisher():
166
166
167
167
rospy .init_node ('SModelSimpleController' )
168
168
169
- topic_name = rospy .get_param ('~topic' , '/UR_1/ SModelRobotOutput' )
169
+ topic_name = rospy .get_param ('~topic' , 'SModelRobotOutput' )
170
170
pub = rospy .Publisher (topic_name , SModelRobotOutput )
171
171
172
172
command = SModelRobotOutput ()
173
173
174
174
while not rospy .is_shutdown ():
175
175
176
- command = genCommand (askForCommand (command ), command )
177
-
176
+ command = genCommand (askForCommand (command ), command )
177
+
178
178
pub .publish (command )
179
179
180
180
rospy .sleep (0.1 )
181
-
181
+
182
182
183
183
if __name__ == '__main__' :
184
184
publisher ()
0 commit comments