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