Skip to content

Commit aec568f

Browse files
committed
added state machine
1 parent fdf3809 commit aec568f

File tree

1 file changed

+169
-0
lines changed

1 file changed

+169
-0
lines changed

scripts/machine.py

Lines changed: 169 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,169 @@
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

Comments
 (0)