def main():
sm = smach.StateMachine(
outcomes=['mission_complete', 'mission_failed', 'aborted'])
theta = 0
with sm:
# smach.StateMachine.add('TORPEDO', Torpedo(), transitions={'torpedo_success':'mission_complete'})
# smach.StateMachine.add('DETECTBUOY', DetectBuoy(), transitions={'buoy_success':'mission_complete', 'buoy_retry': 'DETECTBUOY'})
Sink(sm, 'SINK1', 515, 'HEADING1')
Heading(sm, 'HEADING1', 75, 'FORWARD1')
Forward(sm, 'FORWARD1', 6, 'FORWARD2')
Forward(sm, 'FORWARD2', 12, 'mission_complete')
#Heading(sm, 'HEADING2', 90, 'FORWARD2')
#Forward(sm, 'FORWARD2', 10, 'HEADING3')
#Heading(sm, 'HEADING3', 90, 'FORWARD3')
#Forward(sm, 'FORWARD3', 15, 'mission_complete')
#DetectBuoy(sm, 'DETECTBUOY', 'FORWARD2')
#Forward(sm, 'FORWARD2', 14, 'HEADING2')
#Heading(sm, 'HEADING2', theta + 45, 'FORWARD3')
#Forward(sm, 'FORWARD3', 14, 'SWAY1')
#torpedo fire
#Sway(sm, 'SWAY1', -5, 'FORWARD4')
#Forward(sm, 'FORWARD4', 10, 'SINK2')
#Sink (sm, 'SINK2', 525, 'mission_complete') #resurface
#it = ImageTask() # Image Task should return User data which should be
# further mapped to Heading etc states
#it.init(sm)
sis = IntrospectionServer('ZARNA_MISSION_PLANNER', sm, '/START_ZARNA')
# start introspection server by - rosrun smach_viewer smach_viewer.py
sis.start()
outcome = sm.execute()
boolean = 1
sis.stop()
rospy.loginfo("Mission Complete")
def main():
rospy.init_node('mission_planner')
sm = smach.StateMachine(
outcomes=['mission_complete', 'mission_failed', 'aborted'])
theta = 0
with sm:
Heading(sm, 'HEADING1', theta, 'FORWARD1')
Forward(sm, 'FORWARD1', 14, 'FORWARD2')
Forward(sm, 'FORWARD2', -14, 'mission_complete')
#it = ImageTask() # Image Task should return User data which should be
# further mapped to Heading etc states
#it.init(sm)
sis = IntrospectionServer('ZARNA_MISSION_PLANNER', sm, '/START_ZARNA')
# start introspection server by - rosrun smach_viewer smach_viewer.py
sis.start()
outcome = sm.execute()
rospy.spin()
sis.stop()
#================================Task 1====================================================
if task == 1:
counter = 0
while (1):
data = updateValues()
#print("Xposition: ", data[0])
#print("Size: ", data[1])
#print("Blob count: ", data[2])
print("in outter while looooop")
if (data[2] < 1):
print("1") # object not found/ no blobs
Forward(1.53, 1.53) # turn around
print("object not found")
if (data[2] >= 1): # object detected/ blob found
data = updateValues()
print("object found not in range")
Forward(1.48, 1.48)
#stop()
if (data[0] >= 100 and data[0] <= 200): #blob found and in range
stop()
time.sleep(1)
counter = 0
else:
counter += 1
if counter > 10:
if 0 or ALL:
s = 'Minimum, assigns random series of initial x'
print('-' * len(s) + '\n' + s + '\n' + '-' * len(s))
op = Minimum(White(f))
x, y = op(x=rand(10, [-5, 5], [-7, 7]), method='nelder-mead',
silent=True)
# op.plot()
print('x:', x, 'y:', y, '\nop.x:', op.x, 'op.y:', op.y)
if 0 or ALL:
s = 'Minimum, generates series of initial x on grid'
print('-' * len(s) + '\n' + s + '\n' + '-' * len(s))
x, y = Forward(White(f))(x=grid(3, [-2, 2], [-2, 2]))
plotSurface(x[:, 0], x[:, 1], y[:, 0])
plotIsoMap(x[:, 0], x[:, 1], y[:, 0])
op = Minimum(White(f))
x, y = op(x=rand(3, [-5, 5], [-7, 7]))
op.plot()
print('x:', x, 'y:', y)
if 1 or ALL:
s = 'Minimum, test all optimizers'
print('-' * len(s) + '\n' + s + '\n' + '-' * len(s))
if True:
op = Minimum(White('demo'))
class GameEngine:
"""Putting it all together"""
"""-- Step 1:
Create few players (concrete components)
Create few field Players"""
owen = FieldPlayer("Owen")
beck = FieldPlayer("Beckham")
"""Create a goal keeper"""
khan = GoalKeeper("Khan")
"""-- Step 2:
Just make them pass the ball
(during a warm up session)"""
print " > Warm up Session... "
owen.PassBall()
beck.PassBall()
khan.PassBall()
"""-- Step 3:
Create and assign the responsibilities
(when the match starts)"""
print " > Match is starting.. "
"""Set owen as our first forward"""
forward1 = Forward()
forward1.AssignPlayer(owen)
"""Set Beckham as our midfielder"""
midfielder1 = MidFielder()
midfielder1.AssignPlayer(beck)
"""Now, use these players to do actions
specific to their roles
Owen can pass the ball"""
forward1.PassBall()
"""Beckham can dribble"""
midfielder1.Dribble()
"""Beckham can pass ball too"""
midfielder1.PassBall()
"""And Owen can shoot as well"""
forward1.ShootGoal()
"""-- Step 4:
Now, changing responsibilities
(during a substitution)"""
"""Assume that owen got injured, and we need a new player
to play as our forward1"""
print " > OOps, Owen got injured."
print "Gerrard replaced Owen.. "
"""Create a new player"""
gerrard = FieldPlayer("Gerrard")
"""Ask Gerrard to play in position of Owen"""
forward1.AssignPlayer(gerrard)
forward1.ShootGoal()
"""-- Step 5:
Adding multiple responsibilities
(When a player need to handle multiple roles)
We already have Beckham as our midfielder.
Let us ask him to play as an additional forward"""
onemoreForward = Forward()
onemoreForward.AssignPlayer(beck)
print " > Beckham has multiple responsibilities.. "
"""Now Beckham can shoot"""
onemoreForward.ShootGoal()
"""And use his earlier responsibility to dribble too"""
midfielder1.Dribble()
"""According to our design, you can attach the responsibility of
a forward to a goal keeper too, but when you actually
play football, that's rarely the case"""
print "Press any key to continue..."
raw_input()
def setSpeedsRPS(rpsLeft, rpsRight):
Forward(rpsLeft,rpsRight)
model = White(fUser)
y = model(x=x)
plotIsoMap(x[:, 0], x[:, 1], y[:, 0])
if 0 or ALL:
s = 'White box (compact)'
print('-' * len(s) + '\n' + s + '\n' + '-' * len(s))
y = White(fUser)(x=x)
plotIsoMap(x[:, 0], x[:, 1], y[:, 0])
if 0 or ALL:
s = 'Forward operator on White box model'
print('-' * len(s) + '\n' + s + '\n' + '-' * len(s))
model = White(fUser)
x, y = Forward(model)(x=md.grid(8, [-1, 8], [0, 3]))
plotIsoMap(x[:, 0], x[:, 1], y[:, 0])
if 0 or ALL:
s = 'Forward operator on demo White box'
print('-' * len(s) + '\n' + s + '\n' + '-' * len(s))
x, y = Forward(White('demo'))(x=md.cross(9, [-1, 8], [0, 3]))
plotIsoMap(x[:, 0], x[:, 1], y[:, 0])