def __init__(self):
rospy.init_node('patrol_smach', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# A list to hold then navigation waypoints
nav_states = list()
# Turn the waypoints into SMACH states
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'base_footprint'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1','preempted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'','aborted':'','preempted':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT')
intro_server.start()
# Execute the state machine for the specified number of patrols
while (self.n_patrols == -1 or self.patrol_count < self.n_patrols) and not rospy.is_shutdown():
sm_outcome = self.sm_patrol.execute()
self.patrol_count += 1
rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count))
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
rospy.init_node('competition2')
wall_offset = 0.4
marker = ARTag(20, visual_topic='/viz/marker')
cube = ARCube(2, visual_topic='/viz/cube')
cam_pixel_to_point = CamPixelToPointServer()
squares = []
for i in range(1, 9):
squares.append(ParkingSquare(i))
sq = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
with sq:
# Sequence.add('START', WaitForJoyState())
Sequence.add('STOP1', move_to_stop_line())
Sequence.add('LOCATION1', location1(cam_pixel_to_point))
Sequence.add('STOP2', move_to_stop_line())
Sequence.add('SPLIT_ENTER', enter_split())
Sequence.add('OBSTACLE1', move_to_obstacle())
Sequence.add('MOVEFORWARD', ForwardState(0.2, 1))
Sequence.add('LOCATION2', location2())
Sequence.add('SPLIT_EXIT', exit_split())
Sequence.add('STOP3', move_to_stop_line())
Sequence.add('OFFRAMP', off_ramp())
#
Sequence.add('SEARCH_FOR_MARKERS', search_for_tags(squares, marker, cube))
# Sequence.add('GO_TO_MARKER', go_to_marker(marker, squares))
Sequence.add('PUSH_CUBE', push_cube(cube, marker, squares))
Sequence.add('FIND_SHAPE', find_shape(squares, cam_pixel_to_point))
#
Sequence.add('ONRAMP', on_ramp())
Sequence.add('STOP4', move_to_stop_line(lower=True))
Sequence.add('LOCATION3', location3(cam_pixel_to_point))
Sequence.add('STOP5', move_to_stop_line())
Sequence.add('NOTIFY', FunctionState(lambda: notify_finished()))
Sequence.add('RESET', FunctionState(lambda: marker.reset()))
Sequence.add('REDO', ReturnFunctionState(lambda: 'err', ['ok', 'err']), transitions={'err': 'STOP1'})
# sq.userdata.green_shape = 'square'
sis = IntrospectionServer('smach_server', sq, '/SM_ROOT')
sis.start()
print('Executing...')
sq.execute()
def main():
rospy.init_node('bbauv_suavc_smach')
sauvc = smach.StateMachine(outcomes=['succeeded','preempted'])
with sauvc:
smach.StateMachine.add('COUNTDOWN_START',Countdown(1.0),transitions={'succeeded':'SMACH_TRUE'})
smach.StateMachine.add('SMACH_TRUE',SmachSwitch(True),transitions={'succeeded':'VISION_TRACK_TRUE'})
smach.StateMachine.add('VISION_TRACK_TRUE',TrackerSwitch(False),transitions={'succeeded':'NAV_TRUE'})
smach.StateMachine.add('NAV_TRUE',NavigationSwitch(True),transitions={'succeeded':'TOPSIDE_FALSE'})
smach.StateMachine.add('TOPSIDE_FALSE',TopsideSwitch(False),transitions={'succeeded':'COUNTDOWN_DIVE'})
smach.StateMachine.add('COUNTDOWN_DIVE',Countdown(1),transitions={'succeeded':'VISION_TRACK_TRUE1'})
smach.StateMachine.add('VISION_TRACK_TRUE1',TrackerSwitch(True),transitions={'succeeded':'NAV_TRUE1'})
smach.StateMachine.add('NAV_TRUE1',NavigationSwitch(False),transitions={'succeeded':'COUNTDOWN_MISSION2'})
smach.StateMachine.add('COUNTDOWN_MISSION2',Countdown(180),transitions={'succeeded':'TOPSIDE_TRUE'})
smach.StateMachine.add('TOPSIDE_TRUE',TopsideSwitch(True),transitions={'succeeded':'COUNTDOWN_END'})
smach.StateMachine.add('COUNTDOWN_END',Countdown(0.1),transitions={'succeeded':'VISION_TRACK_FALSE'})
smach.StateMachine.add('VISION_TRACK_FALSE',TrackerSwitch(False),transitions={'succeeded':'NAV_FALSE'})
smach.StateMachine.add('NAV_FALSE',NavigationSwitch(False),transitions={'succeeded':'SMACH_FALSE'})
smach.StateMachine.add('SMACH_FALSE',SmachSwitch(False),transitions={'succeeded':'succeeded'})
sis = IntrospectionServer('my_introspserver', sauvc,'/SM_ROOT')
sis.start()
outcome = sauvc.execute()
rospy.on_shutdown(shutdown)
rospy.spin()
sis.stop()
def main():
rospy.init_node('sm_detect')
sm0 = StateMachine(outcomes=['succeeded', 'not successful'])
sm0.userdata.action = ''
sm0.userdata.pose = Pose()
with sm0:
smach.StateMachine.add('UI_detection',
UI_detection(),
transitions={'outcome1': 'Answer'})
smach.StateMachine.add('Answer',
Answer(),
transitions={
'success': 'succeeded',
'retry': 'not successful'
})
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
#smach.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target=sm0.execute)
smach_thread.start()
# Signal handler
rospy.spin()
def main():
rospy.init_node('smach_example_state_machine')
# Create a SMACH state machine
sm = StateMachine(outcomes=['outcome4'])
sm.userdata.sm_counter = 0
sis = IntrospectionServer('example', sm, '/SM_ROOT')
sis.start()
# Open the container
with sm:
# Add states to the container
StateMachine.add('FOO',
Foo(),
transitions={
'outcome1': 'BAR',
'outcome2': 'outcome4'
},
remapping={
'foo_counter_in': 'sm_counter',
'foo_counter_out': 'sm_counter'
})
StateMachine.add('BAR',
Bar(),
transitions={'outcome1': 'FOO'},
remapping={'bar_counter_in': 'sm_counter'})
# Execute SMACH plan
outcome = sm.execute()
rospy.spin()
def __init__(self):
rospy.init_node('monitor_fake_battery', anonymous=False)
rospy.on_shutdown(self.shutdown)
# Set the low battery threshold (between 0 and 100)
self.low_battery_threshold = rospy.get_param('~low_battery_threshold', 50)
# Initialize the state machine
sm_battery_monitor = StateMachine(outcomes=[])
with sm_battery_monitor:
# Add a MonitorState to subscribe to the battery level topic
StateMachine.add('MONITOR_BATTERY',
MonitorState('battery_level', Float32, self.battery_cb),
transitions={'invalid':'RECHARGE_BATTERY',
'valid':'MONITOR_BATTERY',
'preempted':'MONITOR_BATTERY'},)
# Add a ServiceState to simulate a recharge using the set_battery_level service
StateMachine.add('RECHARGE_BATTERY',
ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, request=100),
transitions={'succeeded':'MONITOR_BATTERY',
'aborted':'MONITOR_BATTERY',
'preempted':'MONITOR_BATTERY'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('monitor_battery', sm_battery_monitor, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_battery_monitor.execute()
intro_server.stop()
def main():
rospy.init_node('smach_usecase_executive')
sm_root = StateMachine(outcomes=['preempted', 'aborted', 'succeeded'])
with sm_root:
StateMachine.add('RESET',
ServiceState('/reset', Empty, request=EmptyRequest()),
transitions={'succeeded': 'SPAWN'})
StateMachine.add(
'SPAWN',
ServiceState('/spawn', Empty, request=EmptyRequest()),
#request=SpawnRequest(1.0, 1.0, 2.0, 'kkk')),
transitions={'aborted': 'aborted'})
# transitions={
# 'preemtped': 'preemtped',
# 'aborted': 'aborted',
# 'succeeded': 'succeeded'})
sis = IntrospectionServer('exmaple', sm_root, '/USE_CASE')
sis.start()
outcome = sm_root.execute()
rospy.spin()
def main():
rospy.init_node('tinker_mission_follow')
rospy.loginfo(colored('starting follow and guide task ...', 'green'))
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': '1_Start'})
StateMachine.add('1_Start', MonitorKinectBodyState(),
transitions={'valid':'1_Start', 'invalid':'Sequence'})
sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'], connector_outcome='succeeded')
with sequence:
follow_concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=[])
with follow_concurrence:
Concurrence.add('FollowMe', FollowMeState())
Concurrence.add('KeyWordsRecognition', KeywordsRecognizeState('stop'))
Sequence.add('Follow_concurrence', follow_concurrence)
StateMachine.add('Sequence', sequence, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def main():
rospy.init_node('smach_example_state_machine')
init_robot_pose()
move_base_override.init()
reset_robot_actuators()
odom_sub = rospy.Subscriber('/odom', Odometry, odometry_cb)
sq = Sequence(outcomes=[Transitions.SUCCESS, Transitions.FAILURE],
connector_outcome=Transitions.SUCCESS)
with sq:
Sequence.add('waiting', WaitStartState())
for i in range(3):
Sequence.add('fishing {}'.format(i), create_fish_sequence())
# Sequence.add('inner_door', create_door_state_machine(0.3))
# Sequence.add('outer_door', create_door_state_machine(0.6))
# Create and start the introspection server
sis = IntrospectionServer('strat', sq, '/strat')
sis.start()
# Execute the state machine
outcome = sq.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def main():
rospy.init_node('sm_detect')
sm0 = StateMachine(outcomes=['succeeded','not successful'])
sm0.userdata.action=''
sm0.userdata.pose=Pose()
with sm0:
smach.StateMachine.add('UI_detection', UI_detection(),
transitions={'outcome1':'Answer'})
smach.StateMachine.add('Answer', Answer(),
transitions={'success':'succeeded','retry':'not successful'})
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
#smach.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target = sm0.execute)
smach_thread.start()
# Signal handler
rospy.spin()
def __init__(self):
rospy.init_node('patrol_smach', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
self.patrol_count = 0
self.n_patrols = 1
# A random number generator for use in the transition callback
self.rand = Random()
# A list to hold then nav_states
nav_states = list()
# Turn the waypoints into SMACH states
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Register a callback function to fire on state transitions within the sm_patrol state machine
self.sm_patrol.register_transition_cb(self.transition_cb, cb_args=[])
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'NAV_STATE_0','aborted':'NAV_STATE_0'})
StateMachine.add('CHECK_COUNT', CheckCount(self.n_patrols), transitions={'succeeded':'NAV_STATE_0','aborted':'','preempted':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT')
intro_server.start()
# Execute the state machine
self.sm_outcome = self.sm_patrol.execute()
rospy.loginfo('State Machine Outcome: ' + str(self.sm_outcome))
intro_server.stop()
os._exit(0)
def main():
rospy.init_node('smach_usecase_step_04')
# Construct static goals
polygon_big = turtle_actionlib.msg.ShapeGoal(edges = 11, radius = 4.0)
polygon_small = turtle_actionlib.msg.ShapeGoal(edges = 6, radius = 0.5)
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Open the container
with sm0:
# Reset turtlesim
StateMachine.add('RESET',
ServiceState('reset', std_srvs.srv.Empty),
{'succeeded':'SPAWN'})
# Create a second turtle
StateMachine.add('SPAWN',
ServiceState('spawn', turtlesim.srv.Spawn,
request = turtlesim.srv.SpawnRequest(0.0,0.0,0.0,'turtle2')),
{'succeeded':'TELEPORT1'})
# Teleport turtle 1
StateMachine.add('TELEPORT1',
ServiceState('turtle1/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(5.0,1.0,0.0)),
{'succeeded':'TELEPORT2'})
# Teleport turtle 2
StateMachine.add('TELEPORT2',
ServiceState('turtle2/teleport_absolute', turtlesim.srv.TeleportAbsolute,
request = turtlesim.srv.TeleportAbsoluteRequest(9.0,5.0,0.0)),
{'succeeded':'BIG'})
# Draw a large polygon with the first turtle
StateMachine.add('BIG',
SimpleActionState('turtle_shape1',turtle_actionlib.msg.ShapeAction,
goal = polygon_big),
{'succeeded':'SMALL'})
# Draw a small polygon with the second turtle
StateMachine.add('SMALL',
SimpleActionState('turtle_shape2',turtle_actionlib.msg.ShapeAction,
goal = polygon_small))
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Set preempt handler
smach_ros.set_preempt_handler(sm0)
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target = sm0.execute)
smach_thread.start()
# Signal handler
rospy.spin()
def __init__(self):
rospy.init_node('rfid_demo', anonymous=True)
rospy.logout('demo_node: Initializing')
rospy.logout('demo_node: Waiting for services')
rospy.wait_for_service('/rfid_servo/servo')
rospy.wait_for_service('/rfid_orient/orient')
rospy.wait_for_service('/rfid_orient/flap')
rospy.wait_for_service('/rfid_handoff/initialize')
rospy.wait_for_service('/rfid_handoff/handoff')
rospy.wait_for_service('/rfid_handoff/wave')
#rospy.wait_for_service( '/rfid_gui/select' )
rospy.logout('demo_node: All services ready.')
rospy.logout('demo_node: Setting up state machine.')
self.sm = sm_rfid_delivery.sm_delivery()
sis = IntrospectionServer('RFID_delivery', self.sm, '/SM_ROOT')
sis.start()
rospy.logout('demo_node: Done setting up state machine.')
self.last_button = time.time()
self.run_demo = False
self.run_hoinit = False
self.run_handoff = False
self._joy_sub = rospy.Subscriber('joy', Joy, self.joy_process)
self._servo = rospy.ServiceProxy('/rfid_servo/servo', ServoSrv)
self.follow1 = lambda: self._servo('person ', 1
) # Stops at first obs
self.follow = lambda: self._servo('person ', 0) # Runs forever
self._handoff = rospy.ServiceProxy('/rfid_handoff/handoff', HandoffSrv)
self.handoff = lambda: self._handoff()
self._hoinit = rospy.ServiceProxy('/rfid_handoff/initialize',
HandoffSrv)
self.hoinit = lambda: self._hoinit()
self._howave = rospy.ServiceProxy('/rfid_handoff/wave', HandoffSrv)
self.howave = lambda: self._howave()
self._flap = rospy.ServiceProxy('/rfid_orient/flap', FlapEarsSrv)
self.flap = lambda: self._flap('')
self._orient = rospy.ServiceProxy('/rfid_orient/orient', String_Int32)
self.orient = lambda tagid: self._orient(tagid)
#self._gui = rospy.ServiceProxy( '/rfid_gui/select', gui_srv )
#self.gui = lambda tags: self._gui( tags )
self._service = rospy.Service('/rfid_demo/demo', Empty, self.demo)
# self._service = rospy.Service( '/rfid_demo/gui', Empty, self.gui_test )
rospy.logout('demo_node: Demo service ready!')
def main():
rospy.init_node("iterator_tutorial")
sm_iterator = construct_sm()
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('iterator_tutorial',sm_iterator,'/ITERATOR_TUTORIAL')
intro_server.start()
outcome = sm_iterator.execute()
rospy.spin()
intro_server.stop()
def __init__(self):
rospy.init_node("rfid_demo", anonymous=True)
rospy.logout("demo_node: Initializing")
rospy.logout("demo_node: Waiting for services")
rospy.wait_for_service("/rfid_servo/servo")
rospy.wait_for_service("/rfid_orient/orient")
rospy.wait_for_service("/rfid_orient/flap")
rospy.wait_for_service("/rfid_handoff/initialize")
rospy.wait_for_service("/rfid_handoff/handoff")
rospy.wait_for_service("/rfid_handoff/wave")
# rospy.wait_for_service( '/rfid_gui/select' )
rospy.logout("demo_node: All services ready.")
rospy.logout("demo_node: Setting up state machine.")
self.sm = sm_rfid_delivery.sm_delivery()
sis = IntrospectionServer("RFID_delivery", self.sm, "/SM_ROOT")
sis.start()
rospy.logout("demo_node: Done setting up state machine.")
self.last_button = time.time()
self.run_demo = False
self.run_hoinit = False
self.run_handoff = False
self._joy_sub = rospy.Subscriber("joy", Joy, self.joy_process)
self._servo = rospy.ServiceProxy("/rfid_servo/servo", ServoSrv)
self.follow1 = lambda: self._servo("person ", 1) # Stops at first obs
self.follow = lambda: self._servo("person ", 0) # Runs forever
self._handoff = rospy.ServiceProxy("/rfid_handoff/handoff", HandoffSrv)
self.handoff = lambda: self._handoff()
self._hoinit = rospy.ServiceProxy("/rfid_handoff/initialize", HandoffSrv)
self.hoinit = lambda: self._hoinit()
self._howave = rospy.ServiceProxy("/rfid_handoff/wave", HandoffSrv)
self.howave = lambda: self._howave()
self._flap = rospy.ServiceProxy("/rfid_orient/flap", FlapEarsSrv)
self.flap = lambda: self._flap("")
self._orient = rospy.ServiceProxy("/rfid_orient/orient", String_Int32)
self.orient = lambda tagid: self._orient(tagid)
# self._gui = rospy.ServiceProxy( '/rfid_gui/select', gui_srv )
# self.gui = lambda tags: self._gui( tags )
self._service = rospy.Service("/rfid_demo/demo", Empty, self.demo)
# self._service = rospy.Service( '/rfid_demo/gui', Empty, self.gui_test )
rospy.logout("demo_node: Demo service ready!")
def main():
rospy.init_node('pr2_touch_simple')
smts = SMTouchSimple()
sm = smts.get_sm_basic()
rospy.sleep(1)
sis = IntrospectionServer('touch_simple', sm, '/INPUT_START_CLICK')
sis.start()
outcome = sm.execute()
sis.stop()
def main():
rospy.init_node('register_head_ellipse')
smer = SMEllipsoidRegistration()
sm = smer.get_sm()
rospy.sleep(1)
sis = IntrospectionServer('register_head', sm, '/UNFREEZE_PC')
sis.start()
outcome = sm.execute()
sis.stop()
def main():
rospy.init_node('smach_sm_touch_face')
smna = SMNavApproach()
sm = smna.get_sm()
rospy.sleep(1)
sis = IntrospectionServer('nav_prep', sm, '/SM_NAV_PREP')
sis.start()
outcome = sm.execute()
sis.stop()
def main():
rospy.init_node('tinker_mission_navigation')
rospy.loginfo(colored('starting navigation task ...', 'green'))
# load waypoints from xml
pose_list = TKpos.PoseList.parse(open(rospy.get_param('~waypoint_xml'), 'r'))
for pose in pose_list.pose:
WayPointGoalState.waypoint_dict[pose.name] = TKpos.Pose.X(pose)
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': 'Sequence'})
sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence:
# Sequence.add('GoToWaypoin1', WayPointGoalState('waypoint1'), transitions={'aborted': 'GoToWaypoin1'})
# Sequence.add('ArriveWaypoint1', SpeakState('I have arrived at way point one'))
# Sequence.add('GoToWaypoin2', WayPointGoalState('waypoint2'),
# transitions={'succeeded': 'ArriveWaypoint2', 'aborted': 'Obstacle'})
# Sequence.add('Obstacle', SpeakState('Obstacle in front of me'))
# Sequence.add('ObstacleDelay', DelayState(10),
# transitions={'succeeded': 'GoToWaypoin2'})
# Sequence.add('ArriveWaypoint2', SpeakState('I have arrived at way point two'))
Sequence.add('GoToWaypoin3', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3'})
Sequence.add('ArriveWaypoint3', SpeakState('I have arrived at way point three'))
Sequence.add('StopCommandAndGo', SpeakState('Please GO. If you want to stop, say stop tinker'))
Sequence.add('Train_human', FollowTrainState())
follow_concurrence = Concurrence(outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded', child_termination_cb=lambda x: True, input_keys=[])
with follow_concurrence:
Concurrence.add('FollowMe', FollowMeState())
# Concurrence.add('KeyWordsRecognition', KeywordsRecognizeState('stop'))
Sequence.add('Follow_concurrence', follow_concurrence)
Sequence.add('GoToWaypoin3Again', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3Again'})
Sequence.add('ArriveWaypoint3Again', SpeakState('I am back at way point three'))
Sequence.add('GoOut', WayPointGoalState('out'), transitions={'aborted': 'GoOut'})
StateMachine.add('Sequence', sequence, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def main():
rospy.init_node("sm_registration_setup")
smrs = SMRegistrationSetup()
sm = smrs.get_sm()
if False:
sm.set_initial_state(['SETUP_TASK_CONTROLLER'])
rospy.sleep(1)
sis = IntrospectionServer('registration_setup', sm, '/NAV_APPROACH')
sis.start()
outcome = sm.execute()
sis.stop()
def execute_smach_container(smach_container, enable_introspection=False,
name='/SM_ROOT', userdata=UserData()):
if not rospy.core.is_initialized():
rospy.init_node('smach')
if enable_introspection:
# Create and start the introspection server
sis = IntrospectionServer('smach_executor', smach_container, name)
sis.start()
outcome = smach_container.execute(userdata)
_logger.info("smach outcome: %s", outcome)
if enable_introspection:
sis.stop()
return outcome
def main():
rospy.init_node('tinker_mission_person_recognition')
rospy.loginfo(colored('starting person recognition task ...', 'green'))
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('0_wait_for_start', MonitorStartButtonState(), transitions={'valid': '0_wait_for_start','invalid': '0_arm_mode'})
StateMachine.add('0_arm_mode', ArmModeState(ArmModeState.Arm_Mode_Kinect), transitions={'succeeded':'0_wait_for_human'})
StateMachine.add('0_wait_for_human', SpeakState('I am tinker , I am waiting for operator'),
transitions={'succeeded': '1_Start'})
StateMachine.add('1_Start', MonitorKinectBodyState(), transitions={'valid':'1_Start', 'invalid':'Sequence'})
sequence = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence:
Sequence.add('2_1_train_speak', SpeakState('please look at the camera'))
Sequence.add('2_2_train', TrainPersonState())
Sequence.add('2_3_train_finish', SpeakState('I have memorized you, thanks'))
Sequence.add('3_wait', DelayState(10))
Sequence.add('4_turn_back', ChassisSimpleMoveState(theta=3.23))
Sequence.add('4_1_turn_back', ChassisSimpleMoveState(x=-1.0))
Sequence.add('5_1_find_operator', FindPersonState())
Sequence.add('5_2_point_operator', MoveArmState(offset=Point(0, 0, 0)), remapping={'target':'person_pose'})
Sequence.add('5_3_say', SpeakState('I have found you'))
Sequence.add('5_4_wait', DelayState(5))
StateMachine.add('Sequence', sequence, {'succeeded': 'Sequence_reset', 'aborted': 'Sequence_reset'})
sequence_reset = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence_reset:
Sequence.add('6_1_arm_init', ArmModeState(ArmModeState.Arm_Mode_Init))
Sequence.add('6_2_report', GenerateReportState(image='human_result.png', text='human_result.txt'))
Sequence.add('6_3_finish', SpeakState('task finished'))
StateMachine.add('Sequence_reset', sequence_reset, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_person_recognition', state, '/tinker_mission_person_recognition')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def visit_waypoints():
pose1 = Pose()
pose1.position.z = 1.0
pose1.position.x = 2
pose2 = Pose()
pose2.position.z = 0.5
sm = create_sequence(
[
GoToState(pose1),
GoToState(pose2),
]
)
introspection_server = IntrospectionServer(str(rospy.get_name()), sm, '/sm_root')
introspection_server.start()
sm.execute()
def main():
rospy.init_node('ar_parking')
marker_tracker = MarkerTracker()
seq = Sequence(outcomes=['ok', 'err'], connector_outcome='ok')
with seq:
Sequence.add('NavigateToStart',
NavigateToNamedPoseState('ar_parking_start'))
Sequence.add(
'FindMarker',
FindMarkerState(marker_tracker, '/cmd_vel_mux/input/teleop'))
Sequence.add('NavigateToMarker',
NavigateToMarkerState(marker_tracker),
transitions={'ok': 'NavigateToStart'})
sis = IntrospectionServer('smach_server', seq, '/SM_ROOT')
sis.start()
seq.execute()
def main():
v = 0.2
w = 1
turn_time = 1.2
move_time = 1.5
marker_tracker = MarkerTracker()
sm = StateMachine(outcomes=['ok', 'err', 'finished'])
with sm:
StateMachine.add('find_marker',
FindMarker(marker_tracker),
transitions={
'ok': 'choose_goal',
'err': None
})
StateMachine.add('choose_goal',
ChooseNewNavGoalState(np.array([0., 0.]), 1.),
transitions={'ok': 'go_to_goal'})
StateMachine.add('go_to_goal',
NavigateToGoalState(),
transitions={
'ok': 'push_box',
'err': None
})
StateMachine.add('push_box',
ActionUntil(
MoveForwardAction(v),
CombinedListener(
[FinishedListener(),
NoBumperListener()])),
transitions={
'finished': None,
'released': 'backup',
})
StateMachine.add('backup',
ActionUntil(MoveForwardAction(-v), TimerListener(5)),
transitions={'timeout': 'find_marker'})
sis = IntrospectionServer('smach_server', sm, '/SM_ROOT')
sis.start()
sm.execute()
def main():
rospy.init_node('competition2')
sq = Sequence(outcomes=['ok'], connector_outcome='ok')
with sq:
Sequence.add('START', WaitForJoyState())
Sequence.add('STOP1', MoveToStopLineGroup())
Sequence.add('LOCATION1', Location1Group())
Sequence.add('STOP2', MoveToStopLineGroup())
Sequence.add('SPLIT_ENTER', EnterSplitGroup())
Sequence.add('OBSTACLE1', MoveToObstacleGroup())
Sequence.add('LOCATION2', Location2Group())
Sequence.add('SPLIT_EXIT', ExitSplitGroup())
Sequence.add('STOP3', MoveToStopLineGroup())
Sequence.add('OFFRAMP', OffRampGroup())
Sequence.add('ARTAG', ArTagGroup())
Sequence.add('JOYLOC', JoystickLocationGroup())
# Sequence.add('FINDSHAPE', FindShapeGroup())
Sequence.add('ONRAMP', OnRampGroup())
Sequence.add('STOP4', MoveToStopLineGroup(lower=True))
Sequence.add('LOCATION3', Location3Group())
Sequence.add('STOP5', MoveToStopLineGroup())
# sq.userdata.green_shape = 'triangle'
sis = IntrospectionServer('smach_server', sq, '/SM_ROOT')
sis.start()
sq.execute()
def __init__(self):
rospy.init_node('TestFollow')
rospy.on_shutdown(self.shutdown)
rospy.logerr('Follow start')
self.smach_bool = False
self.newFollow = StateMachine(outcomes = ['succeeded' , 'aborted','error'])
with self.newFollow:
StateMachine.add('RUNNODE' , RunNode() , transitions = {'succeeded':'FOLLOW' ,
'aborted':'RUNNODE',
})
StateMachine.add('FOLLOW' ,FollowTest() , transitions = {'succeeded':'succeeded' ,
'error':'error',
'aborted':'aborted'} )
intro_server = IntrospectionServer('sm_gpsr',self.newFollow,'/SM_ROOT')
intro_server.start()
out = self.newFollow.execute()
rospy.logerr(out)
intro_server.stop()
self.smach_bool = True
def __init__(self):
rospy.init_node('move_to_and_inspect_point_sm', anonymous=False)
hsm = StateMachine(outcomes=['finished statemachine'])
with hsm:
StateMachine.add( 'GO_TO_POINT',
SimpleActionState( 'pid_global',
MoveAction,
makeMoveGoal("pid_global_plan", -3.0, 0, -0.5, radius_of_acceptance = 2.0)),
transitions = { "succeeded": 'INSPECT_POINT',
"preempted": 'INSPECT_POINT',
"aborted": 'INSPECT_POINT' })
StateMachine.add( 'INSPECT_POINT',
SimpleActionState( 'inspect_point',
MoveAction,
makeMoveGoal("inspect_point", -3.0, 0.0, -0.5, radius_of_acceptance=2.0)),
transitions = { 'succeeded': 'INSPECT_POINT',
"preempted": 'INSPECT_POINT',
"aborted": 'INSPECT_POINT' })
intro_server = IntrospectionServer(str(rospy.get_name()), hsm,'/SM_ROOT')
intro_server.start()
hsm.execute()
#patrol.execute()
print("State machine execute finished")
intro_server.stop()
def main():
rospy.init_node('mission_planner')
sm = smach.StateMachine(
outcomes=['mission_complete', 'mission_failed', 'aborted'])
with sm:
Sink(sm, 5, 'DEPTH')
#state detectGate()
depthTask = Depth(100, 'HEADING')
depthTask.addDepthAction(sm)
headingTask = Heading(200, 'DEPTH+HEADING')
headingTask.addHeadingAction(sm)
depthHeadingTask = DepthHeading(350, 350, 'mission_complete')
depthHeadingTask.addDepthHeading(sm)
sis = IntrospectionServer('ALPHEUS_MISSION_PLANNER', sm,
'/START_ALPHEUS')
sis.start()
outcome = sm.execute()
rospy.spin()
sis.stop()
def main():
from bender_skills import robot_factory
rospy.init_node("crowd_information")
robot = robot_factory.build(
[
"facial_features",
# "AI",
"person_detector",
# "knowledge",
"tts"
],
core=False)
robot.check()
sm = getInstance(robot, True)
ud = smach.UserData()
ud.features = ['gender', 'age']
# introspection server
sis = IntrospectionServer('crowd_information', sm, '/CROWD_SM')
sis.start()
outcome = sm.execute(ud)
sis.stop()
def main():
rospy.init_node('smach_example_hierarchical_state_machine')
# Create the top level SMACH state machine
sm_top = StateMachine(outcomes=['outcome5'])
# Open the container
with sm_top:
StateMachine.add('BAS', Bas(), transitions={'outcome3': 'SUB'})
# Create the sub SMACH state machine
sm_sub = StateMachine(outcomes=['outcome4'])
# Open the container
with sm_sub:
# Add state to the container
StateMachine.add('FOO',
Foo(),
transitions={
'outcome1': 'BAR',
'outcome2': 'outcome4'
})
StateMachine.add('BAR', Bar(), transitions={'outcome1': 'FOO'})
StateMachine.add('SUB', sm_sub, transitions={'outcome4': 'outcome5'})
sis = IntrospectionServer('example', sm_top, '/SM_STATEMACHINE')
sis.start()
# Execute SMACH plan
outcome = sm_top.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('smach_example_state_machine')
sm = StateMachine(['exit'])
with sm:
for key, (x, y, next_point) in WAYPOINTS.items():
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = x
goal.target_pose.pose.position.y = y
goal.target_pose.pose.orientation.w = 1.
StateMachine.add(key, SimpleActionState('move_base',
MoveBaseAction,
goal=goal),
transitions={'succeeded': next_point, 'aborted': 'exit', 'preempted': 'exit'})
# Create and start the introspection server
sis = IntrospectionServer('strat', sm, '/SM_ROOT')
sis.start()
# Execute the state machine
outcome = sm.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def main():
rospy.init_node('tinker_RIPS')
rospy.loginfo(colored('starting RIPS task ...', 'green'))
# load waypoints from xml
pose_list = TKpos.PoseList.parse(open(rospy.get_param('~waypoint_xml'), 'r'))
for pose in pose_list.pose:
WayPointGoalState.waypoint_dict[pose.name] = TKpos.Pose.X(pose)
# Main StateMachine
state = StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with state:
StateMachine.add('Start_Button', MonitorStartButtonState(),
transitions={'valid': 'Start_Button', 'invalid': 'Sequence'})
sequence_1 = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence_1:
Sequence.add('GoToWaypoin1', WayPointGoalState('waypoint1'), transitions={'aborted': 'GoToWaypoin1'})
Sequence.add('ArriveWaypoint1', SpeakState('Wait for continue'))
StateMachine.add('Sequence', sequence_1, {'succeeded': 'Continue', 'aborted': 'aborted'})
StateMachine.add('Continue', MonitorStartButtonState(),
transitions={'valid': 'Continue', 'invalid': 'Sequence_2'})
sequence_2 = Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
with sequence_2:
Sequence.add('GoToWaypoin2', WayPointGoalState('waypoint2'), transitions={'aborted': 'GoToWaypoin2'})
Sequence.add('ArriveWaypoint2', SpeakState('I have arrived at way point two'))
Sequence.add('GoToWaypoin3', WayPointGoalState('waypoint3'), transitions={'aborted': 'GoToWaypoin3'})
Sequence.add('ArriveWaypoint3', SpeakState('I have arrived at way point three'))
StateMachine.add('Sequence_2', sequence_2, {'succeeded': 'succeeded', 'aborted': 'aborted'})
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('tinker_mission_navigation', state, '/tinker_mission_navigation')
intro_server.start()
outcome = state.execute()
rospy.spin()
intro_server.stop()
def main():
rospy.init_node('smach_usecase_step_02')
# Create a SMACH state machine
sm0 = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Open the container
with sm0:
# Reset turtlesim
StateMachine.add('RESET', ServiceState('reset', std_srvs.srv.Empty),
{'succeeded': 'SPAWN'})
# Create a second turtle
StateMachine.add(
'SPAWN',
ServiceState('spawn',
turtlesim.srv.Spawn,
request=turtlesim.srv.SpawnRequest(
0.0, 0.0, 0.0, 'turtle2')))
# Attach a SMACH introspection server
sis = IntrospectionServer('smach_usecase_01', sm0, '/USE_CASE')
sis.start()
# Execute SMACH tree
outcome = sm0.execute()
# Signal ROS shutdown (kill threads in background)
rospy.spin()
sis.stop()
def main():
rospy.init_node('smach_example_state_machine')
init_robot_pose()
move_base_override.init()
reset_robot_actuators()
odom_sub = rospy.Subscriber('/odom', Odometry, odometry_cb)
sq = Sequence(outcomes=[Transitions.SUCCESS, Transitions.FAILURE],
connector_outcome=Transitions.SUCCESS)
with sq:
Sequence.add('waiting', WaitStartState())
for i in range(3):
Sequence.add('fishing {}'.format(i), create_fish_sequence())
# Sequence.add('inner_door', create_door_state_machine(0.3))
# Sequence.add('outer_door', create_door_state_machine(0.6))
# Create and start the introspection server
sis = IntrospectionServer('strat', sq, '/strat')
sis.start()
# Execute the state machine
outcome = sq.execute()
# Wait for ctrl-c to stop the application
rospy.spin()
sis.stop()
def main():
sm_iterator = construct_sm()
sis = IntrospectionServer('smach_viwer', sm_iterator, '/SM_ROOT')
sis.start()
outcome = sm_iterator.execute()
rospy.spin()
sis.stop()
def main():
smach_states = rospy.get_param("smach_states")
# The autonomy_sm handles the full autonomy sequence for the competition run
autonomy_sm = StateMachine(outcomes=['succeeded','aborted','preempted'])
with autonomy_sm:
# TODO: create localization state. might just be a MonitorState
# Sequentially add all the states from the config file to the state machine,
# where state i transitions to state i+1
names = []
for i in range(len(smach_states)):
state = smach_states[i]
name, action_state = create_action_state(state)
# If the state name is already in the state machine, add the new
# one with increasing numbers
if name in names:
name = name+"2"
while name in names:
name = name[:-1]+str(int(name[-1])+1) # increase num
names.append(name)
StateMachine.add_auto(name, action_state, connector_outcomes=['succeeded'])
# StateMachine.add()
# Create the concurrence contained for the fully autonomy sequence. This
# runs the state machine for the competition run. It also concurrently runs
# a state with a timer counting down from 10 minutes and a state that listens
# to the /click/start_button topic. If either of these are triggered, it will
# end autonomy and place us into the teleop state.
# TODO: add 10 minute competition timer state
autonomy_concurrence = Concurrence(outcomes=['enter_teleop', 'stay', 'aborted'],
default_outcome='enter_teleop',
child_termination_cb=autonomy_child_term_cb,
outcome_cb=autonomy_out_cb)
with autonomy_concurrence:
# state that runs full autonomy state machine
Concurrence.add('AUTONOMY', autonomy_sm)
# state that listens for toggle message
Concurrence.add('TOGGLE_LISTEN', MonitorState('/click/start_button', Empty, monitor_cb))
# Top level state machine, containing the autonomy and teleop machines.
top_sm = StateMachine(outcomes=['DONE'])
with top_sm:
StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb), transitions={'invalid':'AUTONOMY_MODE', 'valid':'TELEOP_MODE', 'preempted':'AUTONOMY_MODE'})
StateMachine.add('AUTONOMY_MODE', autonomy_concurrence,
transitions={'enter_teleop':'TELEOP_MODE', 'stay':'AUTONOMY_MODE', 'aborted':'DONE'})
#StateMachine.add('TELEOP_MODE', MonitorState('/click/start_button', Empty, monitor_cb),
# transitions={'invalid':'DONE', 'valid':'TELEOP_MODE', 'preempted':'DONE'})
sis = IntrospectionServer('smach_introspection_server', top_sm, '/COMPETITION_SMACH')
sis.start()
top_sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
rospy.init_node('random_patrol', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Set the userdata.waypoints variable to the pre-defined waypoints
self.sm_patrol.userdata.waypoints = self.waypoints
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
transitions={'succeeded':'NAV_WAYPOINT'},
remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'PICK_WAYPOINT',
'aborted':'PICK_WAYPOINT',
'preempted':'PICK_WAYPOINT'},
remapping={'waypoint_in':'patrol_waypoint'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_patrol.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('final_project_kl', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.keyPointManager = KeyPointManager()
# Create the nav_patrol state machine using a Concurrence container
self.nav_patrol = Concurrence(outcomes=['succeeded', 'key_points', 'stop'],
default_outcome='succeeded',
outcome_map = {'key_points' : {'MONITOR_AR':'invalid'}},
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a AR Tag MonitorState to the nav_patrol machine
with self.nav_patrol:
Concurrence.add('SM_NAV', Patrol().getSM())
Concurrence.add('MONITOR_AR',MonitorState('/ar_pose_marker', AlvarMarkers, self.ar_cb))
# Create the top level state machine
self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_top.userdata.sm_ar_tag = None
with self.sm_top:
StateMachine.add('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'key_points':'PATROL_KEYPOINTS', 'stop':'STOP'})
StateMachine.add('PATROL_KEYPOINTS', PatrolThroughKeyPoints(self.keyPointManager).getSM(), transitions={'succeeded':'STOP'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
# Create the top level SMACH state machine
sm_top = smach.StateMachine(outcomes=['success'])
sm_top.userdata.cup = 0
# Open the container
with sm_top:
smach.StateMachine.add('MOVEAROUND',
MoveAround(),
transitions={'done': 'MOVEAROUND'})
# Execute SMACH plan
outcome = sm_top.execute()
# Attach a SMACH introspection server
sis = IntrospectionServer('msj_move_around', sm_top, '/MOVEAROUND')
sis.start()
# Execute SMACH tree in a separate thread so that we can ctrl-c the script
smach_thread = threading.Thread(target=sm_top.execute)
smach_thread.start()
# Signal handler
rospy.spin()
def test_introspection(self):
"""Test introspection system."""
# Construct state machine
sm = StateMachine(['done'])
sm2 = StateMachine(['done'])
sm3 = StateMachine(['done'])
with sm:
# Note: the following "Getter" state should fail
StateMachine.add('GETTER1', Getter(), {})
StateMachine.add('SM2', sm2, {'done': 'SM3'})
with sm2:
StateMachine.add("SETTER", Setter(), {})
StateMachine.add('SM3', sm3, {'done': 'done'})
with sm3:
StateMachine.add("SETTER", Setter(), {})
StateMachine.add('GETTER2', Getter(), {'done': 'SM2'})
sm.set_initial_state(['GETTER1'])
sm2.set_initial_state(['SETTER'])
sm3.set_initial_state(['SETTER'])
# Run introspector
intro_server = IntrospectionServer('intro_test', sm, '/intro_test')
server_thread = threading.Thread(target=intro_server.start)
server_thread.start()
intro_client = IntrospectionClient()
servers = intro_client.get_servers()
while '/intro_test' not in servers and not rospy.is_shutdown():
servers = intro_client.get_servers()
rospy.loginfo("Smach servers: " + str())
rospy.sleep(0.1)
assert '/intro_test' in servers
# Set initial state
injected_ud = UserData()
injected_ud.a = 'A'
init_set = intro_client.set_initial_state('intro_test',
'/intro_test', ['SM2'],
injected_ud,
timeout=rospy.Duration(10.0))
assert init_set
outcome = sm.execute()
assert outcome == 'done'
def execute_sm(self):
'''
运行状态即机
'''
intro_server = IntrospectionServer('sm_fetchtea', self.sm_fetchtea, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_fetchtea.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
rospy.init_node("iterator_tutorial")
sm_iterator = construct_sm()
# Run state machine introspection server for smach viewer
intro_server = IntrospectionServer('iterator_tutorial', sm_iterator,
'/ITERATOR_TUTORIAL')
intro_server.start()
outcome = sm_iterator.execute()
rospy.spin()
intro_server.stop()
def main():
rospy.init_node('register_head_ellipse')
smer = SMEllipsoidRegistration()
sm = smer.get_sm()
rospy.sleep(1)
sis = IntrospectionServer('register_head', sm, '/UNFREEZE_PC')
sis.start()
outcome = sm.execute()
sis.stop()
def main():
rospy.init_node('pr2_touch_simple')
smts = SMTouchSimple()
sm = smts.get_sm_basic()
rospy.sleep(1)
sis = IntrospectionServer('touch_simple', sm, '/INPUT_START_CLICK')
sis.start()
outcome = sm.execute()
sis.stop()
def main():
rospy.init_node('smach_sm_touch_face')
smna = SMNavApproach()
sm = smna.get_sm()
rospy.sleep(1)
sis = IntrospectionServer('nav_prep', sm, '/SM_NAV_PREP')
sis.start()
outcome = sm.execute()
sis.stop()
def __init__(self):
rospy.init_node('patrol_smach', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# A list to hold then navigation waypoints
nav_states = list()
# Turn the waypoints into SMACH states
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(10.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
# Initialize the patrol state machine
self.sm_patrol = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Add the states to the state machine with the appropriate transitions
with self.sm_patrol:
StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1','preempted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2','preempted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3','preempted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4','preempted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'','aborted':'','preempted':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_patrol, '/SM_ROOT')
intro_server.start()
# Execute the state machine for the specified number of patrols
while (self.n_patrols == -1 or self.patrol_count < self.n_patrols) and not rospy.is_shutdown():
sm_outcome = self.sm_patrol.execute()
self.patrol_count += 1
rospy.loginfo("FINISHED PATROL LOOP: " + str(self.patrol_count))
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
rospy.init_node('bbauv_suavc_smach')
sauvc = smach.StateMachine(outcomes=['succeeded', 'preempted'])
with sauvc:
smach.StateMachine.add('COUNTDOWN_START',
Countdown(1.0),
transitions={'succeeded': 'SMACH_TRUE'})
smach.StateMachine.add('SMACH_TRUE',
SmachSwitch(True),
transitions={'succeeded': 'VISION_TRACK_TRUE'})
smach.StateMachine.add('VISION_TRACK_TRUE',
TrackerSwitch(False),
transitions={'succeeded': 'NAV_TRUE'})
smach.StateMachine.add('NAV_TRUE',
NavigationSwitch(True),
transitions={'succeeded': 'TOPSIDE_FALSE'})
smach.StateMachine.add('TOPSIDE_FALSE',
TopsideSwitch(False),
transitions={'succeeded': 'COUNTDOWN_MISSION'})
smach.StateMachine.add('COUNTDOWN_MISSION',
Countdown(180),
transitions={'succeeded': 'TOPSIDE_TRUE'})
smach.StateMachine.add('TOPSIDE_TRUE',
TopsideSwitch(True),
transitions={'succeeded': 'COUNTDOWN_END'})
smach.StateMachine.add('COUNTDOWN_END',
Countdown(0.1),
transitions={'succeeded': 'VISION_TRACK_FALSE'})
smach.StateMachine.add('VISION_TRACK_FALSE',
TrackerSwitch(False),
transitions={'succeeded': 'NAV_FALSE'})
smach.StateMachine.add('NAV_FALSE',
NavigationSwitch(False),
transitions={'succeeded': 'SMACH_FALSE'})
smach.StateMachine.add('SMACH_FALSE',
SmachSwitch(False),
transitions={'succeeded': 'succeeded'})
sis = IntrospectionServer('my_introspserver', sauvc, '/SM_ROOT')
sis.start()
outcome = sauvc.execute()
rospy.on_shutdown(shutdown)
rospy.spin()
sis.stop()
def __init__(self):
rospy.init_node('final_project_kl', anonymous=False)
rospy.on_shutdown(self.shutdown)
self.keyPointManager = KeyPointManager()
# Create the nav_patrol state machine using a Concurrence container
self.nav_patrol = Concurrence(
outcomes=['succeeded', 'key_points', 'stop'],
default_outcome='succeeded',
outcome_map={'key_points': {
'MONITOR_AR': 'invalid'
}},
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a AR Tag MonitorState to the nav_patrol machine
with self.nav_patrol:
Concurrence.add('SM_NAV', Patrol().getSM())
Concurrence.add(
'MONITOR_AR',
MonitorState('/ar_pose_marker', AlvarMarkers, self.ar_cb))
# Create the top level state machine
self.sm_top = StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'])
self.sm_top.userdata.sm_ar_tag = None
with self.sm_top:
StateMachine.add('PATROL',
self.nav_patrol,
transitions={
'succeeded': 'PATROL',
'key_points': 'PATROL_KEYPOINTS',
'stop': 'STOP'
})
StateMachine.add('PATROL_KEYPOINTS',
PatrolThroughKeyPoints(
self.keyPointManager).getSM(),
transitions={'succeeded': 'STOP'})
StateMachine.add('STOP', Stop(), transitions={'succeeded': ''})
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def main():
rospy.init_node('smach_sm_touch_face')
if sys.argv[1] == 'nav_only':
smtf = SMTouchFace()
sm = smtf.get_nav_prep_sm()
rospy.sleep(1)
sis = IntrospectionServer('nav_prep', sm, '/SM_NAV_PREP')
sis.start()
outcome = sm.execute()
sis.stop()
elif sys.argv[1] == 'full_demo':
smtf = SMTouchFace()
sm_nav = smtf.get_nav_prep_sm()
sm_touch = smtf.get_touch_sm()
sm_full = smach.StateMachine(outcomes=['succeeded','preempted','shutdown', 'aborted'])
with sm_full:
smach.StateMachine.add(
'SM_NAV_PREP',
sm_nav,
transitions={'succeeded' : 'SM_TOUCH_FACE'})
smach.StateMachine.add(
'SM_TOUCH_FACE',
sm_touch)
rospy.sleep(1)
sis = IntrospectionServer('nav_prep', sm_full, '/SM_TOUCH_FACE_FULL')
sis.start()
outcome = sm_full.execute()
sis.stop()
elif sys.argv[1] == 'touch_only':
smtf = SMTouchFace()
sm = smtf.get_touch_sm()
rospy.sleep(1)
sis = IntrospectionServer('touch_face', sm, '/SM_TOUCH_FACE')
sis.start()
outcome = sm.execute()
sis.stop()
def __init__(self):
rospy.init_node('deliver_food', anonymous=False)
self.initialize_destination()
self.lalala = 100
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
nav_states = {}
for room in self.room_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.room_locations[room]
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(60.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states[room] = move_base_state
rospy.loginfo(room + " -> [" + str(round(self.room_locations[room].position.x,2)) + ", " + str(round(self.room_locations[room].position.y,2)) + "]" )
sm_rotate_search = Concurrence(outcomes=['find', 'not_find'],
default_outcome='not_find',
child_termination_cb=self.concurrence_child_termination_callback,
outcome_cb=self.concurrence_outcome_callback)
with sm_rotate_search:
Concurrence.add('ROTATE', Rotate360() )
Concurrence.add('SEARCH', SearchTable() )
sm_table1 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table1:
StateMachine.add('GOTO_TABLE1', nav_states['table1'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table2 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table2:
StateMachine.add('GOTO_TABLE2', nav_states['table2'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table3 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table3:
StateMachine.add('GOTO_TABLE3', nav_states['table3'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table4 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table4:
StateMachine.add('GOTO_TABLE4', nav_states['table4'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table5 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table5:
StateMachine.add('GOTO_TABLE5', nav_states['table5'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
sm_table6 = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_table6:
StateMachine.add('GOTO_TABLE6', nav_states['table6'], transitions={'succeeded':'ROTATE_SEARCH',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('ROTATE_SEARCH', sm_rotate_search, transitions={'find':'DO_STUFFS',
'not_find':'GOTO_KITCHEN'})
StateMachine.add('DO_STUFFS', DoStuffs("testing",5), transitions={'succeeded':'GOTO_KITCHEN',
'aborted':'GOTO_KITCHEN',
'preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'',
'aborted':'',
'preempted':''})
# let's initialize the overall state machine
sm_deliverfood = StateMachine(outcomes=['succeeded','aborted','preempted'])
with sm_deliverfood:
StateMachine.add('COMPUTER_VISION_TASK', ComputerVision(), transitions={'detect1':'TABLE_ONE_TASK',
'detect2':'TABLE_TWO_TASK',
'detect3':'TABLE_THREE_TASK',
'detect4':'TABLE_FOUR_TASK',
'detect5':'TABLE_FIVE_TASK',
'detect6':'TABLE_SIX_TASK'})
StateMachine.add('TABLE_ONE_TASK',sm_table1, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_TWO_TASK',sm_table2, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_THREE_TASK',sm_table3, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_FOUR_TASK',sm_table4, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_FIVE_TASK',sm_table5, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('TABLE_SIX_TASK',sm_table6, transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
StateMachine.add('GOTO_KITCHEN', nav_states['kitchen'], transitions={'succeeded':'COMPUTER_VISION_TASK','aborted':'GOTO_KITCHEN','preempted':'GOTO_KITCHEN'})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('deliver_food', sm_deliverfood, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_deliverfood.execute()
rospy.on_shutdown(self.shutdown)
def __init__(self):
rospy.init_node('clean_house', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Turn the room locations into SMACH move_base action states
nav_states = {}
for room in self.room_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.room_locations[room]
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(15.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states[room] = move_base_state
''' Create individual state machines for assigning tasks to each room '''
# Create a state machine for the living room subtask(s)
sm_living_room = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Then add the subtask(s)
with sm_living_room:
StateMachine.add('VACUUM_FLOOR', VacuumFloor('living_room', 5), transitions={'succeeded':'','aborted':'','preempted':''})
# Create a state machine for the kitchen subtask(s)
sm_kitchen = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Then add the subtask(s)
with sm_kitchen:
StateMachine.add('MOP_FLOOR', MopFloor('kitchen', 5), transitions={'succeeded':'','aborted':'','preempted':''})
# Create a state machine for the bathroom subtask(s)
sm_bathroom = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Then add the subtasks
with sm_bathroom:
StateMachine.add('SCRUB_TUB', ScrubTub('bathroom', 7), transitions={'succeeded':'MOP_FLOOR'})
StateMachine.add('MOP_FLOOR', MopFloor('bathroom', 5), transitions={'succeeded':'','aborted':'','preempted':''})
# Create a state machine for the hallway subtask(s)
sm_hallway = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Then add the subtasks
with sm_hallway:
StateMachine.add('VACUUM_FLOOR', VacuumFloor('hallway', 5), transitions={'succeeded':'','aborted':'','preempted':''})
# Initialize the overall state machine
sm_clean_house = StateMachine(outcomes=['succeeded','aborted','preempted'])
# Build the clean house state machine from the nav states and room cleaning states
with sm_clean_house:
StateMachine.add('START', nav_states['hallway'], transitions={'succeeded':'LIVING_ROOM','aborted':'LIVING_ROOM','preempted':'LIVING_ROOM'})
''' Add the living room subtask(s) '''
StateMachine.add('LIVING_ROOM', nav_states['living_room'], transitions={'succeeded':'LIVING_ROOM_TASKS','aborted':'KITCHEN','preempted':'KITCHEN'})
# When the tasks are done, continue on to the kitchen
StateMachine.add('LIVING_ROOM_TASKS', sm_living_room, transitions={'succeeded':'KITCHEN','aborted':'KITCHEN','preempted':'KITCHEN'})
''' Add the kitchen subtask(s) '''
StateMachine.add('KITCHEN', nav_states['kitchen'], transitions={'succeeded':'KITCHEN_TASKS','aborted':'BATHROOM','preempted':'BATHROOM'})
# When the tasks are done, continue on to the bathroom
StateMachine.add('KITCHEN_TASKS', sm_kitchen, transitions={'succeeded':'BATHROOM','aborted':'BATHROOM','preempted':'BATHROOM'})
''' Add the bathroom subtask(s) '''
StateMachine.add('BATHROOM', nav_states['bathroom'], transitions={'succeeded':'BATHROOM_TASKS','aborted':'HALLWAY','preempted':'HALLWAY'})
# When the tasks are done, return to the hallway
StateMachine.add('BATHROOM_TASKS', sm_bathroom, transitions={'succeeded':'HALLWAY','aborted':'HALLWAY','preempted':'HALLWAY'})
''' Add the hallway subtask(s) '''
StateMachine.add('HALLWAY', nav_states['hallway'], transitions={'succeeded':'HALLWAY_TASKS','aborted':'','preempted':''})
# When the tasks are done, stop
StateMachine.add('HALLWAY_TASKS', sm_hallway, transitions={'succeeded':'','aborted':'','preempted':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('clean_house', sm_clean_house, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = sm_clean_house.execute()
if len(task_list) > 0:
message = "Ooops! Not all chores were completed."
message += "The following rooms need to be revisited: "
message += str(task_list)
else:
message = "All chores complete!"
rospy.loginfo(message)
easygui.msgbox(message, title="Finished Cleaning")
intro_server.stop()
def __init__(self):
rospy.init_node('patrol_smach_concurrence', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Initialize a number of parameters and variables
setup_task_environment(self)
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
# A variable to hold the last/current navigation goal
self.last_nav_state = None
# A flag to indicate whether or not we are rechargin
self.recharging = False
# A list to hold then navigation goa
nav_states = list()
# Turn the waypoints into SMACH states
for waypoint in self.waypoints:
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = waypoint
move_base_state = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(40.0),
server_wait_timeout=rospy.Duration(10.0))
nav_states.append(move_base_state)
# Create a MoveBaseAction state for the docking station
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = 'map'
nav_goal.target_pose.pose = self.docking_station_pose
nav_docking_station = SimpleActionState('move_base', MoveBaseAction, goal=nav_goal, result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(40.0),
server_wait_timeout=rospy.Duration(10.0))
# Initialize the navigation state machine
self.sm_nav = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Add the nav states to the state machine with the appropriate transitions
with self.sm_nav:
StateMachine.add('NAV_STATE_0', nav_states[0], transitions={'succeeded':'NAV_STATE_1','aborted':'NAV_STATE_1'})
StateMachine.add('NAV_STATE_1', nav_states[1], transitions={'succeeded':'NAV_STATE_2','aborted':'NAV_STATE_2'})
StateMachine.add('NAV_STATE_2', nav_states[2], transitions={'succeeded':'NAV_STATE_3','aborted':'NAV_STATE_3'})
StateMachine.add('NAV_STATE_3', nav_states[3], transitions={'succeeded':'NAV_STATE_4','aborted':'NAV_STATE_4'})
StateMachine.add('NAV_STATE_4', nav_states[0], transitions={'succeeded':'','aborted':''})
# Register a callback function to fire on state transitions within the sm_nav state machine
self.sm_nav.register_transition_cb(self.nav_transition_cb, cb_args=[])
# Initialize the recharge state machine
self.sm_recharge = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with self.sm_recharge:
StateMachine.add('NAV_DOCKING_STATION', nav_docking_station, transitions={'succeeded':''})
#StateMachine.add('RECHARGE_BATTERY', ServiceState('battery_simulator/set_battery_level', SetBatteryLevel, 100, response_cb=self.recharge_cb),
#transitions={'succeeded':''})
# Create the nav_patrol state machine using a Concurrence container
self.nav_patrol = Concurrence(outcomes=['succeeded', 'recharge', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_nav machine and a battery MonitorState to the nav_patrol machine
with self.nav_patrol:
Concurrence.add('SM_NAV', self.sm_nav)
Concurrence.add('MONITOR_BATTERY', MonitorState("battery_level", Float32, self.battery_cb))
# Create the top level state machine
self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('PATROL', self.nav_patrol, transitions={'succeeded':'PATROL', 'recharge':'RECHARGE', 'stop':'STOP'})
StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('smach_home_status', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
#####################################
# JO IS AWAKE
#####################################
# State machine for Jo-awake-go-sleep
self.sm_jo_awake_sleep = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_awake_sleep:
StateMachine.add('LOOK_WAKE', MonitorState("/JO/sleep", Empty, self.empty_cb), transitions={'valid':'GOING_SLEEP', 'preempted':'preempted', 'invalid':'GOING_SLEEP'})
StateMachine.add('GOING_SLEEP', JoGoingSleep(),
transitions={'succeeded':'LOOK_IN_BED'})
StateMachine.add('LOOK_IN_BED', MonitorState("/myo_disconnected", Empty, self.empty_cb), transitions={'valid':'IN_BED', 'preempted':'preempted', 'invalid':'IN_BED'})
StateMachine.add('IN_BED', JoInBed(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-awake-bothgo-sleep
self.sm_jo_awake_bothsleep = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_awake_bothsleep:
StateMachine.add('LOOK_WAKE', MonitorState("/BOTH/sleep", Empty, self.empty_cb), transitions={'valid':'GOING_SLEEP', 'preempted':'preempted', 'invalid':'GOING_SLEEP'})
StateMachine.add('GOING_SLEEP', BothGoingSleep(),
transitions={'succeeded':'LOOK_IN_BED'})
StateMachine.add('LOOK_IN_BED', MonitorState("/myo_disconnected", Empty, self.empty_cb), transitions={'valid':'IN_BED', 'preempted':'preempted', 'invalid':'IN_BED'})
StateMachine.add('IN_BED', BothInBed(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-awake-go-out
self.sm_jo_awake_out = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_awake_out:
StateMachine.add('LOOK_OUT', MonitorState("/JO/go_out", Empty, self.empty_cb), transitions={'valid':'PAUSE', 'preempted':'preempted', 'invalid':'PAUSE'})
StateMachine.add('PAUSE', Pause(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-awake
self.sm_jo_awake = Concurrence(outcomes=['succeeded', 'stop', 'go_sleep', 'go_out'],
default_outcome='succeeded',
child_termination_cb=self.jo_awake_child_termination_cb,
outcome_cb=self.jo_awake_outcome_cb)
with self.sm_jo_awake:
Concurrence.add('SM_GO_TO_SLEEP', self.sm_jo_awake_sleep)
Concurrence.add('SM_BOTH_GO_TO_SLEEP', self.sm_jo_awake_bothsleep)
Concurrence.add('SM_GO_OUT', self.sm_jo_awake_out)
#####################################
# JO IS SLEEPING
#####################################
# State machine for Jo-sleep-waking
self.sm_jo_sleep_waking = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_sleep_waking:
StateMachine.add('WAIT_WAKE_UP', MonitorState("/JO/wake_up", Empty, self.empty_cb), transitions={'valid':'WAKING_UP', 'preempted':'preempted', 'invalid':'WAKING_UP'})
StateMachine.add('WAKING_UP', JoWakingUp(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-sleep-bothwaking
self.sm_jo_sleep_bothwaking = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_sleep_bothwaking:
StateMachine.add('WAIT_WAKE_UP', MonitorState("/BOTH/wake_up", Empty, self.empty_cb), transitions={'valid':'WAKING_UP', 'preempted':'preempted', 'invalid':'WAKING_UP'})
StateMachine.add('WAKING_UP', BothWakingUp(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-awake
self.sm_jo_sleep = Concurrence(outcomes=['succeeded','aborted','preempted', 'wake_up'],
default_outcome='succeeded',
child_termination_cb=self.jo_sleep_child_termination_cb,
outcome_cb=self.jo_sleep_outcome_cb)
with self.sm_jo_sleep:
Concurrence.add('SM_WAKE_UP', self.sm_jo_sleep_waking)
Concurrence.add('SM_BOTH_WAKE_UP', self.sm_jo_sleep_bothwaking)
#####################################
# JO IS OUT TODO
#####################################
# State machine for Jo-out-back
self.sm_jo_out_back = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_out_back:
StateMachine.add('WAIT_BACK_HOME', MonitorState("/JO/back_home", Empty, self.empty_cb), transitions={'valid':'WAIT_MYO', 'preempted':'preempted', 'invalid':'WAIT_MYO'})
StateMachine.add('WAIT_MYO', MonitorState("/myo_connected", Empty, self.empty_cb), transitions={'valid':'COMING_BACK', 'preempted':'preempted', 'invalid':'COMING_BACK'})
StateMachine.add('COMING_BACK', JoBackHome(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-out-bothback
self.sm_jo_out_bothback = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo_out_bothback:
StateMachine.add('WAIT_BACK_HOME', MonitorState("/BOTH/back_home", Empty, self.empty_cb), transitions={'valid':'WAIT_MYO', 'preempted':'preempted', 'invalid':'WAIT_MYO'})
StateMachine.add('WAIT_MYO', MonitorState("/myo_connected", Empty, self.empty_cb), transitions={'valid':'COMING_BACK', 'preempted':'preempted', 'invalid':'COMING_BACK'})
StateMachine.add('COMING_BACK', BothBackHome(),
transitions={'succeeded':'succeeded'})
# State machine for Jo-out
self.sm_jo_out = Concurrence(outcomes=['succeeded','aborted','preempted', 'back_home'],
default_outcome='succeeded',
child_termination_cb=self.jo_out_child_termination_cb,
outcome_cb=self.jo_out_outcome_cb)
with self.sm_jo_out:
Concurrence.add('SM_BACK_HOME', self.sm_jo_out_back)
Concurrence.add('SM_BOTH_BACK_HOME', self.sm_jo_out_bothback)
#####################################
# TOP LVL JO SM
#####################################
# State machine for JO
self.sm_jo = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_jo:
StateMachine.add('AWAKE', self.sm_jo_awake, transitions={'succeeded':'succeeded', 'stop':'aborted', 'go_sleep':'SLEEP', 'go_out':'OUT'})
StateMachine.add('SLEEP', self.sm_jo_sleep, transitions={'succeeded':'succeeded', 'wake_up':'AWAKE'})
StateMachine.add('OUT', self.sm_jo_out, transitions={'succeeded':'succeeded', 'back_home':'AWAKE'})
#####################################
# TOP LVL CAROLE SM TODO
#####################################
# State machine for CAROLE
self.sm_carole = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_carole:
StateMachine.add('WAIT3', MonitorState("/TEST/wait3", Empty, self.empty_cb), transitions={'valid':'PAUSE', 'preempted':'preempted', 'invalid':'PAUSE'})
StateMachine.add('PAUSE', Pause(),
transitions={'succeeded':'WAIT3',
'aborted':'aborted'})
#####################################
# TOP LVL EAT SM TODO
#####################################
# State machine for EAT
self.sm_eat = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_eat:
StateMachine.add('WAIT2', MonitorState("/TEST/wait2", Empty, self.empty_cb), transitions={'valid':'PAUSE', 'preempted':'preempted', 'invalid':'PAUSE'})
StateMachine.add('PAUSE', Pause(),
transitions={'succeeded':'WAIT2',
'aborted':'aborted'})
#####################################
# TOP LVL SHOWER SM
#####################################
# State machine for SHOWER
self.sm_shower = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_shower:
StateMachine.add('WAIT_SHOWER', MonitorState("/HOME/go_shower", Empty, self.empty_cb), transitions={'valid':'PREPARING_SHOWER', 'preempted':'preempted', 'invalid':'PREPARING_SHOWER'})
StateMachine.add('PREPARING_SHOWER', PreparingShower(),
transitions={'succeeded':'GO_SHOWER',
'aborted':'WAIT1'})
StateMachine.add('GO_SHOWER', GoShower(),
transitions={'succeeded':'STOP_SHOWER',
'aborted':'aborted'})
StateMachine.add('STOP_SHOWER', StopShower(),
transitions={'succeeded':'WAIT1',
'aborted':'aborted'})
#####################################
# TOP LVL SM
#####################################
# Create the top level state machine
self.sm_top = Concurrence(outcomes=['succeeded', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
Concurrence.add('SM_JO', self.sm_jo)
Concurrence.add('SM_CAROLE', self.sm_carole)
Concurrence.add('SM_EAT', self.sm_eat)
Concurrence.add('SM_SHOWER', self.sm_shower)
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('petit_smach_ai', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (pi/2, pi, 3*pi/2, 0)
# Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
self.waypoints = list()
self.square_size = 1.0
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
self.waypoints.append(Pose(Point(0.0, 0.0, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(self.square_size, 0.0, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(self.square_size, self.square_size, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(0.0, self.square_size, 0.0), quaternions[2]))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)
self.stopping = False
self.recharging = False
self.robot_side = 1
# State machine for Action1
self.sm_action1 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action1.userdata.mandibles_sleep = 0.1
with self.sm_action1:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'UPDATE_DROPCUBE_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPCUBE_OBJ', UpdateObjectiveDropCubes(),
# transitions={'succeeded':'CLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPCUBE_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioCube_cb, response_cb=self.updatePrioCube_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'HCLOSE_MANDIBLES',
'aborted':'HCLOSE_MANDIBLES'},
remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action2
self.sm_action2 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action2.userdata.speed = -0.1;
self.sm_action2.userdata.distance = 0.3;
self.sm_action2.userdata.mandibles_sleep = 0.1
with self.sm_action2:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'OPEN_MANDIBLES',
'aborted':'aborted'})
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'MOVE_BACKWARD',
'aborted':'aborted'})
StateMachine.add('MOVE_BACKWARD', MoveForward(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action3
self.sm_action3 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action3.userdata.mandibles_sleep = 0.1
self.sm_action3.userdata.speed = -0.1;
self.sm_action3.userdata.distance = 0.2;
with self.sm_action3:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'MOVE_PUSH',
'aborted':'aborted'})
StateMachine.add('MOVE_PUSH', MovePush(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action4
self.sm_action4 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action4.userdata.mandibles_sleep = 0.1
self.sm_action4.userdata.speed_x = 0.1
self.sm_action4.userdata.speed_y = 0.1
self.sm_action4.userdata.distance = 0.5;
with self.sm_action4:
StateMachine.add('CLOSE_MANDIBLES', CloseMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'SLIDE',
'aborted':'aborted'})
StateMachine.add('SLIDE', Slide(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action5
self.sm_action5 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action5.userdata.mandibles_sleep = 0.1
with self.sm_action5:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'UPDATE_DROPSHELL_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(),
# transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'ALMOSTCLOSE_MANDIBLES',
'aborted':'ALMOSTCLOSE_MANDIBLES'})
StateMachine.add('ALMOSTCLOSE_MANDIBLES', AlmostCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action6
self.sm_action6 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action6.userdata.mandibles_sleep = 0.1
self.sm_action6.userdata.speed = 0.1;
self.sm_action6.userdata.distance = 0.2;
with self.sm_action6:
StateMachine.add('OPEN_MANDIBLES', OpenMandibles(),
transitions={'succeeded':'NAV_WAYPOINT',
'aborted':'aborted'})
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'FORWARD',
'aborted':'aborted'})
StateMachine.add('FORWARD', MoveForward(),
transitions={'succeeded':'UPDATE_DROPSHELL_OBJ',
'aborted':'aborted'})
# StateMachine.add('UPDATE_DROPSHELL_OBJ', UpdateObjectiveDropShell(),
# transitions={'succeeded':'HCLOSE_MANDIBLES',
# 'aborted':'aborted'})
StateMachine.add('UPDATE_DROPSHELL_OBJ', ServiceState('/PETIT/update_priority', UpdatePriority, request_cb=self.requestPrioShell_cb, response_cb=self.updatePrioShell_cb,
output_keys=['waypoint_out'],
input_keys=['robot_side']),
transitions={'succeeded':'HCLOSE_MANDIBLES',
'aborted':'HCLOSE_MANDIBLES'})
StateMachine.add('HCLOSE_MANDIBLES', HalfCloseMandibles(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Action7
self.sm_action7 = StateMachine(outcomes=['succeeded','aborted','preempted'], input_keys=['waypoint_in'], output_keys=['waypoint_out'])
self.sm_action7.userdata.mandibles_sleep = 0.1
with self.sm_action7:
StateMachine.add('NAV_WAYPOINT', Nav2Waypoint(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'})
# State machine for Actions
self.sm_actions = StateMachine(outcomes=['succeeded','aborted','preempted'])
self.sm_actions.userdata.waypoints = self.waypoints
with self.sm_actions:
StateMachine.add('PICK_WAYPOINT', ServiceState('/PETIT/get_objective', GetObjective, response_cb=self.objective_cb,
output_keys=['waypoint_out'],
outcomes=['action1','action2','action3','action4','action5','action6','action7','aborted','succeeded','preempted']),
transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','action7':'SM_ACTION7','aborted':'SM_ACTION1'},
remapping={'waypoint_out':'patrol_waypoint'})
#StateMachine.add('PICK_WAYPOINT', PickWaypoint(),
# transitions={'action1':'SM_ACTION1','action2':'SM_ACTION2','action3':'SM_ACTION3','action4':'SM_ACTION4','action5':'SM_ACTION5','action6':'SM_ACTION6','aborted':'SM_ACTION1'},
# remapping={'waypoint_out':'patrol_waypoint'})
StateMachine.add('SM_ACTION1', self.sm_action1, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION2', self.sm_action2, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION3', self.sm_action3, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION4', self.sm_action4, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION5', self.sm_action5, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION6', self.sm_action6, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('SM_ACTION7', self.sm_action7, transitions={'succeeded':'REMOVE_OBJECTIVE', 'aborted':'aborted'},
remapping={'waypoint_in':'patrol_waypoint',
'waypoint_out':'remove_waypoint'})
StateMachine.add('REMOVE_OBJECTIVE', RemoveObjective(),
transitions={'succeeded':'succeeded',
'aborted':'aborted'},
remapping={'waypoint_in':'remove_waypoint'})
# State machine with concurrence
self.sm_concurrent = Concurrence(outcomes=['succeeded', 'stop'],
default_outcome='succeeded',
child_termination_cb=self.concurrence_child_termination_cb,
outcome_cb=self.concurrence_outcome_cb)
# Add the sm_actions machine and a battery MonitorState to the nav_patrol machine
with self.sm_concurrent:
Concurrence.add('SM_ACTIONS', self.sm_actions)
Concurrence.add('MONITOR_TIME', MonitorState("/GENERAL/remain", Int32, self.time_cb))
Concurrence.add('MONITOR_BATTERY', MonitorState("/PETIT/adc", Int32, self.battery_cb))
# Create the top level state machine
self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
# @smach.cb_interface()
# def requestPrioCube_cb(userdata, request):
# update_request = UpdatePriority().Request
# update_request.goal.pose.position.x = userdata.robot_side*(1.500 - 0.300)
# update_request.goal.pose.position.y = 0.800
# update_request.prio = 100
# return update_request
StateMachine.add('WAIT_COLOR', MonitorState("/GENERAL/color", Int32, self.color_cb), transitions={'valid':'WAIT_START', 'preempted':'WAIT_START', 'invalid':'WAIT_START'})
StateMachine.add('WAIT_START', MonitorState("/GENERAL/start", Empty, self.start_cb), transitions={'valid':'CONCURRENT', 'preempted':'CONCURRENT', 'invalid':'CONCURRENT'})
StateMachine.add('CONCURRENT', self.sm_concurrent, transitions={'succeeded':'CONCURRENT', 'stop':'STOP'})
#StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def __init__(self):
rospy.init_node('petit_smach_ai', anonymous=False)
# Set the shutdown function (stop the robot)
rospy.on_shutdown(self.shutdown)
# Create a list to hold the target quaternions (orientations)
quaternions = list()
# First define the corner orientations as Euler angles
euler_angles = (0, pi, pi/2, -pi/2, pi/4, 0)
# Then convert the angles to quaternions
for angle in euler_angles:
q_angle = quaternion_from_euler(0, 0, angle, axes='sxyz')
q = Quaternion(*q_angle)
quaternions.append(q)
# Create a list to hold the waypoint poses
self.waypoints = list()
self.square_size = 1.0
# Append each of the four waypoints to the list. Each waypoint
# is a pose consisting of a position and orientation in the map frame.
self.waypoints.append(Pose(Point(-0.4, 0.65, 0.0), quaternions[0]))
self.waypoints.append(Pose(Point(-1.0, 0.50, 0.0), quaternions[1]))
self.waypoints.append(Pose(Point(-0.4, 0.35, 0.0), quaternions[2]))
self.waypoints.append(Pose(Point(-0.8, 0.65, 0.0), quaternions[3]))
self.waypoints.append(Pose(Point(-1.2, 0.30, 0.0), quaternions[4]))
self.waypoints.append(Pose(Point(-0.4, 0.657, 0.0), quaternions[5]))
# Publisher to manually control the robot (e.g. to stop it)
self.cmd_vel_pub = rospy.Publisher('/PETIT/cmd_vel', Twist)
self.stopping = False
self.recharging = False
self.robot_side = 1
# State machine for Actions
self.sm_actions = StateMachine(outcomes=['succeeded','aborted','preempted'])
with self.sm_actions:
# Calib X
StateMachine.add('CALIBRATE_X', CalibX(),
transitions={'succeeded':'CALIBRATE_Y',
'aborted':'aborted'})
# Calib Y
StateMachine.add('CALIBRATE_Y', CalibY(),
transitions={'succeeded':'MAX_SPEED_1',
'aborted':'aborted'})
# PICK-UP
# Change speed
StateMachine.add('MAX_SPEED_1', MaxLinearSpeed(70000),
transitions={'succeeded':'GOTO_1_1',
'aborted':'aborted'})
# NavPoint 1
StateMachine.add('GOTO_1_1', Nav2Waypoint(self.waypoints[0]),
transitions={'succeeded':'MAX_SPEED_2',
'aborted':'aborted'})
# Change speed
StateMachine.add('MAX_SPEED_2', MaxLinearSpeed(40000),
transitions={'succeeded':'PAUSE_1',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_1', Pause(1.0),
transitions={'succeeded':'FORWARD_1',
'aborted':'aborted'})
# Avancer
StateMachine.add('FORWARD_1', MoveForward(0.15),
transitions={'succeeded':'FORKS_10',
'aborted':'aborted'})
# Monter fourches
StateMachine.add('FORKS_10', Forks(90),
transitions={'succeeded':'FORKS_11',
'aborted':'aborted'})
StateMachine.add('FORKS_11', Forks(87),
transitions={'succeeded':'FORKS_12',
'aborted':'aborted'})
StateMachine.add('FORKS_12', Forks(83),
transitions={'succeeded':'BACKWARD_1',
'aborted':'aborted'})
# Reculer
StateMachine.add('BACKWARD_1', MoveForward(-0.15),
transitions={'succeeded':'ROTATE_1',
'aborted':'aborted'})
# Rotation
StateMachine.add('ROTATE_1', MoveRotate(pi),
transitions={'succeeded':'MAX_SPEED_3',
'aborted':'aborted'})
# DROP-OFF
# Change speed
StateMachine.add('MAX_SPEED_3', MaxLinearSpeed(70000),
transitions={'succeeded':'GOTO_2_1',
'aborted':'aborted'})
# NavPoint 2
StateMachine.add('GOTO_2_1', Nav2Waypoint(self.waypoints[1]),
transitions={'succeeded':'MAX_SPEED_4',
'aborted':'aborted'})
# Change speed
StateMachine.add('MAX_SPEED_4', MaxLinearSpeed(40000),
transitions={'succeeded':'PAUSE_2',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_2', Pause(1.0),
transitions={'succeeded':'FORWARD_2',
'aborted':'aborted'})
# Avancer
StateMachine.add('FORWARD_2', MoveForward(0.13),
transitions={'succeeded':'FORKS_20',
'aborted':'aborted'})
# Baisser fourches
StateMachine.add('FORKS_20', Forks(87),
transitions={'succeeded':'FORKS_21',
'aborted':'aborted'})
StateMachine.add('FORKS_21', Forks(91),
transitions={'succeeded':'FORKS_22',
'aborted':'aborted'})
StateMachine.add('FORKS_22', Forks(95),
transitions={'succeeded':'BACKWARD_2',
'aborted':'aborted'})
# Reculer
StateMachine.add('BACKWARD_2', MoveForward(-0.15),
transitions={'succeeded':'ROTATE_2',
'aborted':'aborted'})
# Rotation
StateMachine.add('ROTATE_2', MoveRotate(0),
transitions={'succeeded':'GOTO_3_1',
'aborted':'aborted'})
# NavPoint 3
StateMachine.add('GOTO_3_1', Nav2Waypoint(self.waypoints[2]),
transitions={'succeeded':'PAUSE_3',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_3', Pause(2.0),
transitions={'succeeded':'GOTO_4_1',
'aborted':'aborted'})
# NavPoint 4
StateMachine.add('GOTO_4_1', Nav2Waypoint(self.waypoints[3]),
transitions={'succeeded':'PAUSE_4',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_4', Pause(2.0),
transitions={'succeeded':'GOTO_2_2',
'aborted':'aborted'})
# PICK-UP
# NavPoint 2
StateMachine.add('GOTO_2_2', Nav2Waypoint(self.waypoints[1]),
transitions={'succeeded':'PAUSE_5',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_5', Pause(1.0),
transitions={'succeeded':'FORWARD_3',
'aborted':'aborted'})
# Avancer
StateMachine.add('FORWARD_3', MoveForward(0.15),
transitions={'succeeded':'FORKS_30',
'aborted':'aborted'})
# Monter fourches
StateMachine.add('FORKS_30', Forks(90),
transitions={'succeeded':'FORKS_31',
'aborted':'aborted'})
StateMachine.add('FORKS_31', Forks(87),
transitions={'succeeded':'FORKS_32',
'aborted':'aborted'})
StateMachine.add('FORKS_32', Forks(83),
transitions={'succeeded':'BACKWARD_3',
'aborted':'aborted'})
# Reculer
StateMachine.add('BACKWARD_3', MoveForward(-0.15),
transitions={'succeeded':'ROTATE_3',
'aborted':'aborted'})
# Rotation
StateMachine.add('ROTATE_3', MoveRotate(0),
transitions={'succeeded':'GOTO_1_2',
'aborted':'aborted'})
# DROP-OFF
# NavPoint 1
StateMachine.add('GOTO_1_2', Nav2Waypoint(self.waypoints[5]),
transitions={'succeeded':'PAUSE_6',
'aborted':'aborted'})
# Pause
StateMachine.add('PAUSE_6', Pause(1.0),
transitions={'succeeded':'FORWARD_4',
'aborted':'aborted'})
# Avancer
StateMachine.add('FORWARD_4', MoveForward(0.13),
transitions={'succeeded':'FORKS_40',
'aborted':'aborted'})
# Baisser fourches
StateMachine.add('FORKS_40', Forks(87),
transitions={'succeeded':'FORKS_41',
'aborted':'aborted'})
StateMachine.add('FORKS_41', Forks(91),
transitions={'succeeded':'FORKS_42',
'aborted':'aborted'})
StateMachine.add('FORKS_42', Forks(95),
transitions={'succeeded':'BACKWARD_4',
'aborted':'aborted'})
# Reculer
StateMachine.add('BACKWARD_4', MoveForward(-0.15),
transitions={'succeeded':'ROTATE_4',
'aborted':'aborted'})
# Rotation
StateMachine.add('ROTATE_4', MoveRotate(pi),
transitions={'succeeded':'GOTO_5_1',
'aborted':'aborted'})
# NavPoint 5
StateMachine.add('GOTO_5_1', Nav2Waypoint(self.waypoints[4]),
transitions={'succeeded':'CALIBRATE_X',
'aborted':'aborted'})
# Create the top level state machine
self.sm_top = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
# Add nav_patrol, sm_recharge and a Stop() machine to sm_top
with self.sm_top:
StateMachine.add('ACTIONS', self.sm_actions, transitions={'succeeded':'ACTIONS', 'aborted':'STOP'})
#StateMachine.add('RECHARGE', self.sm_recharge, transitions={'succeeded':'PATROL'})
StateMachine.add('STOP', Stop(), transitions={'succeeded':''})
# Create and start the SMACH introspection server
intro_server = IntrospectionServer('patrol', self.sm_top, '/SM_ROOT')
intro_server.start()
# Execute the state machine
sm_outcome = self.sm_top.execute()
rospy.loginfo('State Machine Outcome: ' + str(sm_outcome))
intro_server.stop()
def tasker():
rospy.init_node('tasker')
wfg = WaitForGoalState() # We don't want multiple subscribers so we need one WaitForGoal state
runner = SMACHRunner(rgoap_subclasses)
## sub machines
sq_move_to_new_goal = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded')
with sq_move_to_new_goal:
Sequence.add('WAIT_FOR_GOAL', wfg)
Sequence.add('MOVE_BASE_RGOAP', MoveBaseRGOAPState(runner))
sq_autonomous_rgoap = Sequence(outcomes=['preempted'],
connector_outcome='succeeded')
with sq_autonomous_rgoap:
Sequence.add('SLEEP_UNTIL_ENABLED', get_sleep_until_smach_enabled_smach())
Sequence.add('AUTONOMOUS_RGOAP', AutonomousRGOAPState(runner),
transitions={'succeeded':'SLEEP_UNTIL_ENABLED',
'aborted':'SLEEP'})
Sequence.add('SLEEP', SleepState(5),
transitions={'succeeded':'SLEEP_UNTIL_ENABLED'})
## tasker machine
sm_tasker = StateMachine(outcomes=['succeeded', 'aborted', 'preempted',
'field_error', 'undefined_task'],
input_keys=['task_goal'])
with sm_tasker:
## add all tasks to be available
# states using rgoap
StateMachine.add('MOVE_TO_NEW_GOAL_RGOAP', sq_move_to_new_goal)
StateMachine.add('INCREASE_AWARENESS_RGOAP', IncreaseAwarenessRGOAPState(runner))
StateMachine.add('AUTONOMOUS_RGOAP_CYCLE', sq_autonomous_rgoap)
StateMachine.add('AUTONOMOUS_RGOAP_SINGLE_GOAL', AutonomousRGOAPState(runner))
# states from uashh_smach
StateMachine.add('LOOK_AROUND', get_lookaround_smach())
StateMachine.add('GLIMPSE_AROUND', get_lookaround_smach(glimpse=True))
StateMachine.add('MOVE_ARM_CRAZY', get_lookaround_smach(crazy=True))
StateMachine.add('MOVE_TO_RANDOM_GOAL', get_random_goal_smach())
StateMachine.add('MOVE_TO_NEW_GOAL_AND_RETURN', task_go_and_return.get_go_and_return_smach())
StateMachine.add('PATROL_TO_NEW_GOAL', task_patrol.get_patrol_smach())
StateMachine.add('MOVE_AROUND', task_move_around.get_move_around_smach())
StateMachine.add('SLEEP_FIVE_SEC', SleepState(5))
## now the task receiver is created and automatically links to
## all task states added above
task_states_labels = sm_tasker.get_children().keys()
task_states_labels.sort() # sort alphabetically and then by _RGOAP
task_states_labels.sort(key=lambda label: '_RGOAP' in label, reverse=True)
task_receiver_transitions = {'undefined_outcome':'undefined_task'}
task_receiver_transitions.update({l:l for l in task_states_labels})
StateMachine.add('TASK_RECEIVER',
UserDataToOutcomeState(task_states_labels,
'task_goal',
lambda ud: ud.task_id),
task_receiver_transitions)
sm_tasker.set_initial_state(['TASK_RECEIVER'])
rospy.loginfo('tasker starting, available tasks: %s', ', '.join(task_states_labels))
pub = rospy.Publisher('/task/available_tasks', String, latch=True)
thread.start_new_thread(rostopic.publish_message, (pub, String, [', '.join(task_states_labels)], 1))
asw = ActionServerWrapper('activate_task', TaskActivationAction,
wrapped_container=sm_tasker,
succeeded_outcomes=['succeeded'],
aborted_outcomes=['aborted', 'undefined_task'],
preempted_outcomes=['preempted'],
goal_key='task_goal'
)
# Create and start the introspection server
sis = IntrospectionServer('smach_tasker_action', sm_tasker, '/SM_ROOT')
sis.start()
asw.run_server()
rospy.spin()
sis.stop()
return ggoal
smach.StateMachine.add(
'GRASP',
SimpleActionState( 'overhead_grasp',
OverheadGraspAction,
goal_cb = grasp_goal_cb,
input_keys = ['grasp_poses']),
remapping = {'grasp_poses':'object_poses'},
transitions = { 'succeeded': 'succeeded',
'aborted':'THREE_TRIES' })
return sm
if __name__ == '__main__':
rospy.init_node('smach_sm_grasp')
sm = sm_grasp()
sis = IntrospectionServer('Grasping', sm, '/SM_GRASPING')
sis.start()
outcome = sm.execute()
sis.stop()
def __init__(self):
rospy.init_node("deliver_food", anonymous=False)
self.initialize_destination()
self.lalala = 100
# Track success rate of getting to the goal locations
self.n_succeeded = 0
self.n_aborted = 0
self.n_preempted = 0
nav_states = {}
for room in self.room_locations.iterkeys():
nav_goal = MoveBaseGoal()
nav_goal.target_pose.header.frame_id = "map"
nav_goal.target_pose.pose = self.room_locations[room]
move_base_state = SimpleActionState(
"move_base",
MoveBaseAction,
goal=nav_goal,
result_cb=self.move_base_result_cb,
exec_timeout=rospy.Duration(60.0),
server_wait_timeout=rospy.Duration(10.0),
)
nav_states[room] = move_base_state
rospy.loginfo(
room
+ " -> ["
+ str(round(self.room_locations[room].position.x, 2))
+ ", "
+ str(round(self.room_locations[room].position.y, 2))
+ "]"
)
sm_rotate_search = Concurrence(
outcomes=["find", "not_find"],
default_outcome="not_find",
child_termination_cb=self.concurrence_child_termination_callback,
outcome_cb=self.concurrence_outcome_callback,
)
with sm_rotate_search:
Concurrence.add("ROTATE", Rotate360(0.4, 2 * pi))
Concurrence.add("SEARCH", SearchTable())
sm_table1 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table1:
StateMachine.add(
"GOTO_TABLE1",
nav_states["table1"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""}
)
sm_table2 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table2:
StateMachine.add(
"GOTO_TABLE2",
nav_states["table2"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""}
)
sm_table3 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table3:
StateMachine.add(
"GOTO_TABLE3",
nav_states["table3"],
transitions={
"succeeded": "ROTATE_SEARCH",
"aborted": "GOTO_CHECKPOINT_2",
"preempted": "GOTO_CHECKPOINT_2",
},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a"}
)
# if something wrong when we tried to go to the table
StateMachine.add(
"GOTO_CHECKPOINT_2",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_TABLE3_2", "aborted": "GOTO_CHECKPOINT_1"},
)
StateMachine.add(
"GOTO_CHECKPOINT_1",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_CHECKPOINT_2z", "aborted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_CHECKPOINT_2z",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_TABLE3_2", "aborted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE3_2",
nav_states["table3"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"},
)
StateMachine.add(
"CLEARING_NOISE_GO",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_TABLE3_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE3_3",
nav_states["table3"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"},
)
# if something wrong when we tried to go back to kitchen
StateMachine.add(
"GOTO_CHECKPOINT_1a",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_CHECKPOINT_2a",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_a",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_KITCHEN_2",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_b",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"}
)
sm_table4 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table4:
StateMachine.add(
"GOTO_TABLE4",
nav_states["table4"],
transitions={
"succeeded": "ROTATE_SEARCH",
"aborted": "GOTO_CHECKPOINT_2",
"preempted": "GOTO_CHECKPOINT_2",
},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a", "preempted": "GOTO_CHECKPOINT_1a"},
)
# if something wrong when we tried to go to the table
StateMachine.add(
"GOTO_CHECKPOINT_2",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_TABLE4_2", "aborted": "GOTO_CHECKPOINT_1"},
)
StateMachine.add(
"GOTO_CHECKPOINT_1",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_CHECKPOINT_2", "aborted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE4_2",
nav_states["table4"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"},
)
StateMachine.add(
"CLEARING_NOISE_GO",
Rotate360(0.4, 2 * pi),
transitions={"full_rotate": "GOTO_TABLE4_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE4_3",
nav_states["table4"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"},
)
# if something wrong when we tried to go back to kitchen
StateMachine.add(
"GOTO_CHECKPOINT_1a",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_CHECKPOINT_2a",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_a",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_KITCHEN_2",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_b",
Rotate360(0.4, 2 * pi),
transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"}
)
sm_table5 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table5:
StateMachine.add(
"GOTO_TABLE5",
nav_states["table5"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "", "preempted": ""}
)
sm_table6 = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_table6:
StateMachine.add(
"GOTO_TABLE6",
nav_states["table6"],
transitions={
"succeeded": "ROTATE_SEARCH",
"aborted": "GOTO_CHECKPOINT_2",
"preempted": "GOTO_CHECKPOINT_2",
},
)
StateMachine.add(
"ROTATE_SEARCH", sm_rotate_search, transitions={"find": "DO_STUFFS", "not_find": "GOTO_KITCHEN"}
)
StateMachine.add(
"DO_STUFFS",
DoStuffs(5),
transitions={"succeeded": "GOTO_KITCHEN", "aborted": "GOTO_KITCHEN", "preempted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "GOTO_CHECKPOINT_1a", "preempted": "GOTO_CHECKPOINT_1a"},
)
# if something wrong when we tried to go to the table
StateMachine.add(
"GOTO_CHECKPOINT_2",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_TABLE6_2", "aborted": "GOTO_CHECKPOINT_1"},
)
StateMachine.add(
"GOTO_CHECKPOINT_1",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_CHECKPOINT_2", "aborted": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE6_2",
nav_states["table6"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "CLEARING_NOISE_GO"},
)
StateMachine.add(
"CLEARING_NOISE_GO",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_TABLE6_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_TABLE6_3",
nav_states["table6"],
transitions={"succeeded": "ROTATE_SEARCH", "aborted": "GOTO_KITCHEN"},
)
# if something wrong when we tried to go back to kitchen
StateMachine.add(
"GOTO_CHECKPOINT_1a",
nav_states["checkpoint1"],
transitions={"succeeded": "GOTO_KITCHEN_2", "aborted": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_CHECKPOINT_2a",
nav_states["checkpoint2"],
transitions={"succeeded": "GOTO_CHECKPOINT_1a", "aborted": "CLEARING_NOISE_BACK_a"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_a",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_CHECKPOINT_2a", "not_full_rotate": "GOTO_CHECKPOINT_2a"},
)
StateMachine.add(
"GOTO_KITCHEN_2",
nav_states["kitchen"],
transitions={"succeeded": "", "aborted": "CLEARING_NOISE_BACK_b"},
)
StateMachine.add(
"CLEARING_NOISE_BACK_b",
Rotate360(0.8, 2 * pi),
transitions={"full_rotate": "GOTO_KITCHEN_3", "not_full_rotate": "GOTO_KITCHEN"},
)
StateMachine.add(
"GOTO_KITCHEN_3", nav_states["kitchen"], transitions={"succeeded": "", "aborted": "GOTO_KITCHEN_2"}
)
# let's initialize the overall state machine
sm_deliverfood = StateMachine(outcomes=["succeeded", "aborted", "preempted"])
with sm_deliverfood:
StateMachine.add("STARTING_TASK", Welcome(), transitions={"succeeded": "COMPUTER_VISION_TASK"})
StateMachine.add(
"COMPUTER_VISION_TASK",
ComputerVision(),
transitions={
"detect1": "TABLE_ONE_TASK",
"detect2": "TABLE_TWO_TASK",
"detect3": "TABLE_THREE_TASK",
"detect4": "TABLE_FOUR_TASK",
"detect5": "TABLE_FIVE_TASK",
"detect6": "TABLE_SIX_TASK",
"preempted": "",
},
)
StateMachine.add(
"TABLE_ONE_TASK",
sm_table1,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_TWO_TASK",
sm_table2,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_THREE_TASK",
sm_table3,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_FOUR_TASK",
sm_table4,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_FIVE_TASK",
sm_table5,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"TABLE_SIX_TASK",
sm_table6,
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
StateMachine.add(
"GOTO_KITCHEN",
nav_states["kitchen"],
transitions={
"succeeded": "COMPUTER_VISION_TASK",
"aborted": "GOTO_KITCHEN",
"preempted": "GOTO_KITCHEN",
},
)
# Create and start the SMACH introspection server
intro_server = IntrospectionServer("deliver_food", sm_deliverfood, "/SM_ROOT")
intro_server.start()
# Execute the state machine
sm_outcome = sm_deliverfood.execute()
rospy.on_shutdown(self.shutdown)