From 8137e171a115c049140af05c4928f1d084f88b3a Mon Sep 17 00:00:00 2001 From: Chrisnonfer Date: Fri, 5 Oct 2018 16:52:17 -0700 Subject: [PATCH 1/4] Added smach_surface.py and added new state to basic_states.py --- src/ai/basic_states.py | 14 ++++++++++++++ src/ai/smach_surface.py | 34 ++++++++++++++++++++++++++++++++++ 2 files changed, 48 insertions(+) create mode 100755 src/ai/smach_surface.py diff --git a/src/ai/basic_states.py b/src/ai/basic_states.py index a403973a..b8562454 100644 --- a/src/ai/basic_states.py +++ b/src/ai/basic_states.py @@ -403,3 +403,17 @@ def callback(self, msg, userdata): c.levelOut() c.yawLeftAbsolute(userdata.heading_input) c.publish() + +class surface(SubscribeState): + def __init__(self): + SubscribeState.__init__(self, 'depth', Float32Stamped, self.surface_callback, outcomes=['success']) + + + def surface_callback(self, depth_message, userdata): + if depth_message.data < -0.1: + control = control_wrapper() + control.diveAbsolute(-0.1) + control.publish() + else: + self.exit('success') + \ No newline at end of file diff --git a/src/ai/smach_surface.py b/src/ai/smach_surface.py new file mode 100755 index 00000000..0e645d04 --- /dev/null +++ b/src/ai/smach_surface.py @@ -0,0 +1,34 @@ +#!/usr/bin/python +import rospy +import smach +import smach_ros +from start_switch import start_switch +import roulette_states +import basic_states + +class surface_ai(smach.StateMachine): + def __init__(self): + smach.StateMachine.__init__(self, outcomes=['success']) + + with self: + smach.StateMachine.add('MOVE_TO_PINGER', roulette_states.MoveToPinger(), transitions={'success': 'SURFACE', 'fail':'MOVE_TO_PINGER', 'timeout': 'MOVE_TO_PINGER'}) + smach.StateMachine.add('SURFACE', basic_states.surface(), transitions={'success': 'success'}) + + +if __name__ == '__main__': + rospy.init_node('ai') + + while rospy.get_time() == 0: + continue + + + sm = smach.StateMachine(outcomes=['success']) + with sm: + smach.StateMachine.add('START_SWITCH', start_switch(), transitions={'success': 'SURFACE_TASK'}) + smach.StateMachine.add('SURFACE_TASK', surface_ai(), transitions={'success': 'success'}) + + + sis = smach_ros.IntrospectionServer('smach_server', sm, '/SM_ROOT') + sis.start() + outcomes = sm.execute() + sis.stop() From 99635f8c0d53e573c62e4451c43f2e430a7dff74 Mon Sep 17 00:00:00 2001 From: Chrisnonfer Date: Thu, 18 Oct 2018 17:39:15 -0700 Subject: [PATCH 2/4] Put basic_states.py and smach_surface.py through the linter. --- src/ai/basic_states.py | 1 - src/ai/smach_surface.py | 10 ++++++---- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/src/ai/basic_states.py b/src/ai/basic_states.py index b8562454..1dc18295 100644 --- a/src/ai/basic_states.py +++ b/src/ai/basic_states.py @@ -416,4 +416,3 @@ def surface_callback(self, depth_message, userdata): control.publish() else: self.exit('success') - \ No newline at end of file diff --git a/src/ai/smach_surface.py b/src/ai/smach_surface.py index 0e645d04..ff4bdd6a 100755 --- a/src/ai/smach_surface.py +++ b/src/ai/smach_surface.py @@ -9,11 +9,13 @@ class surface_ai(smach.StateMachine): def __init__(self): smach.StateMachine.__init__(self, outcomes=['success']) - + with self: - smach.StateMachine.add('MOVE_TO_PINGER', roulette_states.MoveToPinger(), transitions={'success': 'SURFACE', 'fail':'MOVE_TO_PINGER', 'timeout': 'MOVE_TO_PINGER'}) + smach.StateMachine.add('MOVE_TO_PINGER', roulette_states.MoveToPinger(), + transitions={'success': 'SURFACE', 'fail': 'MOVE_TO_PINGER', + 'timeout': 'MOVE_TO_PINGER'}) smach.StateMachine.add('SURFACE', basic_states.surface(), transitions={'success': 'success'}) - + if __name__ == '__main__': rospy.init_node('ai') @@ -21,7 +23,7 @@ def __init__(self): while rospy.get_time() == 0: continue - + sm = smach.StateMachine(outcomes=['success']) with sm: smach.StateMachine.add('START_SWITCH', start_switch(), transitions={'success': 'SURFACE_TASK'}) From 07c42d1fe2321ff41ca34b60499afaf4cbb4de8b Mon Sep 17 00:00:00 2001 From: Chrisnonfer Date: Sun, 21 Oct 2018 10:25:25 -0700 Subject: [PATCH 3/4] Added DIVE state --- src/ai/smach_surface.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/ai/smach_surface.py b/src/ai/smach_surface.py index ff4bdd6a..d53318e0 100755 --- a/src/ai/smach_surface.py +++ b/src/ai/smach_surface.py @@ -11,6 +11,8 @@ def __init__(self): smach.StateMachine.__init__(self, outcomes=['success']) with self: + smach.StateMachine.add('DIVE', basic_states.goToDepth(0.5), + transitions={'success': 'MOVE_TO_PINGER', 'fail': 'FAIL', 'timeout': 'FAIL'}) smach.StateMachine.add('MOVE_TO_PINGER', roulette_states.MoveToPinger(), transitions={'success': 'SURFACE', 'fail': 'MOVE_TO_PINGER', 'timeout': 'MOVE_TO_PINGER'}) From 185cf30bc06aafa52f2992b03f2a8ff0bc3e13ff Mon Sep 17 00:00:00 2001 From: Chrisnonfer Date: Sun, 21 Oct 2018 10:26:11 -0700 Subject: [PATCH 4/4] Added DIVE state --- src/ai/smach_surface.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/ai/smach_surface.py b/src/ai/smach_surface.py index d53318e0..27ec74d0 100755 --- a/src/ai/smach_surface.py +++ b/src/ai/smach_surface.py @@ -11,7 +11,7 @@ def __init__(self): smach.StateMachine.__init__(self, outcomes=['success']) with self: - smach.StateMachine.add('DIVE', basic_states.goToDepth(0.5), + smach.StateMachine.add('DIVE', basic_states.goToDepth(-0.5), transitions={'success': 'MOVE_TO_PINGER', 'fail': 'FAIL', 'timeout': 'FAIL'}) smach.StateMachine.add('MOVE_TO_PINGER', roulette_states.MoveToPinger(), transitions={'success': 'SURFACE', 'fail': 'MOVE_TO_PINGER',