5
5
import smach
6
6
import smach_ros
7
7
8
+ from controller import Controller
9
+
10
+ # Initialize controller
11
+ ctrl = Controller ()
8
12
# define state Idle
9
13
class Idle (smach .State ):
10
14
def __init__ (self ):
11
15
smach .State .__init__ (self , outcomes = ['gotToolInput' ], input_keys = ['tool_id' ], output_keys = ['tool_id' ])
12
16
13
17
def execute (self , userdata ):
14
18
userdata .tool_id = - 1
19
+ ctrl .set_camera_angles (ctrl .HOME_POS_CAMERA_01 )
20
+ ctrl .set_arm_joint_angles (ctrl .HOME_POS_MANIPULATOR_01 )
15
21
rospy .loginfo ('Executing state IDLE' )
16
22
while (True ):
17
23
# TODO: Ask for Input
@@ -21,149 +27,176 @@ def execute(self, userdata):
21
27
22
28
# Return success
23
29
return 'gotToolInput'
24
-
30
+
25
31
26
32
27
33
# define state FindTool
28
34
class FindTool (smach .State ):
29
35
def __init__ (self ):
30
- smach .State .__init__ (self , outcomes = ['foundTool' ], input_keys = ['tool_id' ])
36
+ smach .State .__init__ (self , outcomes = ['foundTool' ], input_keys = ['tool_id' ], output_keys = [ 'frame_name' ] )
31
37
32
38
def execute (self , userdata ):
33
39
rospy .loginfo ('Executing state FINDTOOL' )
34
40
print (userdata .tool_id )
41
+ ## TODO: change the name of the variable frame_name
42
+ userdata .frame_name = "random"
35
43
return 'foundTool'
36
-
44
+
37
45
# define state IK1
38
46
class IK1 (smach .State ):
39
47
def __init__ (self ):
40
- smach .State .__init__ (self , outcomes = ['noIK' ,'foundIK' ])
48
+ smach .State .__init__ (self , outcomes = ['noIK' ,'foundIK' ], input_keys = [ 'frame_name' ] )
41
49
42
50
def execute (self , userdata ):
51
+ success = ctrl .go_to_pose (userdata .frame_name )
43
52
rospy .loginfo ('Executing state IK1' )
44
- return 'foundIK'
53
+ return 'foundIK' if success else 'noIK'
45
54
46
55
# define state Move
47
- class Move (smach .State ):
48
- def __init__ (self ):
49
- smach .State .__init__ (self , outcomes = ['reached' ])
56
+ # class Move(smach.State):
57
+ # def __init__(self):
58
+ # smach.State.__init__(self, outcomes=['reached'])
50
59
51
- def execute (self , userdata ):
52
- rospy .loginfo ('Executing state IK1' )
53
- return 'reached'
60
+ # def execute(self, userdata):
61
+ # rospy.loginfo('Executing state IK1')
62
+ # return 'reached'
54
63
55
64
# define state Grab
56
65
class Grab (smach .State ):
57
66
def __init__ (self ):
58
67
smach .State .__init__ (self , outcomes = ['grabSuccess' ,'grabFailure' ])
59
68
60
69
def execute (self , userdata ):
70
+ ctrl .close_gripper ()
61
71
rospy .loginfo ('Executing state IK1' )
62
- return 'grabSuccess'
72
+ if ctrl .check_grasp ():
73
+ return 'grabSuccess'
74
+ else :
75
+ return 'grabFailure'
63
76
64
77
# define state MoveHome2
65
78
class MoveHome2 (smach .State ):
66
79
def __init__ (self ):
67
80
smach .State .__init__ (self , outcomes = ['reached' ])
68
81
69
82
def execute (self , userdata ):
83
+ ctrl .set_camera_angles (ctrl .HOME_POS_CAMERA_02 )
84
+ ctrl .set_arm_joint_angles (ctrl .HOME_POS_MANIPULATOR_02 )
70
85
rospy .loginfo ('Executing state MoveHome2' )
71
86
return 'reached'
72
87
73
88
# define state OreintCamera
74
- class OreintCamera (smach .State ):
75
- def __init__ (self ):
76
- smach .State .__init__ (self , outcomes = ['reached' ])
89
+ # class OrientCamera (smach.State):
90
+ # def __init__(self):
91
+ # smach.State.__init__(self, outcomes=['reached'])
77
92
78
- def execute (self , userdata ):
79
- rospy .loginfo ('Executing state OreintCamera ' )
80
- return 'reached'
93
+ # def execute(self, userdata):
94
+ # rospy.loginfo('Executing state OrientCamera ')
95
+ # return 'reached'
81
96
82
97
# define state FindAttention
83
98
class FindAttention (smach .State ):
99
+
84
100
def __init__ (self ):
85
- smach .State .__init__ (self , outcomes = ['giveTool' ])
101
+ smach .State .__init__ (self , outcomes = ['giveTool' ], output_keys = [ 'frame_name' ] )
86
102
87
103
def execute (self , userdata ):
104
+ ## Call service to get the frame id of the hand centroid
105
+ ## TODO: change the name of the variable frame_name
106
+ userdata .frame_name = "random"
88
107
rospy .loginfo ('Executing state FindAttention' )
89
108
return 'giveTool'
90
109
91
110
# define state IK2
92
111
class IK2 (smach .State ):
93
112
def __init__ (self ):
94
- smach .State .__init__ (self , outcomes = ['foundIK' ])
113
+ smach .State .__init__ (self , outcomes = ['foundIK' ], input_keys = [ 'frame_name' ] )
95
114
96
115
def execute (self , userdata ):
116
+ ## Wait till IK not found. Change tolerance and call compute IK again
117
+ ## Break go to pose into find pose and compute IK functions
118
+ ## Make a different function with gripper pointing upwards and call it from here
119
+ success = ctrl .go_to_pose (userdata .frame_name )
120
+ # if not success
97
121
rospy .loginfo ('Executing state IK2' )
98
122
return 'foundIK'
99
123
100
- # define state MoveGive
101
- class MoveGive (smach .State ):
102
- def __init__ (self ):
103
- smach .State .__init__ (self , outcomes = ['reached' ])
124
+ # # define state MoveGive
125
+ # class MoveGive(smach.State):
126
+ # def __init__(self):
127
+ # smach.State.__init__(self, outcomes=['reached'])
104
128
105
- def execute (self , userdata ):
106
- rospy .loginfo ('Executing state MoveGive' )
107
- return 'reached'
129
+ # def execute(self, userdata):
130
+ # rospy.loginfo('Executing state MoveGive')
131
+ # return 'reached'
108
132
109
133
# define state ChangePID
110
134
class ChangePID (smach .State ):
111
135
def __init__ (self ):
112
- smach .State .__init__ (self , outcomes = ['reached ' ])
136
+ smach .State .__init__ (self , outcomes = ['changed ' ])
113
137
114
138
def execute (self , userdata ):
139
+ ## Service call to change the PID values
115
140
rospy .loginfo ('Executing state ChangePID' )
116
- return 'reached '
141
+ return 'changed '
117
142
118
143
# define state OpenGripper
119
144
class OpenGripper (smach .State ):
120
145
def __init__ (self ):
121
- smach .State .__init__ (self , outcomes = ['reached ' ])
146
+ smach .State .__init__ (self , outcomes = ['opened ' ])
122
147
123
148
def execute (self , userdata ):
149
+ ## Detect when to Open gripper
150
+ self .open_gripper ()
124
151
rospy .loginfo ('Executing state OpenGripper' )
125
- return 'reached '
152
+ return 'opened '
126
153
127
154
def main ():
128
155
rospy .init_node ('attention_bot' )
129
156
157
+
130
158
# Create a SMACH state machine
131
159
sm = smach .StateMachine (outcomes = ['stop' ])
132
160
sm .userdata .tool_id = - 1
133
-
161
+ sis = smach_ros .IntrospectionServer ('server_name' , sm , '/SM_ROOT' )
162
+ sis .start ()
134
163
# Open the container
135
164
with sm :
136
165
# Add states to the container
137
- smach .StateMachine .add ('IDLE' , Idle (),
166
+ smach .StateMachine .add ('IDLE' , Idle (),
138
167
transitions = {'gotToolInput' :'FINDTOOL' })
139
- smach .StateMachine .add ('FINDTOOL' , FindTool (),
168
+ smach .StateMachine .add ('FINDTOOL' , FindTool (),
140
169
transitions = {'foundTool' :'IK1' })
141
- smach .StateMachine .add ('IK1' , IK1 (),
142
- transitions = {'noIK' :'stop' ,'foundIK' :'MOVE ' })
143
- smach .StateMachine .add ('MOVE' , Move (),
144
- transitions = {'reached' :'GRAB' })
145
- smach .StateMachine .add ('GRAB' , Grab (),
170
+ smach .StateMachine .add ('IK1' , IK1 (),
171
+ transitions = {'noIK' :'stop' ,'foundIK' :'GRAB ' })
172
+ # smach.StateMachine.add('MOVE', Move(),
173
+ # transitions={'reached':'GRAB'})
174
+ smach .StateMachine .add ('GRAB' , Grab (),
146
175
transitions = {'grabSuccess' :'MOVEHOME2' ,'grabFailure' :'FINDTOOL' })
147
- smach .StateMachine .add ('MOVEHOME2' , MoveHome2 (),
148
- transitions = {'reached' :'ORIENTCAMERA' })
149
- smach .StateMachine .add ('ORIENTCAMERA' , OreintCamera (),
176
+ smach .StateMachine .add ('MOVEHOME2' , MoveHome2 (),
150
177
transitions = {'reached' :'ATTENTIONSEEKER' })
151
- smach .StateMachine .add ('ATTENTIONSEEKER' , FindAttention (),
178
+ # smach.StateMachine.add('ORIENTCAMERA', OrientCamera(),
179
+ # transitions={'reached':'ATTENTIONSEEKER'})
180
+ smach .StateMachine .add ('ATTENTIONSEEKER' , FindAttention (),
152
181
transitions = {'giveTool' :'IK2' })
153
- smach .StateMachine .add ('IK2' , IK2 (),
154
- transitions = {'foundIK' :'MOVEGIVE' })
155
- smach .StateMachine .add ('MOVEGIVE' , MoveGive (),
156
- transitions = {'reached' :'CHANGEPID' })
157
- smach .StateMachine .add ('CHANGEPID' , ChangePID (),
158
- transitions = {'reached' :'OPENGRIPPER' })
159
- smach .StateMachine .add ('OPENGRIPPER' , OpenGripper (),
160
- transitions = {'reached' :'IDLE' })
161
-
182
+ smach .StateMachine .add ('IK2' , IK2 (),
183
+ transitions = {'foundIK' :'CHANGEPID' })
184
+ # smach.StateMachine.add('MOVEGIVE', MoveGive(),
185
+ # transitions={'reached':'CHANGEPID'})
186
+ smach .StateMachine .add ('CHANGEPID' , ChangePID (),
187
+ transitions = {'changed' :'OPENGRIPPER' })
188
+ smach .StateMachine .add ('OPENGRIPPER' , OpenGripper (),
189
+ transitions = {'opened' :'IDLE' })
190
+
191
+
162
192
163
193
# Execute SMACH plan
164
194
outcome = sm .execute ()
195
+ sis .stop ()
196
+
197
+
165
198
166
199
167
200
168
201
if __name__ == '__main__' :
169
- main ()
202
+ main ()
0 commit comments