def main():
rospy.init_node('drop')
myCom = Comms(taskMode='drop')
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={
'started': 'SEARCHBOX',
'killed': 'killed'
})
smach.StateMachine.add('SEARCHBOX',
SearchBox(myCom),
transitions={
'timeout': 'DISENGAGE',
'aborted': 'DISENGAGE',
'foundBox': 'CENTERBOX'
})
smach.StateMachine.add('CENTERBOX',
CenterBox(myCom),
transitions={
'centered': 'CENTERBOX2',
'centering': 'CENTERBOX',
'lost': 'SEARCHBOX',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('CENTERBOX2',
CenterBox2(myCom),
transitions={
'centered': 'DROP',
'centering': 'CENTERBOX2',
'lost': 'DROP',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('DROP',
Drop(myCom),
transitions={
'completed': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
introServer = smach_ros.IntrospectionServer('mission_server', sm,
'/MISSION/DROP')
introServer.start()
try:
sm.execute()
except Exception as e:
rospy.logerr(str(e))
myCom.failTask()
finally:
rospy.signal_shutdown("bins task ended")
def main():
rospy.init_node('lane')
myCom = Comms(False)
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={'started':'SEARCH',
'killed':'killed'})
smach.StateMachine.add('SEARCH',
Search(myCom),
transitions={'foundLanes':'STABLIZE',
'timeout':'DISENGAGE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('STABLIZE',
Stablize(myCom),
transitions={'stablized':'ALIGN',
'stablizing':'STABLIZE',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('ALIGN',
Align(myCom),
transitions={'aligned':'CENTER',
'aligning':'ALIGN',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTER',
Center(myCom),
transitions={'centered':'FORWARD',
'centering':'CENTER',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('FORWARD',
Forward(myCom),
transitions={'completed':'DISENGAGE',
'aborted':'DISENGAGE'})
introServer = smach_ros.IntrospectionServer('mission_server',
sm,
'/MISSION/LANE')
introServer.start()
try:
sm.execute()
except Exception as e:
rospy.logerr(str(e))
myCom.failTask()
finally:
rospy.signal_shutdown("bins task ended")
def main():
rospy.init_node('drop')
myCom = Comms(taskMode='drop')
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={'started':'SEARCHBOX',
'killed':'killed'})
smach.StateMachine.add('SEARCHBOX',
SearchBox(myCom),
transitions={'timeout': 'DISENGAGE',
'aborted': 'DISENGAGE',
'foundBox': 'CENTERBOX'})
smach.StateMachine.add('CENTERBOX',
CenterBox(myCom),
transitions={'centered': 'CENTERBOX2',
'centering': 'CENTERBOX',
'lost': 'SEARCHBOX',
'aborted': 'DISENGAGE'})
smach.StateMachine.add('CENTERBOX2',
CenterBox2(myCom),
transitions={'centered': 'DROP',
'centering': 'CENTERBOX2',
'lost': 'DROP',
'aborted': 'DISENGAGE'})
smach.StateMachine.add('DROP',
Drop(myCom),
transitions={'completed': 'DISENGAGE',
'aborted': 'DISENGAGE'})
introServer = smach_ros.IntrospectionServer('mission_server',
sm,
'/MISSION/DROP')
introServer.start()
try:
sm.execute()
except Exception as e:
rospy.logerr(str(e))
myCom.failTask()
finally:
rospy.signal_shutdown("bins task ended")
def main():
rospy.init_node('bins')
myCom = Comms()
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={'started':'SEARCH',
'killed':'killed'})
smach.StateMachine.add('SEARCH',
Search(myCom),
transitions={'foundBins':'CENTER',
'timeout':'DISENGAGE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTER',
Center(myCom),
transitions={'centered':'ALIGN',
'centering':'CENTER',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('ALIGN',
Align(myCom),
transitions={'aligned':'CENTERAGAIN',
'aligning':'ALIGN',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTERAGAIN',
CenterAgain(myCom),
transitions={'centered':'FIRE',
'centering':'CENTERAGAIN',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('FIRE',
Fire(myCom),
transitions={'completed':'succeeded',
'next':'SEARCH2',
'aborted':'DISENGAGE'})
smach.StateMachine.add('SEARCH2',
Search2(myCom),
transitions={'foundBins':'CENTER2',
'lost':'DISENGAGE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTER2',
Center2(myCom),
transitions={'centered':'ALIGN',
'centering':'CENTER2',
'lost':'DISENGAGE',
'aborted':'DISENGAGE'})
introServer = smach_ros.IntrospectionServer('mission_server',
sm,
'/MISSION/BINS')
introServer.start()
try:
sm.execute()
except Exception as e:
rospy.logerr(str(e))
myCom.failTask()
finally:
rospy.signal_shutdown("bins task ended")
def main():
rospy.init_node('lane_acoustic')
myCom = Comms(True)
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={
'started': 'MAINCENTERBOX',
'killed': 'killed'
})
smach.StateMachine.add('MAINCENTERBOX',
MainCenterBox(myCom),
transitions={
'centered': 'ALIGNBOXLANE',
'centering': 'MAINCENTERBOX',
'lost': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('CENTERBOX',
CenterBox(myCom),
transitions={
'centered': 'ALIGNBOXLANE',
'centering': 'CENTERBOX',
'lost': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('ALIGNBOXLANE',
AlignBoxLane(myCom),
transitions={
'aligned': 'SEARCH',
'aligning': 'ALIGNBOXLANE',
'next_corner': 'CENTERBOX',
'lost': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('SEARCH',
Search(myCom),
transitions={
'foundLanes': 'STABLIZE',
'timeout': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('STABLIZE',
Stablize(myCom),
transitions={
'stablized': 'ALIGN',
'stablizing': 'STABLIZE',
'lost': 'SEARCH',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('ALIGN',
Align(myCom),
transitions={
'aligned': 'CENTER',
'aligning': 'ALIGN',
'lost': 'SEARCH',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('CENTER',
Center(myCom),
transitions={
'centered': 'FORWARD',
'centering': 'CENTER',
'lost': 'SEARCH',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('FORWARD',
Forward(myCom),
transitions={
'completed': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
introServer = smach_ros.IntrospectionServer('mission_server', sm,
'/MISSION/LANE')
introServer.start()
try:
sm.execute()
except Exception as e:
rospy.logerr(str(e))
myCom.failTask()
finally:
rospy.signal_shutdown("bins task ended")
def main():
rospy.init_node('bins')
myCom = Comms()
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={
'started': 'SEARCH',
'killed': 'killed'
})
smach.StateMachine.add('SEARCH',
Search(myCom),
transitions={
'foundBins': 'CENTER',
'timeout': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('CENTER',
Center(myCom),
transitions={
'centered': 'ALIGN',
'centering': 'CENTER',
'lost': 'SEARCH',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('ALIGN',
Align(myCom),
transitions={
'aligned': 'CENTERAGAIN',
'aligning': 'ALIGN',
'lost': 'SEARCH',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('CENTERAGAIN',
CenterAgain(myCom),
transitions={
'centered': 'FIRE',
'centering': 'CENTERAGAIN',
'lost': 'SEARCH',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('FIRE',
Fire(myCom),
transitions={
'completed': 'succeeded',
'next': 'SEARCH2',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('SEARCH2',
Search2(myCom),
transitions={
'foundBins': 'CENTER2',
'lost': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('CENTER2',
Center2(myCom),
transitions={
'centered': 'ALIGN',
'centering': 'CENTER2',
'lost': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
introServer = smach_ros.IntrospectionServer('mission_server', sm,
'/MISSION/BINS')
introServer.start()
try:
sm.execute()
except Exception as e:
rospy.logerr(str(e))
myCom.failTask()
finally:
rospy.signal_shutdown("bins task ended")
def main():
rospy.init_node('pickup')
myCom = Comms()
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={
'started': 'SEARCHSITE',
'killed': 'killed'
})
smach.StateMachine.add('SEARCHSITE',
SearchSite(myCom),
transitions={
'foundSite': 'CENTERSITE',
'timeout': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('CENTERSITE',
CenterSite(myCom),
transitions={
'centered': 'SEARCH',
'centering': 'CENTERSITE',
'lost': 'SEARCHSITE',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('SEARCH',
Search(myCom),
transitions={
'foundSamples': 'CENTER',
'searching': 'SEARCH',
'timeout': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('CENTER',
Center(myCom),
transitions={
'centered': 'ALIGN',
'centering': 'CENTER',
'lost': 'SEARCH',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('ALIGN',
Align(myCom),
transitions={
'aligned': 'CENTER2',
'aligning': 'ALIGN',
'lost': 'SEARCH',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('CENTER2',
Center2(myCom),
transitions={
'centered': 'APPROACH',
'centering': 'CENTER2',
'lost': 'SEARCH',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('APPROACH',
Approach(myCom),
transitions={
'completed': 'CENTER3',
'approaching': 'APPROACH',
'lost': 'SEARCH',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('CENTER3',
Center3(myCom),
transitions={
'centered': 'GRAB',
'centering': 'CENTER3',
'lost': 'GRAB',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('GRAB',
Grab(myCom),
transitions={
'grabbed': 'SURFACE',
'aborted': 'DISENGAGE'
})
smach.StateMachine.add('SURFACE',
Surface(myCom),
transitions={
'completed': 'DISENGAGE',
'aborted': 'DISENGAGE'
})
introServer = smach_ros.IntrospectionServer('mission_server', sm,
'/MISSION/PICKUP')
introServer.start()
try:
sm.execute()
except Exception as e:
rospy.logerr(str(e))
myCom.failTask()
finally:
rospy.signal_shutdown("bins task ended")
def main():
rospy.init_node('pickup')
myCom = Comms()
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'killed'])
with sm:
smach.StateMachine.add('DISENGAGE',
Disengage(myCom),
transitions={'started':'SEARCHSITE',
'killed':'killed'})
smach.StateMachine.add('SEARCHSITE',
SearchSite(myCom),
transitions={'foundSite':'CENTERSITE',
'timeout':'DISENGAGE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTERSITE',
CenterSite(myCom),
transitions={'centered':'SEARCH',
'centering':'CENTERSITE',
'lost':'SEARCHSITE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('SEARCH',
Search(myCom),
transitions={'foundSamples':'CENTER',
'searching':'SEARCH',
'timeout':'DISENGAGE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTER',
Center(myCom),
transitions={'centered':'ALIGN',
'centering':'CENTER',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('ALIGN',
Align(myCom),
transitions={'aligned': 'CENTER2',
'aligning': 'ALIGN',
'lost': 'SEARCH',
'aborted': 'DISENGAGE'})
smach.StateMachine.add('CENTER2',
Center2(myCom),
transitions={'centered':'APPROACH',
'centering':'CENTER2',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('APPROACH',
Approach(myCom),
transitions={'completed':'CENTER3',
'approaching':'APPROACH',
'lost':'SEARCH',
'aborted':'DISENGAGE'})
smach.StateMachine.add('CENTER3',
Center3(myCom),
transitions={'centered':'GRAB',
'centering':'CENTER3',
'lost':'GRAB',
'aborted':'DISENGAGE'})
smach.StateMachine.add('GRAB',
Grab(myCom),
transitions={'grabbed':'SURFACE',
'aborted':'DISENGAGE'})
smach.StateMachine.add('SURFACE',
Surface(myCom),
transitions={'completed':'DISENGAGE',
'aborted':'DISENGAGE'})
introServer = smach_ros.IntrospectionServer('mission_server',
sm,
'/MISSION/PICKUP')
introServer.start()
try:
sm.execute()
except Exception as e:
rospy.logerr(str(e))
myCom.failTask()
finally:
rospy.signal_shutdown("bins task ended")
