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")