def create(self): control_topic = 'control' joint_trajectory_topic = 'motion/controller/joint_trajectory' joy_topic = '/hmi/joystick' moveit_action = 'move_group' # x:68 y:631, x:130 y:353 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.unused = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:414 y:360, x:130 y:353, x:506 y:357, x:233 y:351, x:720 y:363, x:715 y:411 _sm_randmovements_0 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['unused'], conditions=[('failed', [('WaitKey', 'unavailable')]), ('finished', [('RandMovements', 'done')]), ('finished', [('WaitKey', 'received')]), ('failed', [('RandMovements', 'failed')])]) with _sm_randmovements_0: # x:148 y:122 OperatableStateMachine.add('WaitKey', WaitForMessageState( topic=joy_topic, condition=lambda x: any(x.buttons), buffered=False, clear=False), transitions={ 'received': 'finished', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:366 y:118 OperatableStateMachine.add('RandMovements', SweetieBotRandHeadMovements( controller='joint_state_head', duration=120, interval=[3, 5], max2356=[0.3, 0.3, 1.5, 1.5], min2356=[-0.3, -0.3, -1.5, -1.5]), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'config': 'unused'}) with _state_machine: # x:108 y:53 OperatableStateMachine.add('RandMovements', _sm_randmovements_0, transitions={ 'finished': 'HeadBasic', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'unused': 'unused'}) # x:351 y:150 OperatableStateMachine.add('HeadBasic', SrdfStateToMoveit( config_name='head_upright', move_group='head', action_topic=moveit_action, robot_name=''), transitions={ 'reached': 'GoodBye', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name', 'move_group': 'move_group', 'robot_name': 'robot_name', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:329 y:271 OperatableStateMachine.add( 'GoodBye', CompoundAction( t1=[0, 0.0], type1='voice/play_wav', cmd1='thank_you_for_attention_nopony_was_harmed', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='introduce_herself', t3=[2, 2.0], type3='voice/play_wav', cmd3='am_i_really_a_good_actress', t4=[2, 2.0], type4='motion/joint_trajectory', cmd4='head_suprised'), transitions={ 'success': 'Bow', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:288 y:547 OperatableStateMachine.add('RandMovements2', SweetieBotRandHeadMovements( controller='joint_state_head', duration=120, interval=[3, 5], max2356=[0.3, 0.3, 1.5, 1.5], min2356=[-0.3, -0.3, -1.5, -1.5]), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'config': 'unused'}) # x:319 y:387 OperatableStateMachine.add('Bow', CompoundAction( t1=[0, 2.0], type1='motion/joint_trajectory', cmd1='bow_begin', t2=[1, 2.0], type2='motion/joint_trajectory', cmd2='bow_end', t3=[0, 0.0], type3=None, cmd3='', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'RandMovements2', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) return _state_machine
def create(self): voice_topic = 'control' torque_off_service = 'motion/controller/torque_off/set_operational' joint_state_control_topic = 'motion/controller/joint_state/out_joints_src_reset' joy_topic = '/hmi/joystick' eyes_topic = 'control' # x:68 y:535, x:786 y:525 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.be_evil = self.be_evil # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:554 y:394, x:130 y:397 _sm_waitforjoyevent_0 = OperatableStateMachine( outcomes=['received', 'unavailable']) with _sm_waitforjoyevent_0: # x:288 y:158 OperatableStateMachine.add('JoyEvent', SubscriberState(topic=joy_topic, blocking=True, clear=False), transitions={ 'received': 'CheckEvent', 'unavailable': 'unavailable' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:485 y:159 OperatableStateMachine.add( 'CheckEvent', DecisionState(outcomes=['noactivity', 'activity'], conditions=lambda msg: 'activity' if any(msg.buttons) or any(msg.axes[0:5]) else 'noactivity'), transitions={ 'noactivity': 'JoyEvent', 'activity': 'received' }, autonomy={ 'noactivity': Autonomy.Off, 'activity': Autonomy.Off }, remapping={'input_value': 'message'}) # x:538 y:386, x:347 y:384, x:134 y:383, x:831 y:385, x:906 y:386 _sm_idlebehavior_1 = ConcurrencyContainer( outcomes=['finished', 'failed'], conditions=[('finished', [('RandomHeadMovements', 'done')]), ('failed', [('WaitForJoyEvent', 'unavailable')]), ('finished', [('WaitForJoyEvent', 'received')])]) with _sm_idlebehavior_1: # x:474 y:110 OperatableStateMachine.add('RandomHeadMovements', SweetieBotRandHeadMovements( controller='joint_state_head', duration=100, interval=[1, 4], max2356=[0.3, 0.3, 1, 1], min2356=[-0.3, -0.3, -1, -1]), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:144 y:133 OperatableStateMachine.add('WaitForJoyEvent', _sm_waitforjoyevent_0, transitions={ 'received': 'finished', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Inherit, 'unavailable': Autonomy.Inherit }) with _state_machine: # x:79 y:99 OperatableStateMachine.add( 'CheckEvil', DecisionState(outcomes=['good', 'evil'], conditions=lambda x: 'evil' if x else 'good'), transitions={ 'good': 'NormalEyes', 'evil': 'RedEyes' }, autonomy={ 'good': Autonomy.Off, 'evil': Autonomy.Off }, remapping={'input_value': 'be_evil'}) # x:550 y:467 OperatableStateMachine.add('AskForAssistance', TextCommandState(type='voice/play_wav', command='assistance', topic=voice_topic), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:440 y:126 OperatableStateMachine.add('WaitForMovementFinish', WaitState(wait_time=0.0), transitions={'done': 'DoTricks'}, autonomy={'done': Autonomy.Off}) # x:251 y:503 OperatableStateMachine.add( 'StartingStance', PublisherState(topic=joint_state_control_topic, msg_type=JointState, value={ 'name': [ 'joint51', 'joint52', 'joint53', 'eyes_pitch', 'eyes_yaw' ], 'position': [0.2, 0.0, 0.2, 0.0, 0.0] }), transitions={ 'done': 'CheckEvil2', 'failed': 'AskForAssistance' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }) # x:731 y:153 OperatableStateMachine.add('DoTricks', self.use_behavior( DoTricksSM, 'DoTricks'), transitions={ 'finished': 'NormalLook', 'failed': 'AskForAssistance' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'be_evil': 'be_evil'}) # x:27 y:219 OperatableStateMachine.add('RedEyes', TextCommandState(type='eyes/emotion', command='red_eyes', topic=eyes_topic), transitions={'done': 'IdleBehavior'}, autonomy={'done': Autonomy.Off}) # x:222 y:65 OperatableStateMachine.add('NormalEyes', TextCommandState(type='eyes/emotion', command='normal', topic=eyes_topic), transitions={'done': 'IdleBehavior'}, autonomy={'done': Autonomy.Off}) # x:481 y:305 OperatableStateMachine.add('IdleBehavior', _sm_idlebehavior_1, transitions={ 'finished': 'StartingStance', 'failed': 'AskForAssistance' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:181 y:360 OperatableStateMachine.add( 'CheckEvil2', DecisionState(outcomes=['evil', 'good'], conditions=lambda x: 'evil' if x else 'good'), transitions={ 'evil': 'EvilLook', 'good': 'WaitForMovementFinish' }, autonomy={ 'evil': Autonomy.Off, 'good': Autonomy.Off }, remapping={'input_value': 'be_evil'}) # x:188 y:294 OperatableStateMachine.add( 'EvilLook', TextCommandState(type='eyes/emotion', command='evil_look', topic=eyes_topic), transitions={'done': 'WaitForMovementFinish'}, autonomy={'done': Autonomy.Off}) # x:880 y:311 OperatableStateMachine.add('NormalLook', TextCommandState(type='eyes/emotion', command='normal_look', topic=eyes_topic), transitions={'done': 'IdleBehavior'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:583 y:683, x:578 y:422 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.unused = None _state_machine.userdata.rand_head_config = {'interval': [0.5, 1]} # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:278 y:24 OperatableStateMachine.add('SesectBehavior', OperatorDecisionState( outcomes=['dance', 'rand_moves'], hint='Select behavior', suggestion='dance'), transitions={ 'dance': 'TestMovement', 'rand_moves': 'RandHead' }, autonomy={ 'dance': Autonomy.Full, 'rand_moves': Autonomy.Full }) # x:114 y:352 OperatableStateMachine.add( 'TurnOffJointStateController', SetBoolState( service='motion/controller/joint_state/set_operational', value=False), transitions={ 'true': 'SingASong', 'false': 'failed', 'failure': 'failed' }, autonomy={ 'true': Autonomy.High, 'false': Autonomy.Off, 'failure': Autonomy.Off }, remapping={ 'success': 'success', 'message': 'message' }) # x:136 y:518 OperatableStateMachine.add('SingASong', TextCommandState(type='voice/play_wav', command='mmm_song', topic='control'), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Full}) # x:248 y:213 OperatableStateMachine.add( 'TestMovement', ExecuteJointTrajectory( action_topic='motion/controller/joint_trajectory', trajectory_param='dance14', trajectory_param='/saved_msgs/joint_trajectory'), transitions={ 'success': 'TurnOffJointStateController', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Low, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) # x:939 y:351 OperatableStateMachine.add( 'RandSelector', DecisionState( outcomes=['first', 'second'], conditions=(lambda x: 'first' if random.uniform(0, 1) < 0.5 else 'second')), transitions={ 'first': 'PlaceHead', 'second': 'TestMovement' }, autonomy={ 'first': Autonomy.Full, 'second': Autonomy.Full }, remapping={'input_value': 'unused'}) # x:704 y:603 OperatableStateMachine.add( 'HeadShake', ExecuteJointTrajectory( action_topic='motion/controller/joint_trajectory', trajectory_param='head_shake', trajectory_param='/saved_msgs/joint_trajectory'), transitions={ 'success': 'finished', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) # x:958 y:510 OperatableStateMachine.add('PlaceHead', SrdfStateToMoveit( config_name='head_basic', move_group='head', action_topic='move_group', robot_name=''), transitions={ 'reached': 'HeadShake', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name', 'move_group': 'move_group', 'robot_name': 'robot_name', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:725 y:125 OperatableStateMachine.add( 'RandHead', SweetieBotRandHeadMovements(controller='joint_state_head', duration=120, interval=[3, 5], max2356=[0.3, 0.3, 1.5, 1.5], min2356=[-0.3, -0.3, -1.5, -1.5]), transitions={ 'done': 'RandSelector', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'config': 'rand_head_config'}) return _state_machine
def create(self): leap_topic = '/hmi/leap_motion/data' eyes_cmd_topic = 'control' voice_topic = 'control' leap_pose_topic = '/hmi/leap_motion/pose' # x:286 y:634, x:989 y:651 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.be_evil = False _state_machine.userdata.pose = PoseStamped() _state_machine.userdata.unused = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:353, x:343 y:356, x:209 y:349, x:843 y:453, x:473 y:355, x:638 y:361, x:907 y:552 _sm_randheadmovements_0 = ConcurrencyContainer(outcomes=['object_detected', 'failed'], input_keys=['unused'], conditions=[ ('object_detected', [('WaitUntlObject', 'still_object')]), ('object_detected', [('WaitUntlObject', 'moving_object')]), ('failed', [('WaitUntlObject', 'no_object')]), ('object_detected', [('RandHeadMoveme', 'done')]), ('failed', [('RandHeadMoveme', 'failed')]) ]) with _sm_randheadmovements_0: # x:158 y:110 OperatableStateMachine.add('WaitUntlObject', LeapMotionMonitor(leap_motion_topic=leap_topic, exit_states=['still_object', 'moving_object'], pose_topic=leap_pose_topic, parameters=[2.0,0.02,0.2]), transitions={'no_object': 'failed', 'still_object': 'object_detected', 'moving_object': 'object_detected'}, autonomy={'no_object': Autonomy.Off, 'still_object': Autonomy.Off, 'moving_object': Autonomy.Off}, remapping={'pose': 'pose'}) # x:605 y:103 OperatableStateMachine.add('RandHeadMoveme', SweetieBotRandHeadMovements(controller='joint_state_head', duration=120, interval=[1,4], max2356=[0.3,0.5,1,1], min2356=[-0.3,-0.1,-1,-1]), transitions={'done': 'object_detected', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'config': 'unused'}) # x:30 y:353, x:357 y:355, x:671 y:353, x:140 y:353, x:237 y:356, x:530 y:353, x:742 y:441, x:730 y:353, x:452 y:353, x:732 y:406 _sm_waitnoobjecttimeout_1 = ConcurrencyContainer(outcomes=['no_object', 'timeout', 'failure', 'too_close'], conditions=[ ('no_object', [('WaitNoObject', 'no_object')]), ('failure', [('WaitNoObject', 'moving_object')]), ('failure', [('WaitNoObject', 'still_object')]), ('failure', [('FollowObject', 'failed')]), ('timeout', [('Wait', 'done')]), ('too_close', [('FollowObject', 'too_close')]) ]) with _sm_waitnoobjecttimeout_1: # x:162 y:101 OperatableStateMachine.add('WaitNoObject', LeapMotionMonitor(leap_motion_topic=leap_topic, exit_states=['no_object'], pose_topic=leap_pose_topic, parameters=[4.0,0.02,0.2]), transitions={'no_object': 'no_object', 'still_object': 'failure', 'moving_object': 'failure'}, autonomy={'no_object': Autonomy.Off, 'still_object': Autonomy.Off, 'moving_object': Autonomy.Off}, remapping={'pose': 'pose'}) # x:463 y:95 OperatableStateMachine.add('FollowObject', SweetieBotFollowHeadPoseSmart(pose_topic=leap_pose_topic, follow_joint_state_controller='joint_state_head', discomfort_time=4, neck_control_parameteres=[-0.13,0.3,0.20,0.2], deactivate=True, controlled_chains=['eyes','head']), transitions={'failed': 'failure', 'too_close': 'too_close'}, autonomy={'failed': Autonomy.Off, 'too_close': Autonomy.Off}) # x:332 y:94 OperatableStateMachine.add('Wait', WaitState(wait_time=self.play_timeout), transitions={'done': 'timeout'}, autonomy={'done': Autonomy.Off}) # x:30 y:353, x:130 y:353, x:330 y:366, x:737 y:355, x:898 y:596, x:899 y:486, x:900 y:533, x:902 y:443, x:568 y:345 _sm_watchonleapmotionobject_2 = ConcurrencyContainer(outcomes=['no_object', 'still_object', 'failed', 'to_close'], output_keys=['pose'], conditions=[ ('no_object', [('WaitTillObjectMOving', 'no_object')]), ('still_object', [('WaitTillObjectMOving', 'still_object')]), ('failed', [('WaitTillObjectMOving', 'moving_object')]), ('failed', [('HeadFollowsMovingObject', 'failed')]), ('to_close', [('HeadFollowsMovingObject', 'too_close')]) ]) with _sm_watchonleapmotionobject_2: # x:135 y:135 OperatableStateMachine.add('WaitTillObjectMOving', LeapMotionMonitor(leap_motion_topic='/hmi/leap_motion/data', exit_states=['no_object', 'still_object'], pose_topic='/hmi/leap_motion/pose', parameters=[1.0,0.02,0.2]), transitions={'no_object': 'no_object', 'still_object': 'still_object', 'moving_object': 'failed'}, autonomy={'no_object': Autonomy.High, 'still_object': Autonomy.High, 'moving_object': Autonomy.Off}, remapping={'pose': 'pose'}) # x:583 y:138 OperatableStateMachine.add('HeadFollowsMovingObject', SweetieBotFollowHeadPoseSmart(pose_topic='/hmi/leap_motion/pose', follow_joint_state_controller='joint_state_head', discomfort_time=4.0, neck_control_parameteres=[-0.13,0.3,0.20,0.2], deactivate=True, controlled_chains=['head','eyes']), transitions={'failed': 'failed', 'too_close': 'to_close'}, autonomy={'failed': Autonomy.Off, 'too_close': Autonomy.High}) # x:979 y:146, x:969 y:327 _sm_switchevil_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['be_evil'], output_keys=['be_evil']) with _sm_switchevil_3: # x:269 y:101 OperatableStateMachine.add('CheckState', DecisionState(outcomes=[ 'evil', 'good' ], conditions=lambda x: 'evil' if x else 'good'), transitions={'evil': 'NewState1', 'good': 'NewState2'}, autonomy={'evil': Autonomy.Off, 'good': Autonomy.Off}, remapping={'input_value': 'be_evil'}) # x:423 y:54 OperatableStateMachine.add('NewState1', CalculationState(calculation=lambda x: not random.random() < self.evil_to_good_switch_probability), transitions={'done': 'SwitchEvilMode'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'be_evil', 'output_value': 'new_be_evil'}) # x:425 y:159 OperatableStateMachine.add('NewState2', CalculationState(calculation=lambda x: random.random() < self.good_to_evil_switch_probability), transitions={'done': 'SwitchEvilMode'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'be_evil', 'output_value': 'new_be_evil'}) # x:678 y:91 OperatableStateMachine.add('SwitchEvilMode', self.use_behavior(SwitchEvilModeSM, 'SwitchEvil/SwitchEvilMode'), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'be_evil': 'be_evil', 'new_be_evil': 'new_be_evil'}) with _state_machine: # x:114 y:550 OperatableStateMachine.add('PlaceHead', SrdfStateToMoveit(config_name='head_upright', move_group='head', action_topic='move_group', robot_name=''), transitions={'reached': 'RandHeadMovements', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed'}, autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off}, remapping={'config_name': 'config_name', 'move_group': 'move_group', 'robot_name': 'robot_name', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'}) # x:500 y:88 OperatableStateMachine.add('Brohoof', self.use_behavior(BrohoofSM, 'Brohoof'), transitions={'finished': 'WaitNoObjectTimeout', 'unreachable': 'Greeting', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'unreachable': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'pose'}) # x:699 y:28 OperatableStateMachine.add('Greeting', self.use_behavior(GreetingSM, 'Greeting'), transitions={'finished': 'WaitNoObjectTimeout', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'be_evil': 'be_evil'}) # x:455 y:230 OperatableStateMachine.add('Play', self.use_behavior(PlaySM, 'Play'), transitions={'finished': 'WaitNoObjectTimeout', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'be_evil': 'be_evil'}) # x:78 y:247 OperatableStateMachine.add('SwitchEvil', _sm_switchevil_3, transitions={'finished': 'WatchOnLeapMotionObject', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'be_evil': 'be_evil'}) # x:387 y:6 OperatableStateMachine.add('CheckEvil', DecisionState(outcomes=['evil','good'], conditions=lambda x: 'evil' if x else 'good'), transitions={'evil': 'Greeting', 'good': 'Brohoof'}, autonomy={'evil': Autonomy.Off, 'good': Autonomy.Off}, remapping={'input_value': 'be_evil'}) # x:144 y:77 OperatableStateMachine.add('WatchOnLeapMotionObject', _sm_watchonleapmotionobject_2, transitions={'no_object': 'Play', 'still_object': 'CheckEvil', 'failed': 'failed', 'to_close': 'Bad'}, autonomy={'no_object': Autonomy.Inherit, 'still_object': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'to_close': Autonomy.Inherit}, remapping={'pose': 'pose'}) # x:283 y:293 OperatableStateMachine.add('Bad', self.use_behavior(BadSM, 'Bad'), transitions={'finished': 'SwitchEvil', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'be_evil': 'be_evil'}) # x:646 y:375 OperatableStateMachine.add('WaitNoObjectTimeout', _sm_waitnoobjecttimeout_1, transitions={'no_object': 'PlaceHead', 'timeout': 'Play', 'failure': 'failed', 'too_close': 'Bad'}, autonomy={'no_object': Autonomy.Inherit, 'timeout': Autonomy.Inherit, 'failure': Autonomy.Inherit, 'too_close': Autonomy.Inherit}) # x:106 y:411 OperatableStateMachine.add('RandHeadMovements', _sm_randheadmovements_0, transitions={'object_detected': 'SwitchEvil', 'failed': 'failed'}, autonomy={'object_detected': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'unused': 'unused'}) return _state_machine
def create(self): control_topic = 'control' voice_topic = 'control' joint_trajectory_action = 'motion/controller/joint_trajectory' joy_topic = '/hmi/joystick' moveit_action = 'move_group' storage = 'joint_trajectory' # x:157 y:271, x:501 y:342 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.joint_head_turned_right = [0.0, 0.6, 0.0, 0.0] _state_machine.userdata.head_forward_joints = [0.15, 0.0, -0.15, 0.0] _state_machine.userdata.unused = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:803 y:85, x:480 y:341 _sm_turnright_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['joint_head_turned_right']) with _sm_turnright_0: # x:282 y:82 OperatableStateMachine.add( 'TurnHead', MoveitToJointsState( move_group='head', joint_names=['joint51', 'joint52', 'joint53', 'joint54'], action_topic=moveit_action), transitions={ 'reached': 'TurnBody', 'planning_failed': 'failed', 'control_failed': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off }, remapping={'joint_config': 'joint_head_turned_right'}) # x:466 y:69 OperatableStateMachine.add('TurnBody', CompoundAction( t1=[0, 0.0], type1='motion/joint_trajectory', cmd1='turn_right', t2=[1, 0.0], type2='motion/joint_trajectory', cmd2='turn_right', t3=[2, 0.0], type3='motion/joint_trajectory', cmd3='turn_right', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'finished', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:30 y:353, x:148 y:355, x:529 y:348, x:443 y:347, x:840 y:329, x:752 y:347 _sm_waitbegining_1 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['unused'], conditions=[('finished', [('WaitKey', 'received')]), ('failed', [('WaitKey', 'unavailable')]), ('failed', [('RandHeadMovemets', 'failed')]), ('finished', [('RandHeadMovemets', 'done')])]) with _sm_waitbegining_1: # x:167 y:134 OperatableStateMachine.add('WaitKey', WaitForMessageState( topic=joy_topic, condition=lambda x: any(x.buttons), buffered=False, clear=False), transitions={ 'received': 'finished', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:442 y:126 OperatableStateMachine.add('RandHeadMovemets', SweetieBotRandHeadMovements( controller='joint_state_head', duration=600, interval=[3, 5], max2356=[0.3, 0.3, 1.5, 1.5], min2356=[-0.3, -0.3, -1.5, -1.5]), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'config': 'unused'}) with _state_machine: # x:25 y:176 OperatableStateMachine.add('WaitBegining', _sm_waitbegining_1, transitions={ 'finished': 'HeadPose', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'unused': 'unused'}) # x:343 y:49 OperatableStateMachine.add('WaitKey', WaitForMessageState( topic=joy_topic, condition=lambda x: any(x.buttons), buffered=False, clear=False), transitions={ 'received': 'TurnRight', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:785 y:172 OperatableStateMachine.add('AskAboutProto1', CompoundAction( t1=[0, 0.0], type1='voice/play_wav', cmd1='what_proto1', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='head_suprised_slow', t3=[0, 0.0], type3=None, cmd3='', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'WaitKey2', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:799 y:264 OperatableStateMachine.add('WaitKey2', WaitForMessageState( topic=joy_topic, condition=lambda x: any(x.buttons), buffered=False, clear=False), transitions={ 'received': 'AskAboutProto1Again', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:797 y:379 OperatableStateMachine.add('AskAboutProto1Again', CompoundAction( t1=[0, 0.0], type1='voice/play_wav', cmd1='tell_me_more_about_proto1', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='head_lean_forward_begin', t3=[0, 0.0], type3=None, cmd3='', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'WaitKey4', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:801 y:518 OperatableStateMachine.add( 'WaitKey4', WaitForMessageState(topic=joy_topic, condition=lambda x: any(x.buttons), buffered=False, clear=False), transitions={ 'received': 'AskWhatHappensToProto1', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:603 y:600 OperatableStateMachine.add('AskWhatHappensToProto1', CompoundAction( t1=[0, 0.0], type1='voice/play_wav', cmd1='what_happened_with_proto1', t2=[0, 1.2], type2='motion/joint_trajectory', cmd2='hoof_wave', t3=[0, 0.0], type3=None, cmd3='', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'WaitKey3', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:332 y:601 OperatableStateMachine.add('WaitKey3', WaitForMessageState( topic=joy_topic, condition=lambda x: any(x.buttons), buffered=False, clear=False), transitions={ 'received': 'SayItIsOk', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:554 y:51 OperatableStateMachine.add('TurnRight', _sm_turnright_0, transitions={ 'finished': 'WatchPresentaion', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'joint_head_turned_right': 'joint_head_turned_right' }) # x:81 y:597 OperatableStateMachine.add('SayItIsOk', CompoundAction( t1=[0, 0.0], type1='motion/joint_trajectory', cmd1='head_lean_forward_end', t2=[0, 0.0], type2='voice/play_wav', cmd2='yes_exactly_tell_me_more', t3=[0, 0.0], type3=None, cmd3='', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'WatchPresentaion_2', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:137 y:50 OperatableStateMachine.add( 'Hello', CompoundAction(t1=[0, 0.0], type1='motion/joint_trajectory', cmd1='introduce_herself', t2=[0, 0.0], type2='voice/play_wav', cmd2='hello_im_sweetie_bot_presentation', t3=[0, 0.0], type3=None, cmd3='', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'WaitKey', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:166 y:159 OperatableStateMachine.add('HeadPose', SrdfStateToMoveit( config_name='head_basic', move_group='head', action_topic='move_group', robot_name=''), transitions={ 'reached': 'Hello', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name', 'move_group': 'move_group', 'robot_name': 'robot_name', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:776 y:30 OperatableStateMachine.add( 'WatchPresentaion', self.use_behavior(WatchPresentaionSM, 'WatchPresentaion', default_keys=['rand_head_config']), transitions={ 'finished': 'AskAboutProto1', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'head_pose_joints': 'joint_head_turned_right', 'rand_head_config': 'rand_head_config' }) # x:92 y:465 OperatableStateMachine.add( 'WatchPresentaion_2', self.use_behavior(WatchPresentaionSM, 'WatchPresentaion_2', default_keys=['rand_head_config']), transitions={ 'finished': 'Greeting', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'head_pose_joints': 'head_forward_joints', 'rand_head_config': 'rand_head_config' }) # x:73 y:332 OperatableStateMachine.add( 'Greeting', ExecuteJointTrajectory( action_topic=joint_trajectory_action, trajectory_param='greeting', trajectory_ns='/saved_msgs/joint_trajectory'), transitions={ 'success': 'finished', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) return _state_machine
def create(self): storage = 'joint_trajectory/' joint_state_control_topic = 'motion/controller/joint_state/out_joints_src_reset' eyes_topic = 'control' voice_topic = 'control' joint_trajectory_action = 'motion/controller/joint_trajectory' # x:322 y:652, x:984 y:622 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.counter = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:220 y:573, x:535 y:559 _sm_seizure_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['counter']) with _sm_seizure_0: # x:125 y:111 OperatableStateMachine.add('SayDoNotTouch', TextCommandState(type='voice/play_wav', command='05donottouch', topic=voice_topic), transitions={'done': 'Seizure'}, autonomy={'done': Autonomy.Off}) # x:389 y:247 OperatableStateMachine.add( 'AddCounter', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Seizure'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'counter', 'output_value': 'counter' }) # x:412 y:87 OperatableStateMachine.add( 'Select', DecisionState( outcomes=['say1', 'say2', 'say3', 'end'], conditions=lambda x: ['say1', 'say2', 'say3', 'end'][x]), transitions={ 'say1': 'SayWalk', 'say2': 'SayBlaster', 'say3': 'SayWin', 'end': 'Farewell' }, autonomy={ 'say1': Autonomy.Off, 'say2': Autonomy.Off, 'say3': Autonomy.Off, 'end': Autonomy.High }, remapping={'input_value': 'counter'}) # x:612 y:81 OperatableStateMachine.add('SayWalk', TextCommandState(type='voice/play_wav', command='17walk', topic=voice_topic), transitions={'done': 'AddCounter'}, autonomy={'done': Autonomy.Off}) # x:681 y:173 OperatableStateMachine.add('SayBlaster', TextCommandState(type='voice/play_wav', command='18blaster', topic=voice_topic), transitions={'done': 'AddCounter'}, autonomy={'done': Autonomy.Off}) # x:686 y:240 OperatableStateMachine.add('SayWin', TextCommandState(type='voice/play_wav', command='08win', topic=voice_topic), transitions={'done': 'AddCounter'}, autonomy={'done': Autonomy.Off}) # x:85 y:238 OperatableStateMachine.add( 'Seizure', ExecuteJointTrajectory(action_topic=joint_trajectory_action, trajectory_param='seizure', trajectory_ns=storage), transitions={ 'success': 'Select', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) # x:132 y:419 OperatableStateMachine.add( 'Farewell', ExecuteJointTrajectory(action_topic=joint_trajectory_action, trajectory_param='farewell', trajectory_ns=storage), transitions={ 'success': 'finished', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) with _state_machine: # x:15 y:642 OperatableStateMachine.add('RandomMovements', SweetieBotRandHeadMovements( controller='joint_state_head', duration=100000, interval=[2, 3], max2356=[0.3, 0.3, 1.5, 1.5], min2356=[-0.3, -0.3, -1.5, -1.5]), transitions={'done': 'StartPose'}, autonomy={'done': Autonomy.High}) # x:124 y:352 OperatableStateMachine.add('WaitSayIamAlive', WaitState(wait_time=1), transitions={'done': 'WarmUp'}, autonomy={'done': Autonomy.Off}) # x:102 y:154 OperatableStateMachine.add( 'LookAtHoof', ExecuteJointTrajectory( action_topic='motion/controller/joint_trajectory', trajectory_param='look_on_hoof', trajectory_ns=storage), transitions={ 'success': 'SayIamAlive', 'partial_movement': 'finished', 'invalid_pose': 'finished', 'failure': 'finished' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) # x:96 y:484 OperatableStateMachine.add( 'WarmUp', ExecuteJointTrajectory( action_topic='motion/controller/joint_trajectory', trajectory_param='little_shake_fast', trajectory_ns=storage), transitions={ 'success': 'WaitBeforeDomination', 'partial_movement': 'finished', 'invalid_pose': 'finished', 'failure': 'finished' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) # x:528 y:547 OperatableStateMachine.add('SayMenace', TextCommandState( type='voice/play_wav', command='02technic', topic='/sweetie_bot/voice/voice'), transitions={'done': 'WaitSayMenace'}, autonomy={'done': Autonomy.Off}) # x:529 y:454 OperatableStateMachine.add('WaitSayMenace', WaitState(wait_time=2.3), transitions={'done': 'EvilLook'}, autonomy={'done': Autonomy.Off}) # x:396 y:219 OperatableStateMachine.add( 'Menace', ExecuteJointTrajectory( action_topic='motion/controller/joint_trajectory', trajectory_param='menace', trajectory_ns=storage), transitions={ 'success': 'TryingToAcquireControl', 'partial_movement': 'finished', 'invalid_pose': 'finished', 'failure': 'finished' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) # x:431 y:107 OperatableStateMachine.add('TryingToAcquireControl', WaitState(wait_time=10), transitions={'done': 'SadLook'}, autonomy={'done': Autonomy.Off}) # x:429 y:301 OperatableStateMachine.add('SayBattleSystemActivation', TextCommandState( type='voice/play_wav', command='03domination', topic='/sweetie_bot/voice/voice'), transitions={'done': 'Menace'}, autonomy={'done': Autonomy.Off}) # x:840 y:91 OperatableStateMachine.add('SayDominationFailed', TextCommandState( type='voice/play_wav', command='04notfound', topic='/sweetie_bot/voice/voice'), transitions={'done': 'WaitFailed'}, autonomy={'done': Autonomy.Off}) # x:818 y:281 OperatableStateMachine.add( 'MenceCanceled', ExecuteJointTrajectory( action_topic='motion/controller/joint_trajectory', trajectory_param='menace_canceled', trajectory_ns=storage), transitions={ 'success': 'WaitBeforeHoofStamp', 'partial_movement': 'finished', 'invalid_pose': 'finished', 'failure': 'finished' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) # x:885 y:188 OperatableStateMachine.add('WaitFailed', WaitState(wait_time=1), transitions={'done': 'MenceCanceled'}, autonomy={'done': Autonomy.Off}) # x:139 y:573 OperatableStateMachine.add('WaitBeforeDomination', WaitState(wait_time=2), transitions={'done': 'RedEyes'}, autonomy={'done': Autonomy.Off}) # x:856 y:396 OperatableStateMachine.add('WaitBeforeHoofStamp', WaitState(wait_time=1), transitions={'done': 'HoofStamp'}, autonomy={'done': Autonomy.Off}) # x:826 y:490 OperatableStateMachine.add( 'HoofStamp', ExecuteJointTrajectory( action_topic='motion/controller/joint_trajectory', trajectory_param='hoof_stamp', trajectory_ns=storage), transitions={ 'success': 'NormalLook', 'partial_movement': 'finished', 'invalid_pose': 'finished', 'failure': 'finished' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) # x:385 y:567 OperatableStateMachine.add('RedEyes', TextCommandState(type='eyes/emotion', command='red_eyes', topic=eyes_topic), transitions={'done': 'SayMenace'}, autonomy={'done': Autonomy.Off}) # x:151 y:77 OperatableStateMachine.add( 'LookAround', ExecuteJointTrajectory( action_topic='motion/controller/joint_trajectory', trajectory_param='look_around', trajectory_ns=storage), transitions={ 'success': 'LookAtHoof', 'partial_movement': 'finished', 'invalid_pose': 'finished', 'failure': 'finished' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) # x:375 y:18 OperatableStateMachine.add('Wait', WaitState(wait_time=1), transitions={'done': 'LookAround'}, autonomy={'done': Autonomy.Off}) # x:195 y:10 OperatableStateMachine.add('NormalEyes', TextCommandState(type='eyes/emotion', command='normal', topic=eyes_topic), transitions={'done': 'Wait'}, autonomy={'done': Autonomy.Off}) # x:519 y:374 OperatableStateMachine.add( 'EvilLook', TextCommandState(type='eyes/emotion', command='evil_look', topic=eyes_topic), transitions={'done': 'SayBattleSystemActivation'}, autonomy={'done': Autonomy.Off}) # x:696 y:85 OperatableStateMachine.add( 'SadLook', TextCommandState(type='eyes/emotion', command='sad_look', topic=eyes_topic), transitions={'done': 'SayDominationFailed'}, autonomy={'done': Autonomy.Off}) # x:817 y:569 OperatableStateMachine.add('NormalLook', TextCommandState(type='eyes/emotion', command='normal_look', topic=eyes_topic), transitions={'done': 'Seizure'}, autonomy={'done': Autonomy.High}) # x:1124 y:445 OperatableStateMachine.add('Seizure', _sm_seizure_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'counter': 'counter'}) # x:40 y:35 OperatableStateMachine.add( 'StartPose', PublisherState(topic=joint_state_control_topic, msg_type=JointState, value={ 'name': [ 'joint51', 'joint52', 'joint53', 'eyes_pitch', 'eyes_yaw' ], 'position': [0.2, 0.0, 0, 0.0, 0.0] }), transitions={ 'done': 'NormalEyes', 'failed': 'finished' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }) # x:127 y:254 OperatableStateMachine.add('SayIamAlive', TextCommandState( type='voice/play_wav', command='01alive', topic='/sweetie_bot/voice/voice'), transitions={'done': 'WaitSayIamAlive'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:902 y:403, x:697 y:491 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.be_evil = False _state_machine.userdata.unused = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:32 y:127 OperatableStateMachine.add('SetStandingPose', SetJointState(controller='motion/controller/joint_state_head', pose_param='body_nominal', pose_ns='saved_msgs/joint_state', tolerance=0.017, timeout=10.0, joint_topic="joint_states"), transitions={'done': 'SelectTrick', 'failed': 'failed', 'timeout': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off, 'timeout': Autonomy.Off}) # x:371 y:161 OperatableStateMachine.add('Greeting', self.use_behavior(GreetingSM, 'Greeting'), transitions={'finished': 'RandHead', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'be_evil': 'be_evil'}) # x:371 y:284 OperatableStateMachine.add('Play', self.use_behavior(PlaySM, 'Play'), transitions={'finished': 'RandHead', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'be_evil': 'be_evil'}) # x:202 y:154 OperatableStateMachine.add('SelectTrick', DecisionState(outcomes=['play','cheer','greet'], conditions=lambda x: random.choice(['play','cheer','greet'])), transitions={'play': 'Play', 'cheer': 'Cheer', 'greet': 'Greeting'}, autonomy={'play': Autonomy.Off, 'cheer': Autonomy.Off, 'greet': Autonomy.Off}, remapping={'input_value': 'unused'}) # x:823 y:274 OperatableStateMachine.add('PrepareToWalk', ExecuteJointTrajectory(action_topic='motion/controller/joint_trajectory', trajectory_param='crouch_begin', trajectory_ns='saved_msgs/joint_trajectory'), transitions={'success': 'finished', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed'}, autonomy={'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off}) # x:376 y:38 OperatableStateMachine.add('Cheer', self.use_behavior(CheerSM, 'Cheer'), transitions={'finished': 'RandHead', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'be_evil': 'be_evil'}) # x:774 y:45 OperatableStateMachine.add('RandHead', SweetieBotRandHeadMovements(controller='joint_state_head', duration=8, interval=[3,5], max2356=[0.3,0.3,1.5,1.5], min2356=[-0.3,-0.3,-1.5,-1.5]), transitions={'done': 'SetHeadNominal', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'config': 'unused'}) # x:809 y:138 OperatableStateMachine.add('SetHeadNominal', SetJointState(controller='motion/controller/joint_state_head', pose_param='head_nominal', pose_ns='saved_msgs/joint_state', tolerance=0.017, timeout=10.0, joint_topic="joint_states"), transitions={'done': 'PrepareToWalk', 'failed': 'failed', 'timeout': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off, 'timeout': Autonomy.Off}) return _state_machine
def create(self): joy_topic = '/hmi/joystick' action_button = 12 # x:105 y:208, x:418 y:227 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.rand_head_config = { 'min2356': [-0.5, 0.1, -1.0, -1.0], 'max2356': [0.5, 0.5, 1.0, 1.0] } _state_machine.userdata.head_pose_joints = [0.0, 0.0, 0.0, 0.0, 0.0] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:284, x:130 y:284, x:230 y:284, x:330 y:284, x:430 y:293, x:530 y:293 _sm_waitstart_0 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['rand_head_config'], conditions=[('finished', [('WaitKey1', 'received')]), ('failed', [('WaitKey1', 'unavailable')]), ('finished', [('RandMovements', 'done')]), ('failed', [('RandMovements', 'failed')])]) with _sm_waitstart_0: # x:98 y:61 OperatableStateMachine.add( 'WaitKey1', WaitForMessageState( topic=joy_topic, condition=lambda x: x.buttons[action_button], buffered=False, clear=False), transitions={ 'received': 'finished', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:335 y:61 OperatableStateMachine.add( 'RandMovements', SweetieBotRandHeadMovements(controller='joint_state_head', duration=120, interval=[1, 4], max2356=[0.3, 0.3, 1.5, 1.5], min2356=[-0.3, -0.3, -1.5, -1.5]), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'config': 'rand_head_config'}) with _state_machine: # x:52 y:35 OperatableStateMachine.add('NormalEyes', TextCommandState(type='eyes/emotion', command='normal', topic='control'), transitions={'done': 'WaitStart'}, autonomy={'done': Autonomy.Off}) # x:642 y:281 OperatableStateMachine.add( 'HelloGoodThenEvil', CompoundAction(t1=[0, 0.0], type1='voice/play_wav', cmd1='zdravstvuyte_dorogie_gosti', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='look_on_printer', t3=[2, 0.0], type3='motion/joint_trajectory', cmd3='menace', t4=[3, 4.0], type4='motion/joint_trajectory', cmd4='menace_canceled'), transitions={ 'success': 'MicrophoneTurnOffReaction', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:634 y:65 OperatableStateMachine.add('FakeApoplogy', CompoundAction( t1=[0, 0.0], type1='voice/play_wav', cmd1='a_tochno_sboi_pamayati', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='head_suprised', t3=[0, 2.8], type3='motion/joint_trajectory', cmd3='head_node', t4=[0, 6.5], type4='motion/joint_trajectory', cmd4='hoof_knock'), transitions={ 'success': 'WaitKey3', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:467 y:314 OperatableStateMachine.add( 'WaitKey4', WaitForMessageState( topic=joy_topic, condition=lambda x: x.buttons[action_button], buffered=False, clear=False), transitions={ 'received': 'HeadTurnRightEnd', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:253 y:412 OperatableStateMachine.add( 'FakeApologyAndMenace', CompoundAction(t1=[0, 0.0], type1='voice/play_wav', cmd1='no_esli_snova_otkluchish_mikrofon', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='hoof_knock', t3=[2, 0.0], type3='motion/joint_trajectory', cmd3='head_node', t4=[0, 6.5], type4='motion/joint_trajectory', cmd4='begone'), transitions={ 'success': 'NormalLook', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:74 y:386 OperatableStateMachine.add( 'WaitKey5', WaitForMessageState( topic=joy_topic, condition=lambda x: x.buttons[action_button], buffered=False, clear=False), transitions={ 'received': 'LetsBegin', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:48 y:277 OperatableStateMachine.add('LetsBegin', CompoundAction( t1=[0, 0.0], type1='voice/play_wav', cmd1='nam_pora_nachinat', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='head_node', t3=[1, 1.0], type3='motion/joint_trajectory', cmd3='point_right', t4=[0, 7.0], type4='motion/joint_trajectory', cmd4='hoof_stamp'), transitions={ 'success': 'finished', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:439 y:35 OperatableStateMachine.add( 'WaitKey2', WaitForMessageState( topic=joy_topic, condition=lambda x: x.buttons[action_button], buffered=False, clear=False), transitions={ 'received': 'FakeApoplogy', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:236 y:35 OperatableStateMachine.add('HelloEvil', CompoundAction( t1=[0, 0.0], type1='voice/play_wav', cmd1='zdravstvuyte_kojannie_meshki', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='greeting', t3=[0, 0.0], type3='eyes/emotion', cmd3='red_eyes', t4=[0, 5.5], type4='motion/joint_trajectory', cmd4='applause'), transitions={ 'success': 'WaitKey2', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:243 y:129 OperatableStateMachine.add('PlaceHead', SrdfStateToMoveit( config_name='head_basic', move_group='head', action_topic='move_group', robot_name=''), transitions={ 'reached': 'HelloEvil', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name', 'move_group': 'move_group', 'robot_name': 'robot_name', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) # x:73 y:124 OperatableStateMachine.add( 'WaitStart', _sm_waitstart_0, transitions={ 'finished': 'PlaceHead', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rand_head_config': 'rand_head_config'}) # x:642 y:365 OperatableStateMachine.add('MicrophoneTurnOffReaction', CompoundAction( t1=[0, 0.0], type1='eyes/emotion', cmd1='evil_look', t2=[0, 0.0], type2='motion/joint_trajectory', cmd2='head_turn_right_begin', t3=[2, 0.0], type3='motion/joint_trajectory', cmd3='hoof_stamp', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'WaitKey4', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:231 y:312 OperatableStateMachine.add('NormalLook', CompoundAction(t1=[0, 0.0], type1='eyes/emotion', cmd1='normal', t2=[1, 0.0], type2='eyes/emotion', cmd2='red_eyes', t3=[0, 0.0], type3=None, cmd3='', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'WaitKey5', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:452 y:401 OperatableStateMachine.add('HeadTurnRightEnd', CompoundAction( t1=[0, 0.0], type1='motion/joint_trajectory', cmd1='head_turn_right_end', t2=[0, 0.0], type2=None, cmd2='', t3=[0, 0.0], type3=None, cmd3='', t4=[0, 0.0], type4=None, cmd4=''), transitions={ 'success': 'FakeApologyAndMenace', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'failure': Autonomy.Off }) # x:628 y:165 OperatableStateMachine.add( 'WaitKey3', WaitForMessageState( topic=joy_topic, condition=lambda x: x.buttons[action_button], buffered=False, clear=False), transitions={ 'received': 'HelloGoodThenEvil', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) return _state_machine
def create(self): # x:30 y:353, x:488 y:318 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.unused = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:189 y:30 OperatableStateMachine.add( 'CheckCrouched', CheckJointState(outcomes=['body_crouched', 'unknown'], pose_ns='saved_msgs/joint_state', tolerance=0.17, joint_topic="joint_states", timeout=1.0), transitions={ 'body_crouched': 'StandUp', 'unknown': 'RandHeadMovements' }, autonomy={ 'body_crouched': Autonomy.Off, 'unknown': Autonomy.Off }) # x:196 y:256 OperatableStateMachine.add( 'HeadNominal', SetJointState(controller='motion/controller/joint_state_head', pose_param='head_nominal', pose_ns='saved_msgs/joint_state', tolerance=0.017, timeout=10.0, joint_topic="joint_states"), transitions={ 'done': 'finished', 'failed': 'failed', 'timeout': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off, 'timeout': Autonomy.Off }) # x:445 y:78 OperatableStateMachine.add( 'StandUp', ExecuteJointTrajectory( action_topic='motion/controller/joint_trajectory', trajectory_param='crouch_end', trajectory_ns='saved_msgs/joint_trajectory'), transitions={ 'success': 'RandHeadMovements', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }) # x:176 y:144 OperatableStateMachine.add('RandHeadMovements', SweetieBotRandHeadMovements( controller='joint_state_head', duration=self.timeout, interval=[1, 4], max2356=[0.3, 0.3, 1.5, 1.5], min2356=[-0.3, -0.3, -1.5, -1.5]), transitions={ 'done': 'HeadNominal', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'config': 'unused'}) return _state_machine
def create(self): # x:98 y:490, x:1089 y:260 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.be_evil = False _state_machine.userdata.key_pressed_msg = KeyPressed() _state_machine.userdata.config = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:353, x:558 y:270, x:472 y:279, x:284 y:286, x:430 y:353, x:530 y:353, x:630 y:353 _sm_waitkeypressed_0 = ConcurrencyContainer(outcomes=['pressed', 'timeout', 'failed'], input_keys=['key_pressed_msg', 'config'], output_keys=['key_pressed_msg'], conditions=[ ('failed', [('RandMovements', 'failed')]), ('timeout', [('RandMovements', 'done')]), ('pressed', [('WaitKey', 'received')]), ('failed', [('WaitKey', 'unavailable')]) ]) with _sm_waitkeypressed_0: # x:70 y:100 OperatableStateMachine.add('WaitKey', WaitForMessageState(topic='/hmi/joy_decoder/keys_pressed', condition=lambda x: x.keys != [], buffered=False, clear=True), transitions={'received': 'pressed', 'unavailable': 'failed'}, autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off}, remapping={'message': 'key_pressed_msg'}) # x:423 y:91 OperatableStateMachine.add('RandMovements', SweetieBotRandHeadMovements(controller='joint_state_head', duration=self.timeout, interval=[3,5], max2356=[0.3,0.3,1.5,1.5], min2356=[-0.3,-0.3,-1.5,-1.5]), transitions={'done': 'timeout', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'config': 'config'}) with _state_machine: # x:217 y:310 OperatableStateMachine.add('SetHeadPoseNominal', SetJointState(controller='motion/controller/joint_state_head', pose_param='nominal', pose_ns='saved_msgs/joint_state', tolerance=0.017, timeout=10.0, joint_topic="joint_states"), transitions={'done': 'WaitKeyPressed', 'failed': 'failed', 'timeout': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off, 'timeout': Autonomy.Off}) # x:662 y:48 OperatableStateMachine.add('JoyAnimation', self.use_behavior(JoyAnimationSM, 'JoyAnimation'), transitions={'timeout': 'SetHeadPoseNominal', 'failed': 'failed', 'unknown_keys': 'JoyWalk'}, autonomy={'timeout': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'unknown_keys': Autonomy.Inherit}, remapping={'key_pressed_msg': 'key_pressed_msg', 'be_evil': 'be_evil'}) # x:841 y:220 OperatableStateMachine.add('JoyWalk', self.use_behavior(JoyWalkSM, 'JoyWalk'), transitions={'timeout': 'SetHeadPoseNominal', 'failed': 'failed', 'invalid_pose': 'failed', 'unknown_keys': 'CheckKeys'}, autonomy={'timeout': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'invalid_pose': Autonomy.Inherit, 'unknown_keys': Autonomy.Inherit}, remapping={'key_pressed_msg': 'key_pressed_msg'}) # x:641 y:329 OperatableStateMachine.add('CheckKeys', DecisionState(outcomes=['walk','animation','start', 'unknown'], conditions=self.decision), transitions={'walk': 'JoyWalk', 'animation': 'JoyAnimation', 'start': 'RandomChoice', 'unknown': 'WaitKeyPressed'}, autonomy={'walk': Autonomy.Off, 'animation': Autonomy.Off, 'start': Autonomy.Off, 'unknown': Autonomy.Off}, remapping={'input_value': 'key_pressed_msg'}) # x:443 y:565 OperatableStateMachine.add('AutonomousBehavior2', self.use_behavior(AutonomousBehavior2SM, 'AutonomousBehavior2'), transitions={'finished': 'SetHeadPoseNominal', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:194 y:135 OperatableStateMachine.add('WaitKeyPressed', _sm_waitkeypressed_0, transitions={'pressed': 'JoyAnimation', 'timeout': 'RandomChoice', 'failed': 'failed'}, autonomy={'pressed': Autonomy.Inherit, 'timeout': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'key_pressed_msg': 'key_pressed_msg', 'config': 'config'}) # x:545 y:431 OperatableStateMachine.add('RandomChoice', DecisionState(outcomes=['one','two'], conditions=lambda x: 'one' if random.random()<0.0 else 'two'), transitions={'one': 'WaitKeyPressed', 'two': 'WaitKeyPressed'}, autonomy={'one': Autonomy.Off, 'two': Autonomy.Off}, remapping={'input_value': 'config'}) # x:647 y:599 OperatableStateMachine.add('AutonomousBehavior', self.use_behavior(AutonomousBehaviorSM, 'AutonomousBehavior'), transitions={'finished': 'SetHeadPoseNominal', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) return _state_machine