def get_move_around_smach():
sm = StateMachine(outcomes=['preempted'])
#sm.set_initial_state('CHECK_MOVEMENT')
with sm:
StateMachine.add('SLEEP_UNTIL_ENABLED', util.get_sleep_until_smach_enabled_smach(),
transitions={'succeeded':'MOVE_RANDOMLY'})
StateMachine.add('MOVE_RANDOMLY', move_base.get_random_goal_smach('/base_link'),
transitions={'succeeded':'SLEEP_UNTIL_ENABLED',
'aborted':'SLEEP_UNTIL_ENABLED'})
# StateMachine.add("ARM_LOOK_AROUND", look_around.get_lookaround_smach(util.SleepState(LOOKAROUND_SLEEP_DURATION)),
# StateMachine.add('ARM_LOOK_AROUND', util.SleepState(1), # mockup
return sm
def get_per_goal_subsmach():
sq = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded',
input_keys=['x', 'y', 'yaw'])
with sq:
# check for permission
Sequence.add('SLEEP_UNTIL_ENABLED', util.get_sleep_until_smach_enabled_smach())
# nav to goal
Sequence.add('MOVE_BASE_GOAL', move_base.MoveBaseState(),
remapping={'x':'x',
'y':'y',
'yaw':'yaw'}
)
# wait
Sequence.add('PAUSE_AT_GOAL', util.SleepState(5))
return sq
def get_move_around_smach():
sm = StateMachine(outcomes=['preempted'])
#sm.set_initial_state('CHECK_MOVEMENT')
with sm:
StateMachine.add('SLEEP_UNTIL_ENABLED',
util.get_sleep_until_smach_enabled_smach(),
transitions={'succeeded': 'MOVE_RANDOMLY'})
StateMachine.add('MOVE_RANDOMLY',
move_base.get_random_goal_smach('/base_link'),
transitions={
'succeeded': 'SLEEP_UNTIL_ENABLED',
'aborted': 'SLEEP_UNTIL_ENABLED'
})
# StateMachine.add("ARM_LOOK_AROUND", look_around.get_lookaround_smach(util.SleepState(LOOKAROUND_SLEEP_DURATION)),
# StateMachine.add('ARM_LOOK_AROUND', util.SleepState(1), # mockup
return sm
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()
