1
+ #!/usr/bin/env python
2
+
3
+ import roslib
4
+ import rospy
5
+ import smach
6
+ import smach_ros
7
+
8
+ # define state Idle
9
+ class Idle (smach .State ):
10
+ def __init__ (self ):
11
+ smach .State .__init__ (self , outcomes = ['gotToolInput' ], input_keys = ['tool_id' ], output_keys = ['tool_id' ])
12
+
13
+ def execute (self , userdata ):
14
+ userdata .tool_id = - 1
15
+ rospy .loginfo ('Executing state IDLE' )
16
+ while (True ):
17
+ # TODO: Ask for Input
18
+ userdata .tool_id = input ("Enter Tool ID you want:" )
19
+ if (isinstance (userdata .tool_id , int ) and userdata .tool_id > 0 and userdata .tool_id < 4 ):
20
+ break
21
+
22
+ # Return success
23
+ return 'gotToolInput'
24
+
25
+
26
+
27
+ # define state FindTool
28
+ class FindTool (smach .State ):
29
+ def __init__ (self ):
30
+ smach .State .__init__ (self , outcomes = ['foundTool' ], input_keys = ['tool_id' ])
31
+
32
+ def execute (self , userdata ):
33
+ rospy .loginfo ('Executing state FINDTOOL' )
34
+ print (userdata .tool_id )
35
+ return 'foundTool'
36
+
37
+ # define state IK1
38
+ class IK1 (smach .State ):
39
+ def __init__ (self ):
40
+ smach .State .__init__ (self , outcomes = ['noIK' ,'foundIK' ])
41
+
42
+ def execute (self , userdata ):
43
+ rospy .loginfo ('Executing state IK1' )
44
+ return 'foundIK'
45
+
46
+ # define state Move
47
+ class Move (smach .State ):
48
+ def __init__ (self ):
49
+ smach .State .__init__ (self , outcomes = ['reached' ])
50
+
51
+ def execute (self , userdata ):
52
+ rospy .loginfo ('Executing state IK1' )
53
+ return 'reached'
54
+
55
+ # define state Grab
56
+ class Grab (smach .State ):
57
+ def __init__ (self ):
58
+ smach .State .__init__ (self , outcomes = ['grabSuccess' ,'grabFailure' ])
59
+
60
+ def execute (self , userdata ):
61
+ rospy .loginfo ('Executing state IK1' )
62
+ return 'grabSuccess'
63
+
64
+ # define state MoveHome2
65
+ class MoveHome2 (smach .State ):
66
+ def __init__ (self ):
67
+ smach .State .__init__ (self , outcomes = ['reached' ])
68
+
69
+ def execute (self , userdata ):
70
+ rospy .loginfo ('Executing state MoveHome2' )
71
+ return 'reached'
72
+
73
+ # define state OreintCamera
74
+ class OreintCamera (smach .State ):
75
+ def __init__ (self ):
76
+ smach .State .__init__ (self , outcomes = ['reached' ])
77
+
78
+ def execute (self , userdata ):
79
+ rospy .loginfo ('Executing state OreintCamera' )
80
+ return 'reached'
81
+
82
+ # define state FindAttention
83
+ class FindAttention (smach .State ):
84
+ def __init__ (self ):
85
+ smach .State .__init__ (self , outcomes = ['giveTool' ])
86
+
87
+ def execute (self , userdata ):
88
+ rospy .loginfo ('Executing state FindAttention' )
89
+ return 'giveTool'
90
+
91
+ # define state IK2
92
+ class IK2 (smach .State ):
93
+ def __init__ (self ):
94
+ smach .State .__init__ (self , outcomes = ['foundIK' ])
95
+
96
+ def execute (self , userdata ):
97
+ rospy .loginfo ('Executing state IK2' )
98
+ return 'foundIK'
99
+
100
+ # define state MoveGive
101
+ class MoveGive (smach .State ):
102
+ def __init__ (self ):
103
+ smach .State .__init__ (self , outcomes = ['reached' ])
104
+
105
+ def execute (self , userdata ):
106
+ rospy .loginfo ('Executing state MoveGive' )
107
+ return 'reached'
108
+
109
+ # define state ChangePID
110
+ class ChangePID (smach .State ):
111
+ def __init__ (self ):
112
+ smach .State .__init__ (self , outcomes = ['reached' ])
113
+
114
+ def execute (self , userdata ):
115
+ rospy .loginfo ('Executing state ChangePID' )
116
+ return 'reached'
117
+
118
+ # define state OpenGripper
119
+ class OpenGripper (smach .State ):
120
+ def __init__ (self ):
121
+ smach .State .__init__ (self , outcomes = ['reached' ])
122
+
123
+ def execute (self , userdata ):
124
+ rospy .loginfo ('Executing state OpenGripper' )
125
+ return 'reached'
126
+
127
+ def main ():
128
+ rospy .init_node ('attention_bot' )
129
+
130
+ # Create a SMACH state machine
131
+ sm = smach .StateMachine (outcomes = ['stop' ])
132
+ sm .userdata .tool_id = - 1
133
+
134
+ # Open the container
135
+ with sm :
136
+ # Add states to the container
137
+ smach .StateMachine .add ('IDLE' , Idle (),
138
+ transitions = {'gotToolInput' :'FINDTOOL' })
139
+ smach .StateMachine .add ('FINDTOOL' , FindTool (),
140
+ 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 (),
146
+ transitions = {'grabSuccess' :'MOVEHOME2' ,'grabFailure' :'FINDTOOL' })
147
+ smach .StateMachine .add ('MOVEHOME2' , MoveHome2 (),
148
+ transitions = {'reached' :'ORIENTCAMERA' })
149
+ smach .StateMachine .add ('ORIENTCAMERA' , OreintCamera (),
150
+ transitions = {'reached' :'ATTENTIONSEEKER' })
151
+ smach .StateMachine .add ('ATTENTIONSEEKER' , FindAttention (),
152
+ 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
+
162
+
163
+ # Execute SMACH plan
164
+ outcome = sm .execute ()
165
+
166
+
167
+
168
+ if __name__ == '__main__' :
169
+ main ()
0 commit comments