def create(self): # x:30 y:365, x:150 y:452 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:283 y:40 OperatableStateMachine.add('say', Talk_Last_Response(target_time=1), transitions={'continue': 'sub', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:457 y:297 OperatableStateMachine.add('logfalse', LogState(text="false", severity=Logger.REPORT_HINT), transitions={'done': 'sub'}, autonomy={'done': Autonomy.Off}) # x:391 y:168 OperatableStateMachine.add('unavailable_topic_string_log', LogState(text="the topic is not available so finishing", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:973 y:355 OperatableStateMachine.add('takephoto', TakePhotoState(), transitions={'continue': 'move_lips', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:975 y:444 OperatableStateMachine.add('move_lips', MoveRobotLipsState(target_time=0), transitions={'continue': 'finished', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:776 y:71 OperatableStateMachine.add('sub', SubscriberState(topic='/stt_topic', blocking=True, clear=False), transitions={'received': 'chyeck', 'unavailable': 'unavailable_topic_string_log'}, autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off}, remapping={'message': 'message'}) # x:778 y:303 OperatableStateMachine.add('chyeck', CheckConditionState(predicate=lambda message: message.query == "take photo"), transitions={'true': 'takephoto', 'false': 'logfalse'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'message'}) return _state_machine
def create(self): hello = "Hello World!" # x:30 y:365, x:130 y:365 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:129 y:81 OperatableStateMachine.add('Initial_Wait', WaitState(wait_time=self.waiting_time), transitions={'done': 'Hello_Greeting'}, autonomy={'done': Autonomy.Off}) # x:130 y:215 OperatableStateMachine.add('Hello_Greeting', LogState(text=hello, severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.High}) return _state_machine
def create(self): log_msg = "Hello World!" # x:83 y:390 _state_machine = OperatableStateMachine(outcomes=['finished']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:52 y:78 OperatableStateMachine.add( 'Print_Message', LogState(text=log_msg, severity=Logger.REPORT_HINT), transitions={'done': 'Wait_After_Logging'}, autonomy={'done': Autonomy.Low}) # x:40 y:228 OperatableStateMachine.add('Wait_After_Logging', WaitState(wait_time=self.waiting_time), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:83 y:340, x:283 y:190 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:60 y:78 OperatableStateMachine.add( 'Log_Sweep', LogState(text='Now performing a short sweep...', severity=Logger.REPORT_INFO), transitions={'done': 'Perform_Sweep'}, autonomy={'done': Autonomy.Off}) # x:47 y:178 OperatableStateMachine.add('Perform_Sweep', MetricSweepState(sweep_type='short'), transitions={ 'sweeped': 'finished', 'failed': 'failed' }, autonomy={ 'sweeped': Autonomy.Off, 'failed': Autonomy.Off }) return _state_machine
def create(self): hello = "hello world" # x:248 y:340, x:429 y:201 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:182 y:32 OperatableStateMachine.add('Initial_Wait', WaitState(wait_time=self.waiting_time), transitions={'done': 'Print_Greeting'}, autonomy={'done': Autonomy.Off}) # x:176 y:186 OperatableStateMachine.add('Print_Greeting', LogState(text=hello, severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.High}) return _state_machine
def create(self): # x:909 y:103, x:830 y:367 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:105 y:53 OperatableStateMachine.add('Entry', LogState(text="Entering the create twist demo", severity=Logger.REPORT_HINT), transitions={'done': 'Twist1'}, autonomy={'done': Autonomy.Off}) # x:577 y:345 OperatableStateMachine.add('EmergencyStop', CreateTimedStopState(timeout=0.2, cmd_topic='/create_node/cmd_vel', odom_topic='/create_node/odom'), transitions={'done': 'failed', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}) # x:577 y:145 OperatableStateMachine.add('Stop', CreateTimedStopState(timeout=0.24, cmd_topic='/create_node/cmd_vel', odom_topic='/create_node/odom'), transitions={'done': 'Exit', 'failed': 'EmergencyStop'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}) # x:756 y:65 OperatableStateMachine.add('Exit', LogState(text="Exiting the create twist demo", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.High}) # x:73 y:166 OperatableStateMachine.add('Twist1', CreateTimedTwistState(target_time=10.0, velocity=0.25, rotation_rate=0.05, cmd_topic='/create_node/cmd_vel', sensor_topic='/create_node/sensor_state'), transitions={'done': 'Stop', 'failed': 'EmergencyStop'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Off}) return _state_machine
def create(self): hello = "Hello World" # x:30 y:365, x:332 y:473 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:131 y:60 OperatableStateMachine.add( 'waiter', WaitState(wait_time=self.waiting_time), transitions={'done': 'listen for input'}, autonomy={'done': Autonomy.Off}) # x:633 y:55 OperatableStateMachine.add('printer', LogState(text=hello, severity=Logger.REPORT_HINT), transitions={'done': 'phototaker'}, autonomy={'done': Autonomy.Off}) # x:567 y:320 OperatableStateMachine.add('phototaker', TakePhotoState(), transitions={ 'continue': 'finished', 'failed': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }) # x:376 y:48 OperatableStateMachine.add('listen for input', ListeningState(), transitions={ 'continue': 'printer', 'failed': 'failed' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }) return _state_machine
def create(self): hello = "baka!!" # x:30 y:365, x:130 y:365 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.target_joint = [0, 0, 0] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add('Initial_Wait', WaitState(wait_time=self.waiting_time), transitions={'done': 'test_moveit'}, autonomy={'done': Autonomy.Off}) # x:27 y:171 OperatableStateMachine.add('Print_Greeting', LogState(text=hello, severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.High}) # x:172 y:148 OperatableStateMachine.add( 'test_moveit', MoveitToJointsState(move_group='Gantry', joint_names=[ "small_long_joint", "torso_rail_joint", "torso_base_main_joint" ], action_topic='/ariac/gantry/move_group'), transitions={ 'reached': 'Print_Greeting', 'planning_failed': 'failed', 'control_failed': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off }, remapping={'joint_config': 'target_joint'}) return _state_machine
def create(self): # x:255 y:192, x:130 y:324 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:72 y:118 OperatableStateMachine.add('log', LogState(text="test", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:30 y:365 _state_machine = OperatableStateMachine(outcomes=['finished']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:66 y:154 OperatableStateMachine.add('Print_Msg', LogState(text="My param is: " + self.msg, severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:682 y:306, x:452 y:252 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['pos']) _state_machine.userdata.pos = {"x": 0.8, "y": -0.2, "z": 1} # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458, x:530 y:458, x:630 y:458, x:59 y:533, x:830 y:458 _sm_group_0 = ConcurrencyContainer( outcomes=['threshold', 'watchdog', 'fail'], conditions=[('threshold', [('read', 'threshold')]), ('watchdog', [('read', 'watchdog')]), ('fail', [('read', 'fail')]), ('threshold', [('read yaw', 'threshold')]), ('fail', [('read yaw', 'fail')]), ('watchdog', [('read yaw', 'watchdog')])]) with _sm_group_0: # x:86 y:125 OperatableStateMachine.add('read', ReadTorque( watchdog=1, Joint="right_elbow_pitch_joint", Threshold=0.7, min_time=0.4), transitions={ 'threshold': 'threshold', 'watchdog': 'watchdog', 'fail': 'fail' }, autonomy={ 'threshold': Autonomy.Off, 'watchdog': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'torque': 'torque'}) # x:252 y:135 OperatableStateMachine.add('read yaw', ReadTorque( watchdog=1, Joint="right_elbow_pitch_joint", Threshold=0.5, min_time=0.4), transitions={ 'threshold': 'threshold', 'watchdog': 'watchdog', 'fail': 'fail' }, autonomy={ 'threshold': Autonomy.Off, 'watchdog': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'torque': 'torque'}) # x:30 y:458 _sm_read_torque_1 = OperatableStateMachine(outcomes=['done']) with _sm_read_torque_1: # x:142 y:61 OperatableStateMachine.add('log', LogState(text="going down", severity=Logger.REPORT_HINT), transitions={'done': 'Group'}, autonomy={'done': Autonomy.Off}) # x:131 y:164 OperatableStateMachine.add('Group', _sm_group_0, transitions={ 'threshold': 'done', 'watchdog': 'log', 'fail': 'done' }, autonomy={ 'threshold': Autonomy.Inherit, 'watchdog': Autonomy.Inherit, 'fail': Autonomy.Inherit }) # x:30 y:458 _sm_go_down_2 = OperatableStateMachine(outcomes=['done'], input_keys=['GripPose']) with _sm_go_down_2: # x:126 y:194 OperatableStateMachine.add('movePlace', MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15), transitions={ 'done': 'done', 'failed': 'done' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'GripPose'}) # x:30 y:324, x:130 y:324 _sm_retreate_arm_3 = OperatableStateMachine( outcomes=['failed', 'done'], input_keys=['approach_pose', 'PreGripPose']) with _sm_retreate_arm_3: # x:30 y:40 OperatableStateMachine.add('ReturnApproachPose', MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15), transitions={ 'done': 'ReturnPreGrip', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'approach_pose'}) # x:202 y:41 OperatableStateMachine.add('ReturnPreGrip', MoveitMove(move=True, waitForExecution=False, group="RightArm", watchdog=15), transitions={ 'done': 'done', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'PreGripPose'}) # x:30 y:324, x:130 y:324 _sm_releasing_4 = OperatableStateMachine( outcomes=['object', 'no_object']) with _sm_releasing_4: # x:30 y:40 OperatableStateMachine.add('say touchdown', SaraSay(sentence="Touchdown!", input_keys=[], emotion=1, block=False), transitions={'done': 'open gripper'}, autonomy={'done': Autonomy.Off}) # x:139 y:176 OperatableStateMachine.add( 'open gripper', SetGripperState(width=0.14, effort=1), transitions={ 'object': 'object', 'no_object': 'no_object' }, autonomy={ 'object': Autonomy.Off, 'no_object': Autonomy.Off }, remapping={'object_size': 'object_size'}) # x:30 y:324, x:130 y:324 _sm_moveback_5 = OperatableStateMachine(outcomes=['arrived', 'failed']) with _sm_moveback_5: # x:30 y:40 OperatableStateMachine.add('genpose', GenPoseEuler(x=-0.3, y=-0.3, z=0, roll=0, pitch=0, yaw=0), transitions={'done': 'move back'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'backPose'}) # x:40 y:163 OperatableStateMachine.add('move back', SaraMoveBase(reference="base_link"), transitions={ 'arrived': 'arrived', 'failed': 'failed' }, autonomy={ 'arrived': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'pose': 'backPose'}) # x:536 y:72, x:231 y:292 _sm_prepare_grip_6 = OperatableStateMachine( outcomes=['failed', 'done'], input_keys=['pos'], output_keys=['approach_pose', 'grip_pose']) with _sm_prepare_grip_6: # x:50 y:40 OperatableStateMachine.add('Gen place_pos', GenGripperPose(l=0, z=-0.05, planar=True), transitions={ 'done': 'Gen approach_pos', 'fail': 'failed' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'pose_in': 'pos', 'pose_out': 'grip_pose' }) # x:30 y:176 OperatableStateMachine.add('MoveIt_isReachable', MoveitMove(move=False, waitForExecution=True, group="RightArm", watchdog=15), transitions={ 'done': 'log app', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'grip_pose'}) # x:37 y:108 OperatableStateMachine.add('Gen approach_pos', GenGripperPose(l=0.0, z=0.20, planar=True), transitions={ 'done': 'log place pos', 'fail': 'failed' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'pose_in': 'pos', 'pose_out': 'approach_pose' }) # x:41 y:269 OperatableStateMachine.add('log app', LogKeyState( text="{}", severity=Logger.REPORT_HINT), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'approach_pose'}) # x:360 y:180 OperatableStateMachine.add( 'log place pos', LogKeyState(text="place pose is {}", severity=Logger.REPORT_HINT), transitions={'done': 'MoveIt_isReachable'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'grip_pose'}) # x:30 y:324, x:130 y:324 _sm_pregrippose_7 = OperatableStateMachine(outcomes=['done', 'failed'], output_keys=['PreGripPose']) with _sm_pregrippose_7: # x:30 y:40 OperatableStateMachine.add('setPreGripPose', SetKey(Value="PrePlacePose"), transitions={'done': 'gotoPreGrip'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'PreGripPose'}) # x:32 y:106 OperatableStateMachine.add('gotoPreGrip', MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15), transitions={ 'done': 'done', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'PreGripPose'}) # x:30 y:458, x:130 y:458, x:230 y:458 _sm_get_down_8 = ConcurrencyContainer( outcomes=['done'], input_keys=['GripPose'], conditions=[('done', [('Go down', 'done')]), ('done', [('read torque', 'done')])]) with _sm_get_down_8: # x:178 y:127 OperatableStateMachine.add('Go down', _sm_go_down_2, transitions={'done': 'done'}, autonomy={'done': Autonomy.Inherit}, remapping={'GripPose': 'GripPose'}) # x:405 y:150 OperatableStateMachine.add('read torque', _sm_read_torque_1, transitions={'done': 'done'}, autonomy={'done': Autonomy.Inherit}) # x:30 y:324, x:130 y:324 _sm_pretraitement_9 = OperatableStateMachine(outcomes=['fail', 'done'], input_keys=['pos'], output_keys=['pos']) with _sm_pretraitement_9: # x:30 y:40 OperatableStateMachine.add('TF_transformation', TF_transformation(in_ref="map", out_ref="base_link"), transitions={ 'done': 'LOG POSE', 'fail': 'fail' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'in_pos': 'pos', 'out_pos': 'pos' }) # x:33 y:107 OperatableStateMachine.add('LOG POSE', LogKeyState( text="{}", severity=Logger.REPORT_HINT), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'pos'}) with _state_machine: # x:148 y:34 OperatableStateMachine.add('Pretraitement', _sm_pretraitement_9, transitions={ 'fail': 'failed', 'done': 'PreGripPose' }, autonomy={ 'fail': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={'pos': 'pos'}) # x:634 y:410 OperatableStateMachine.add( 'close gripper', SetGripperState(width=0, effort=1), transitions={ 'object': 'finished', 'no_object': 'finished' }, autonomy={ 'object': Autonomy.Off, 'no_object': Autonomy.Off }, remapping={'object_size': 'object_size'}) # x:141 y:522 OperatableStateMachine.add('Get_down', _sm_get_down_8, transitions={'done': 'releasing'}, autonomy={'done': Autonomy.Inherit}, remapping={'GripPose': 'grip_pose'}) # x:159 y:352 OperatableStateMachine.add('look down', SaraSetHeadAngle(pitch=0.6, yaw=-0.3), transitions={'done': 'Move_approach'}, autonomy={'done': Autonomy.Off}) # x:160 y:127 OperatableStateMachine.add( 'PreGripPose', _sm_pregrippose_7, transitions={ 'done': 'Prepare grip', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'PreGripPose': 'PreGripPose'}) # x:156 y:238 OperatableStateMachine.add('Prepare grip', _sm_prepare_grip_6, transitions={ 'failed': 'failed', 'done': 'look down' }, autonomy={ 'failed': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'pos': 'pos', 'approach_pose': 'approach_pose', 'grip_pose': 'grip_pose' }) # x:139 y:444 OperatableStateMachine.add('Move_approach', MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15), transitions={ 'done': 'Get_down', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'approach_pose'}) # x:623 y:525 OperatableStateMachine.add('Moveback', _sm_moveback_5, transitions={ 'arrived': 'close gripper', 'failed': 'failed' }, autonomy={ 'arrived': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:298 y:520 OperatableStateMachine.add('releasing', _sm_releasing_4, transitions={ 'object': 'retreate arm', 'no_object': 'retreate arm' }, autonomy={ 'object': Autonomy.Inherit, 'no_object': Autonomy.Inherit }) # x:459 y:522 OperatableStateMachine.add('retreate arm', _sm_retreate_arm_3, transitions={ 'failed': 'failed', 'done': 'Moveback' }, autonomy={ 'failed': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'approach_pose': 'approach_pose', 'PreGripPose': 'PreGripPose' }) return _state_machine
def create(self): # x:30 y:365, x:242 y:161 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:241 y:23 OperatableStateMachine.add('log', LogState( text="logged to get started :-) ", severity=Logger.REPORT_HINT), transitions={'done': 'listennow'}, autonomy={'done': Autonomy.Off}) # x:936 y:282 OperatableStateMachine.add('wait4', WaitState(wait_time=3), transitions={'done': 'sub_is_listen2'}, autonomy={'done': Autonomy.Off}) # x:913 y:34 OperatableStateMachine.add('sub_is_listening', SubscriberState( topic='/is_robot_listening', blocking=True, clear=False), transitions={ 'received': 'check listen or not', 'unavailable': 'check listen or not' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:1122 y:153 OperatableStateMachine.add( 'check listen or not', CheckConditionState( predicate=lambda message: message.data == "not listening"), transitions={ 'true': 'listen2', 'false': 'sub_is_listening' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'message'}) # x:709 y:49 OperatableStateMachine.add( 'wait2', WaitState(wait_time=1), transitions={'done': 'sub_is_listening'}, autonomy={'done': Autonomy.Off}) # x:522 y:205 OperatableStateMachine.add( 'chk', CheckConditionState( predicate=lambda message: message.data == "not listening"), transitions={ 'true': 'fin', 'false': 'sub_is_listen2' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'message'}) # x:328 y:237 OperatableStateMachine.add('fin', LogState(text="finished interaction", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:460 y:24 OperatableStateMachine.add('listennow', ListeningState(), transitions={ 'continue': 'wait2', 'failed': 'wait2' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }) # x:1048 y:421 OperatableStateMachine.add('listen2', ListeningState(), transitions={ 'continue': 'wait4', 'failed': 'wait4' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }) # x:730 y:302 OperatableStateMachine.add('sub_is_listen2', SubscriberState( topic='/is_robot_listening', blocking=True, clear=False), transitions={ 'received': 'chk', 'unavailable': 'chk' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) return _state_machine
def create(self): # x:923 y:114 _state_machine = OperatableStateMachine(outcomes=['finished']) _state_machine.userdata.none = None _state_machine.userdata.do_turn_torso = False _state_machine.userdata.pushing_side = 'right' if self.hand_side == 'left' else 'left' _state_machine.userdata.torso_side = self.hand_side # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:333 y:440, x:433 y:240 _sm_opening_motion_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['do_turn_torso', 'pushing_side', 'none', 'torso_side']) with _sm_opening_motion_0: # x:438 y:28 OperatableStateMachine.add( 'Branch_Torso_Open', DecisionState(outcomes=['turn', 'fixed'], conditions=lambda x: 'turn' if x else 'fixed'), transitions={ 'turn': 'Turn_Torso', 'fixed': 'Go_To_Open_Door_Pose_Straight' }, autonomy={ 'turn': Autonomy.Low, 'fixed': Autonomy.Low }, remapping={'input_value': 'do_turn_torso'}) # x:57 y:278 OperatableStateMachine.add( 'Go_To_Open_Door_Pose_Turned', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState. DOOR_OPEN_POSE_TURNED, vel_scaling=0.05, ignore_collisions=True, link_paddings={}), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.High }, remapping={'side': 'pushing_side'}) # x:705 y:228 OperatableStateMachine.add( 'Go_To_Open_Door_Pose_Straight', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState. DOOR_OPEN_POSE_STRAIGHT, vel_scaling=0.05, ignore_collisions=True, link_paddings={}), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.High }, remapping={'side': 'pushing_side'}) # x:76 y:128 OperatableStateMachine.add( 'Turn_Torso', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.TURN_TORSO_FULL, vel_scaling=0.05, ignore_collisions=False, link_paddings={}), transitions={ 'done': 'Go_To_Open_Door_Pose_Turned', 'failed': 'Log_No_Turn' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.High }, remapping={'side': 'torso_side'}) # x:484 y:128 OperatableStateMachine.add( 'Set_Turn_Torso_False', CalculationState(calculation=lambda x: False), transitions={'done': 'Go_To_Open_Door_Pose_Straight'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'none', 'output_value': 'do_turn_torso' }) # x:305 y:128 OperatableStateMachine.add( 'Log_No_Turn', LogState(text="Skip turning because of collision", severity=Logger.REPORT_INFO), transitions={'done': 'Set_Turn_Torso_False'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:287 y:78 OperatableStateMachine.add( 'Decide_If_Turn', OperatorDecisionState(outcomes=['turn_torso', 'fixed_torso'], hint=None, suggestion=None), transitions={ 'turn_torso': 'Decide_Go_To_Stand', 'fixed_torso': 'Go_To_Door_Ready_Pose' }, autonomy={ 'turn_torso': Autonomy.Full, 'fixed_torso': Autonomy.Full }) # x:125 y:478 OperatableStateMachine.add( 'Go_To_Door_Ready_Pose', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE, vel_scaling=0.2, ignore_collisions=False, link_paddings={}), transitions={ 'done': 'Opening_Motion', 'failed': 'Decide_No_Collision_Avoidance' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.High }, remapping={'side': 'pushing_side'}) # x:887 y:378 OperatableStateMachine.add('Ask_If_Open', OperatorDecisionState( outcomes=['open', 'push_again'], hint='Is the door open?', suggestion='open'), transitions={ 'open': 'Go_To_Final_Stand', 'push_again': 'Log_Try_Push_Again' }, autonomy={ 'open': Autonomy.High, 'push_again': Autonomy.Full }) # x:521 y:82 OperatableStateMachine.add('Log_Try_Push_Again', LogState( text='Move robot closer to door', severity=Logger.REPORT_HINT), transitions={'done': 'Decide_If_Turn'}, autonomy={'done': Autonomy.Full}) # x:620 y:628 OperatableStateMachine.add( 'Back_To_Door_Ready_Pose', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}), transitions={ 'done': 'Branch_Torso_Retract', 'failed': 'Open_Manually' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.High }, remapping={'side': 'pushing_side'}) # x:136 y:328 OperatableStateMachine.add( 'Set_Turn_Torso_True', CalculationState(calculation=lambda x: True), transitions={'done': 'Go_To_Door_Ready_Pose'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'none', 'output_value': 'do_turn_torso' }) # x:186 y:622 OperatableStateMachine.add('Opening_Motion', _sm_opening_motion_0, transitions={ 'finished': 'Back_To_Door_Ready_Pose', 'failed': 'Open_Manually' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'do_turn_torso': 'do_turn_torso', 'pushing_side': 'pushing_side', 'none': 'none', 'torso_side': 'torso_side' }) # x:893 y:201 OperatableStateMachine.add( 'Go_To_Final_Stand', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.2, ignore_collisions=False, link_paddings={}), transitions={ 'done': 'finished', 'failed': 'finished' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.High }, remapping={'side': 'none'}) # x:826 y:528 OperatableStateMachine.add( 'Turn_Torso_Center_Pose', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState. TURN_TORSO_CENTER_POSE, vel_scaling=0.05, ignore_collisions=False, link_paddings={}), transitions={ 'done': 'Ask_If_Open', 'failed': 'Open_Manually' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.High }, remapping={'side': 'none'}) # x:1034 y:628 OperatableStateMachine.add( 'Branch_Torso_Retract', DecisionState(outcomes=['turn', 'fixed'], conditions=lambda x: 'turn' if x else 'fixed'), transitions={ 'turn': 'Turn_Torso_Center_Pose', 'fixed': 'Ask_If_Open' }, autonomy={ 'turn': Autonomy.Low, 'fixed': Autonomy.Low }, remapping={'input_value': 'do_turn_torso'}) # x:26 y:278 OperatableStateMachine.add( 'Go_To_Stand', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.2, ignore_collisions=False, link_paddings={}), transitions={ 'done': 'Set_Turn_Torso_True', 'failed': 'Go_To_Door_Ready_Pose' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.High }, remapping={'side': 'none'}) # x:136 y:128 OperatableStateMachine.add( 'Decide_Go_To_Stand', DecisionState(outcomes=['stand', 'skip'], conditions=lambda x: 'stand' if not x else 'skip'), transitions={ 'stand': 'Check_Hand_Space', 'skip': 'Set_Turn_Torso_True' }, autonomy={ 'stand': Autonomy.Low, 'skip': Autonomy.Low }, remapping={'input_value': 'do_turn_torso'}) # x:360 y:378 OperatableStateMachine.add( 'Decide_No_Collision_Avoidance', OperatorDecisionState(outcomes=[ 'replan_ignore_collisions', 'continue', 'open_manual' ], hint="Try again and ignore collisions?", suggestion='replan_ignore_collisions'), transitions={ 'replan_ignore_collisions': 'Go_To_Door_Ready_Pose_NC', 'continue': 'Opening_Motion', 'open_manual': 'Open_Manually' }, autonomy={ 'replan_ignore_collisions': Autonomy.Full, 'continue': Autonomy.Full, 'open_manual': Autonomy.Full }) # x:365 y:528 OperatableStateMachine.add( 'Go_To_Door_Ready_Pose_NC', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE, vel_scaling=0.3, ignore_collisions=True, link_paddings={}), transitions={ 'done': 'Opening_Motion', 'failed': 'Open_Manually' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.High }, remapping={'side': 'pushing_side'}) # x:651 y:528 OperatableStateMachine.add('Open_Manually', LogState(text="Open door manually", severity=Logger.REPORT_HINT), transitions={'done': 'Ask_If_Open'}, autonomy={'done': Autonomy.Full}) # x:40 y:178 OperatableStateMachine.add( 'Check_Hand_Space', LogState( text="Make sure the hands have enough space to the door", severity=Logger.REPORT_HINT), transitions={'done': 'Go_To_Stand'}, autonomy={'done': Autonomy.Full}) return _state_machine
def create(self): POINT_LOCATION = 0 SELECTED_OBJECT_ID = 1 WAYPOINT_GOAL_POSE = 2 GHOST_JOINT_STATES = 3 concurrent_states = dict() concurrent_mapping = list() type1 = POINT_LOCATION type2 = SELECTED_OBJECT_ID # x:717 y:559, x:344 y:460 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] concurrent_states['input1'] = InputState( request=type1, message="Please give me data 1...") concurrent_states['input2'] = InputState( request=type2, message="Please give me data 2...") concurrent_states['calc'] = CalculationState(lambda x: 0) concurrent_mapping.append({ 'outcome': 'done', 'condition': { 'input1': 'received', 'input2': 'received' } }) # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add( 'Request_Data', InputState(request=SELECTED_OBJECT_ID, message="Please give me some data..."), transitions={ 'received': 'Log_Received', 'aborted': 'Log_Aborted', 'no_connection': 'Log_No_Connection', 'data_error': 'Log_Data_Error' }, autonomy={ 'received': Autonomy.High, 'aborted': Autonomy.High, 'no_connection': Autonomy.High, 'data_error': Autonomy.High }, remapping={'data': 'data'}) # x:30 y:247 OperatableStateMachine.add('Log_Received', LogState(text="Got behavior data!", severity=Logger.REPORT_INFO), transitions={'done': 'Print_Result'}, autonomy={'done': Autonomy.Off}) # x:324 y:255 OperatableStateMachine.add( 'Log_Aborted', LogState(text="Operator aborted data request.", severity=Logger.REPORT_WARN), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:489 y:241 OperatableStateMachine.add('Log_No_Connection', LogState( text="Unable to connect to OCS!", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:158 y:243 OperatableStateMachine.add('Log_Data_Error', LogState( text="Failed to deserialize data.", severity=Logger.REPORT_ERROR), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:27 y:411 OperatableStateMachine.add( 'Print_Result', CalculationState(calculation=self.print_input_data), transitions={'done': 'Concurrent_Request'}, autonomy={'done': Autonomy.Low}, remapping={ 'input_value': 'data', 'output_value': 'output_value' }) # x:242 y:586 {?input_keys = ['calc_input_value'],?output_keys = ['input1_data', 'input2_data', 'calc_output_value']} OperatableStateMachine.add( 'Concurrent_Request', ConcurrentState(states=concurrent_states, outcomes=['done', 'failed'], outcome_mapping=concurrent_mapping), transitions={ 'done': 'Print_Result_1', 'failed': 'failed' }, autonomy={ 'done': Autonomy.High, 'failed': Autonomy.Full }, remapping={ 'calc_input_value': 'data', 'input1_data': 'input1_data', 'input2_data': 'input2_data', 'calc_output_value': 'calc_output_value' }) # x:520 y:481 OperatableStateMachine.add( 'Print_Result_1', CalculationState(calculation=self.print_input_data), transitions={'done': 'Print_Result_2'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'input1_data', 'output_value': 'output_value' }) # x:495 y:646 OperatableStateMachine.add( 'Print_Result_2', CalculationState(calculation=self.print_input_data), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'input2_data', 'output_value': 'output_value' }) return _state_machine
def create(self): # x:1534 y:100, x:62 y:356 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], output_keys=['ARMidMAIN', 'BothArmsFull', 'BothArmsEMPTY']) _state_machine.userdata.arm_idR = 'right_arm' _state_machine.userdata.arm_idL = 'left_arm' _state_machine.userdata.ARMidMAIN = '' _state_machine.userdata.BothArmsFull = '' _state_machine.userdata.Ja_value = 'Ja' _state_machine.userdata.Nee_value = 'Nee' _state_machine.userdata.BothArmsEMPTY = '' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:70 y:32 OperatableStateMachine.add('CheckGripperRight', CheckGripperattached(), transitions={ 'True': 'CheckGripperLeft', 'False': 'CheckGripperLeft_2', 'invalid_arm_id': 'failed' }, autonomy={ 'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off }, remapping={'arm_id': 'arm_idR'}) # x:69 y:94 OperatableStateMachine.add('CheckGripperLeft', CheckGripperattached(), transitions={ 'True': 'SetBothArmFull', 'False': 'ResetBothArmFull', 'invalid_arm_id': 'failed' }, autonomy={ 'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off }, remapping={'arm_id': 'arm_idL'}) # x:702 y:22 OperatableStateMachine.add('SetArmRight', ReplaceState(), transitions={'done': 'LogRighterarm'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'arm_idR', 'result': 'ARMidMAIN' }) # x:702 y:85 OperatableStateMachine.add('SetArmLeft', ReplaceState(), transitions={'done': 'LogLeftArm'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'arm_idL', 'result': 'ARMidMAIN' }) # x:528 y:142 OperatableStateMachine.add('SetBothArmFull', ReplaceState(), transitions={'done': 'BothArmsFullLOG'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'Ja_value', 'result': 'BothArmsFull' }) # x:888 y:19 OperatableStateMachine.add( 'LogRighterarm', LogState(text="The Right arm has nothing attached to it", severity=Logger.REPORT_HINT), transitions={'done': 'Arm_id is:'}, autonomy={'done': Autonomy.Off}) # x:887 y:77 OperatableStateMachine.add( 'LogLeftArm', LogState(text="The Left arm has nothing attached to it", severity=Logger.REPORT_HINT), transitions={'done': 'Arm_id is:'}, autonomy={'done': Autonomy.Off}) # x:530 y:84 OperatableStateMachine.add('ResetBothArmFull', ReplaceState(), transitions={'done': 'SetArmLeft'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'Nee_value', 'result': 'BothArmsFull' }) # x:530 y:26 OperatableStateMachine.add('ResetBothArmFull_2', ReplaceState(), transitions={'done': 'SetArmRight'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'Nee_value', 'result': 'BothArmsFull' }) # x:1149 y:41 OperatableStateMachine.add('Arm_id is:', LogKeyState( text="The chosen arm_id is:{}", severity=Logger.REPORT_HINT), transitions={'done': 'BothArmsFullLOG'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'ARMidMAIN'}) # x:1365 y:90 OperatableStateMachine.add('BothArmsFullLOG', LogKeyState( text='Zijn beide armen vol: {}', severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'BothArmsFull'}) # x:300 y:28 OperatableStateMachine.add('CheckGripperLeft_2', CheckGripperattached(), transitions={ 'True': 'ResetBothArmFull_2', 'False': 'SetBothArmEMPTY', 'invalid_arm_id': 'failed' }, autonomy={ 'True': Autonomy.Off, 'False': Autonomy.Off, 'invalid_arm_id': Autonomy.Off }, remapping={'arm_id': 'arm_idL'}) # x:319 y:412 OperatableStateMachine.add( 'SetBothArmEMPTY', ReplaceState(), transitions={'done': 'ResetBothArmFull_2'}, autonomy={'done': Autonomy.Off}, remapping={ 'value': 'Ja_value', 'result': 'BothArmsEMPTY' }) return _state_machine
def create(self): pull_affordance = "pull" affordance_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == "left" else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM pull_displacement = 0.3 # meters # x:383 y:840, x:483 y:490 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.none = None _state_machine.userdata.step_back_distance = 1.0 # meters _state_machine.userdata.grasp_preference = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] self._pull_displacement = pull_displacement # [/MANUAL_CREATE] # x:1033 y:40, x:333 y:90, x:1033 y:190 _sm_go_to_grasp_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'again'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference']) with _sm_go_to_grasp_0: # x:33 y:49 OperatableStateMachine.add('Get_Grasp_Info', GetTemplateGraspState(), transitions={'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'Inform_Grasp_Failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low, 'not_available': Autonomy.Low}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'grasp': 'grasp_pose'}) # x:40 y:293 OperatableStateMachine.add('Convert_Waypoints', CalculationState(calculation=lambda msg: [msg.pose]), transitions={'done': 'Plan_To_Grasp'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_pose', 'output_value': 'grasp_waypoints'}) # x:242 y:292 OperatableStateMachine.add('Plan_To_Grasp', PlanEndeffectorCartesianWaypointsState(ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_To_Grasp_Pose', 'incomplete': 'Move_To_Grasp_Pose', 'failed': 'Decide_Which_Grasp'}, autonomy={'planned': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.High}, remapping={'waypoints': 'grasp_waypoints', 'hand': 'hand_side', 'frame_id': 'grasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:494 y:175 OperatableStateMachine.add('Move_To_Grasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Optional_Template_Adjustment', 'failed': 'Decide_Which_Grasp'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:226 y:177 OperatableStateMachine.add('Inform_Grasp_Failed', LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:970 y:294 OperatableStateMachine.add('Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'again'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:41 y:178 OperatableStateMachine.add('Extract_Frame_Id', CalculationState(calculation=lambda pose: pose.header.frame_id), transitions={'done': 'Convert_Waypoints'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_pose', 'output_value': 'grasp_frame_id'}) # x:712 y:78 OperatableStateMachine.add('Optional_Template_Adjustment', OperatorDecisionState(outcomes=["grasp", "pregrasp", "skip"], hint="Consider adjusting the template's pose", suggestion="skip"), transitions={'grasp': 'Get_Grasp_Info', 'pregrasp': 'again', 'skip': 'finished'}, autonomy={'grasp': Autonomy.Full, 'pregrasp': Autonomy.Full, 'skip': Autonomy.High}) # x:754 y:294 OperatableStateMachine.add('Decide_Which_Grasp', OperatorDecisionState(outcomes=["same", "next"], hint='Try the same grasp or the next one?', suggestion='same'), transitions={'same': 'Optional_Template_Adjustment', 'next': 'Increase_Preference_Index'}, autonomy={'same': Autonomy.High, 'next': Autonomy.High}) # x:133 y:390, x:433 y:190, x:983 y:140 _sm_perform_grasp_1 = OperatableStateMachine(outcomes=['finished', 'failed', 'next'], input_keys=['hand_side', 'grasp_preference', 'template_id', 'pregrasp_pose'], output_keys=['grasp_preference']) with _sm_perform_grasp_1: # x:68 y:76 OperatableStateMachine.add('Get_Finger_Configuration', GetTemplateFingerConfigState(), transitions={'done': 'Close_Fingers', 'failed': 'failed', 'not_available': 'Inform_Closing_Failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High, 'not_available': Autonomy.High}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'finger_config': 'finger_config'}) # x:293 y:328 OperatableStateMachine.add('Convert_Waypoints', CalculationState(calculation=lambda msg: [msg.pose]), transitions={'done': 'Plan_Back_To_Pregrasp'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_waypoints'}) # x:496 y:328 OperatableStateMachine.add('Plan_Back_To_Pregrasp', PlanEndeffectorCartesianWaypointsState(ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_Back_To_Pregrasp_Pose', 'incomplete': 'Move_Back_To_Pregrasp_Pose', 'failed': 'failed'}, autonomy={'planned': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Low}, remapping={'waypoints': 'pregrasp_waypoints', 'hand': 'hand_side', 'frame_id': 'pregrasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:662 y:228 OperatableStateMachine.add('Move_Back_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'Increase_Preference_Index', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Low}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:296 y:228 OperatableStateMachine.add('Extract_Frame_Id', CalculationState(calculation=lambda pose: pose.header.frame_id), transitions={'done': 'Convert_Waypoints'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'pregrasp_pose', 'output_value': 'pregrasp_frame_id'}) # x:673 y:128 OperatableStateMachine.add('Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'next'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:81 y:228 OperatableStateMachine.add('Close_Fingers', HandTrajectoryState(hand_type=self.hand_type), transitions={'done': 'finished', 'failed': 'Extract_Frame_Id'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'finger_trajectory': 'finger_config', 'hand_side': 'hand_side'}) # x:490 y:75 OperatableStateMachine.add('Inform_Closing_Failed', LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:733 y:190, x:383 y:40 _sm_go_to_pregrasp_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference', 'pregrasp_pose']) with _sm_go_to_pregrasp_2: # x:27 y:68 OperatableStateMachine.add('Get_Pregrasp_Info', GetTemplatePregraspState(), transitions={'done': 'Plan_To_Pregrasp_Pose', 'failed': 'failed', 'not_available': 'Inform_Pregrasp_Failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High, 'not_available': Autonomy.High}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'pre_grasp': 'pregrasp_pose'}) # x:269 y:153 OperatableStateMachine.add('Inform_Pregrasp_Failed', LogState(text="No grasp choice left!", severity=Logger.REPORT_WARN), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Low}) # x:537 y:228 OperatableStateMachine.add('Move_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={'done': 'finished', 'failed': 'Decide_Which_Pregrasp'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.High}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:25 y:328 OperatableStateMachine.add('Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Get_Pregrasp_Info'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:266 y:228 OperatableStateMachine.add('Plan_To_Pregrasp_Pose', PlanEndeffectorPoseState(ignore_collisions=False, include_torso=False, allowed_collisions=[], planner_id="RRTConnectkConfigDefault"), transitions={'planned': 'Move_To_Pregrasp_Pose', 'failed': 'Decide_Which_Pregrasp'}, autonomy={'planned': Autonomy.Low, 'failed': Autonomy.High}, remapping={'target_pose': 'pregrasp_pose', 'hand': 'hand_side', 'joint_trajectory': 'joint_trajectory'}) # x:266 y:327 OperatableStateMachine.add('Decide_Which_Pregrasp', OperatorDecisionState(outcomes=["same", "next"], hint='Try the same pregrasp or the next one?', suggestion='same'), transitions={'same': 'Get_Pregrasp_Info', 'next': 'Increase_Preference_Index'}, autonomy={'same': Autonomy.High, 'next': Autonomy.High}) # x:30 y:444, x:162 y:478, x:230 y:478 _sm_planning_pipeline_3 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['stand_pose'], output_keys=['plan_header']) with _sm_planning_pipeline_3: # x:34 y:57 OperatableStateMachine.add('Create_Step_Goal', CreateStepGoalState(pose_is_pelvis=True), transitions={'done': 'Plan_To_Waypoint', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'target_pose': 'stand_pose', 'step_goal': 'step_goal'}) # x:553 y:481 OperatableStateMachine.add('Modify_Plan', InputState(request=InputState.FOOTSTEP_PLAN_HEADER, message='Modify plan, VALIDATE, and confirm.'), transitions={'received': 'finished', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'plan_header'}) # x:34 y:484 OperatableStateMachine.add('Plan_To_Waypoint', PlanFootstepsState(mode=self.parameter_set), transitions={'planned': 'Modify_Plan', 'failed': 'Decide_Replan_without_Collision'}, autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'}) # x:139 y:314 OperatableStateMachine.add('Decide_Replan_without_Collision', OperatorDecisionState(outcomes=['replan', 'fail'], hint='Try replanning without collision avoidance.', suggestion='replan'), transitions={'replan': 'Replan_without_Collision', 'fail': 'failed'}, autonomy={'replan': Autonomy.Low, 'fail': Autonomy.Full}) # x:319 y:406 OperatableStateMachine.add('Replan_without_Collision', PlanFootstepsState(mode='drc_step_no_collision'), transitions={'planned': 'Modify_Plan', 'failed': 'failed'}, autonomy={'planned': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'step_goal': 'step_goal', 'plan_header': 'plan_header'}) # x:1103 y:424, x:130 y:478 _sm_grasp_trigger_4 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id', 'grasp_preference']) with _sm_grasp_trigger_4: # x:86 y:72 OperatableStateMachine.add('Go_to_Pregrasp', _sm_go_to_pregrasp_2, transitions={'finished': 'Open_Fingers', 'failed': 'Grasp_Manually'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id', 'pregrasp_pose': 'pregrasp_pose'}) # x:789 y:172 OperatableStateMachine.add('Perform_Grasp', _sm_perform_grasp_1, transitions={'finished': 'finished', 'failed': 'Grasp_Manually', 'next': 'Close_Fingers'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'next': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id', 'pregrasp_pose': 'pregrasp_pose'}) # x:332 y:178 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState(hand_type=self.hand_type, configuration=0.0), transitions={'done': 'Go_to_Grasp', 'failed': 'Grasp_Manually'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.High}, remapping={'hand_side': 'hand_side'}) # x:332 y:28 OperatableStateMachine.add('Close_Fingers', FingerConfigurationState(hand_type=self.hand_type, configuration=1.0), transitions={'done': 'Go_to_Pregrasp', 'failed': 'Grasp_Manually'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.High}, remapping={'hand_side': 'hand_side'}) # x:324 y:428 OperatableStateMachine.add('Grasp_Manually', OperatorDecisionState(outcomes=["fingers_closed", "abort"], hint="Grasp the object manually, continue when fingers are closed.", suggestion=None), transitions={'fingers_closed': 'finished', 'abort': 'failed'}, autonomy={'fingers_closed': Autonomy.Full, 'abort': Autonomy.Full}) # x:543 y:172 OperatableStateMachine.add('Go_to_Grasp', _sm_go_to_grasp_0, transitions={'finished': 'Perform_Grasp', 'failed': 'Grasp_Manually', 'again': 'Close_Fingers'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'again': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id'}) # x:30 y:478, x:130 y:478, x:230 y:478 _sm_walk_to_template_5 = OperatableStateMachine(outcomes=['finished', 'failed', 'aborted'], input_keys=['template_id', 'grasp_preference', 'hand_side']) with _sm_walk_to_template_5: # x:265 y:28 OperatableStateMachine.add('Decide_Request_Template', DecisionState(outcomes=['request', 'continue'], conditions=lambda x: 'continue' if x is not None else 'request'), transitions={'request': 'Request_Template', 'continue': 'Get_Stand_Pose'}, autonomy={'request': Autonomy.Low, 'continue': Autonomy.Off}, remapping={'input_value': 'template_id'}) # x:1033 y:106 OperatableStateMachine.add('Increment_Stand_Pose', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Inform_About_Retry'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'grasp_preference', 'output_value': 'grasp_preference'}) # x:1162 y:29 OperatableStateMachine.add('Inform_About_Retry', LogState(text="Stand pose choice failed. Trying again.", severity=Logger.REPORT_INFO), transitions={'done': 'Get_Stand_Pose'}, autonomy={'done': Autonomy.Off}) # x:567 y:118 OperatableStateMachine.add('Inform_About_Fail', LogState(text="Unable to find a suitable stand pose for the template.", severity=Logger.REPORT_WARN), transitions={'done': 'Decide_Repeat_Request'}, autonomy={'done': Autonomy.Off}) # x:554 y:274 OperatableStateMachine.add('Get_Goal_from_Operator', InputState(request=InputState.WAYPOINT_GOAL_POSE, message="Provide a waypoint in front of the template."), transitions={'received': 'Walk_To_Waypoint', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'plan_header'}) # x:279 y:110 OperatableStateMachine.add('Request_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Specify target template"), transitions={'received': 'Get_Stand_Pose', 'aborted': 'aborted', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Off, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'template_id'}) # x:825 y:461 OperatableStateMachine.add('Wait_For_Stand', CheckCurrentControlModeState(target_mode=CheckCurrentControlModeState.STAND, wait=True), transitions={'correct': 'finished', 'incorrect': 'failed'}, autonomy={'correct': Autonomy.Low, 'incorrect': Autonomy.Full}, remapping={'control_mode': 'control_mode'}) # x:1143 y:277 OperatableStateMachine.add('Decide_Stand_Preference', OperatorDecisionState(outcomes=["same", "next", "abort"], hint="Same or next stand pose?", suggestion="next"), transitions={'same': 'Inform_About_Retry', 'next': 'Increment_Stand_Pose', 'abort': 'aborted'}, autonomy={'same': Autonomy.Full, 'next': Autonomy.Full, 'abort': Autonomy.Full}) # x:842 y:152 OperatableStateMachine.add('Planning_Pipeline', _sm_planning_pipeline_3, transitions={'finished': 'Walk_To_Waypoint', 'failed': 'Decide_Stand_Preference', 'aborted': 'Decide_Stand_Preference'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'stand_pose': 'stand_pose', 'plan_header': 'plan_header'}) # x:833 y:276 OperatableStateMachine.add('Walk_To_Waypoint', ExecuteStepPlanActionState(), transitions={'finished': 'Wait_For_Stand', 'failed': 'Decide_Stand_Preference'}, autonomy={'finished': Autonomy.Off, 'failed': Autonomy.Full}, remapping={'plan_header': 'plan_header'}) # x:554 y:195 OperatableStateMachine.add('Decide_Repeat_Request', OperatorDecisionState(outcomes=['repeat_id', 'request_goal'], hint=None, suggestion=None), transitions={'repeat_id': 'Request_Template', 'request_goal': 'Get_Goal_from_Operator'}, autonomy={'repeat_id': Autonomy.Low, 'request_goal': Autonomy.High}) # x:547 y:27 OperatableStateMachine.add('Get_Stand_Pose', GetTemplateStandPoseState(), transitions={'done': 'Planning_Pipeline', 'failed': 'Inform_About_Fail', 'not_available': 'Inform_About_Fail'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Low, 'not_available': Autonomy.High}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'stand_pose': 'stand_pose'}) # x:133 y:340, x:383 y:140 _sm_perform_step_back_6 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['step_back_distance']) with _sm_perform_step_back_6: # x:78 y:78 OperatableStateMachine.add('Plan_Steps_Back', FootstepPlanRelativeState(direction=FootstepPlanRelativeState.DIRECTION_BACKWARD), transitions={'planned': 'Do_Steps_Back', 'failed': 'failed'}, autonomy={'planned': Autonomy.High, 'failed': Autonomy.Full}, remapping={'distance': 'step_back_distance', 'plan_header': 'plan_header'}) # x:74 y:228 OperatableStateMachine.add('Do_Steps_Back', ExecuteStepPlanActionState(), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'plan_header': 'plan_header'}) # x:133 y:340, x:333 y:90 _sm_release_trigger_7 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['hand_side', 'none']) with _sm_release_trigger_7: # x:82 y:78 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState(hand_type=self.hand_type, configuration=0), transitions={'done': 'Take_Hand_Back', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'hand_side': 'hand_side'}) # x:96 y:178 OperatableStateMachine.add('Take_Hand_Back', LogState(text="Take hand slightly back", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Full}) # x:733 y:240, x:33 y:289 _sm_pull_trigger_8 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none']) with _sm_pull_trigger_8: # x:202 y:28 OperatableStateMachine.add('Ready_To_Pull', LogState(text="Ready to pull the trigger down", severity=Logger.REPORT_INFO), transitions={'done': 'Get_Pull_Affordance'}, autonomy={'done': Autonomy.High}) # x:192 y:328 OperatableStateMachine.add('Plan_Pull', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={'done': 'Execute_Pull', 'incomplete': 'Execute_Pull', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full}, remapping={'affordance': 'affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction'}) # x:176 y:428 OperatableStateMachine.add('Execute_Pull', ExecuteTrajectoryMsgState(controller=affordance_controller), transitions={'done': 'Decide_Repeat_Pull', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'joint_trajectory': 'joint_trajectory'}) # x:183 y:228 OperatableStateMachine.add('Scale_Pull_Affordance', CalculationState(calculation=lambda x: x), transitions={'done': 'Plan_Pull'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'affordance', 'output_value': 'affordance'}) # x:173 y:128 OperatableStateMachine.add('Get_Pull_Affordance', GetTemplateAffordanceState(identifier=pull_affordance), transitions={'done': 'Scale_Pull_Affordance', 'failed': 'failed', 'not_available': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'affordance'}) # x:437 y:228 OperatableStateMachine.add('Decide_Repeat_Pull', OperatorDecisionState(outcomes=['done', 'repeat'], hint="Pull further?", suggestion='done'), transitions={'done': 'finished', 'repeat': 'Get_Pull_Affordance'}, autonomy={'done': Autonomy.High, 'repeat': Autonomy.Full}) with _state_machine: # x:73 y:78 OperatableStateMachine.add('Request_Trigger_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place trigger template"), transitions={'received': 'Decide_Walking', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed'}, autonomy={'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full}, remapping={'data': 'template_id'}) # x:337 y:78 OperatableStateMachine.add('Decide_Walking', OperatorDecisionState(outcomes=["walk", "stand"], hint="Walk to template?", suggestion="walk"), transitions={'walk': 'Walk_To_Template', 'stand': 'Set_Manipulate'}, autonomy={'walk': Autonomy.High, 'stand': Autonomy.Full}) # x:844 y:322 OperatableStateMachine.add('Pull_Trigger', _sm_pull_trigger_8, transitions={'finished': 'Release_Trigger', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none'}) # x:836 y:422 OperatableStateMachine.add('Release_Trigger', _sm_release_trigger_7, transitions={'finished': 'Warn_Stand', 'failed': 'Warn_Stand'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'none': 'none'}) # x:826 y:678 OperatableStateMachine.add('Go_To_Stand_Pose', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.1, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={'done': 'Set_Stand', 'failed': 'failed'}, autonomy={'done': Autonomy.Low, 'failed': Autonomy.Full}, remapping={'side': 'none'}) # x:566 y:78 OperatableStateMachine.add('Set_Manipulate', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE), transitions={'changed': 'Set_Template_Frame', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:858 y:578 OperatableStateMachine.add('Warn_Stand', LogState(text="Will go to stand now", severity=Logger.REPORT_INFO), transitions={'done': 'Go_To_Stand_Pose'}, autonomy={'done': Autonomy.High}) # x:566 y:678 OperatableStateMachine.add('Set_Stand', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.STAND), transitions={'changed': 'Decide_Step_Back', 'failed': 'failed'}, autonomy={'changed': Autonomy.Low, 'failed': Autonomy.Full}) # x:337 y:678 OperatableStateMachine.add('Decide_Step_Back', OperatorDecisionState(outcomes=["walk", "stand"], hint="Step back?", suggestion="walk"), transitions={'walk': 'Perform_Step_Back', 'stand': 'finished'}, autonomy={'walk': Autonomy.High, 'stand': Autonomy.Full}) # x:77 y:672 OperatableStateMachine.add('Perform_Step_Back', _sm_perform_step_back_6, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'step_back_distance': 'step_back_distance'}) # x:330 y:172 OperatableStateMachine.add('Walk_To_Template', _sm_walk_to_template_5, transitions={'finished': 'Set_Manipulate', 'failed': 'failed', 'aborted': 'Set_Manipulate'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit}, remapping={'template_id': 'template_id', 'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side'}) # x:841 y:222 OperatableStateMachine.add('Grasp_Trigger', _sm_grasp_trigger_4, transitions={'finished': 'Pull_Trigger', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'hand_side': 'hand_side', 'template_id': 'template_id', 'grasp_preference': 'grasp_preference'}) # x:846 y:128 OperatableStateMachine.add('Look_At_Trigger', LookAtTargetState(), transitions={'done': 'Grasp_Trigger'}, autonomy={'done': Autonomy.Off}, remapping={'frame': 'template_frame'}) # x:837 y:28 OperatableStateMachine.add('Set_Template_Frame', CalculationState(calculation=lambda x: "template_tf_%d" % x), transitions={'done': 'Look_At_Trigger'}, autonomy={'done': Autonomy.Low}, remapping={'input_value': 'template_id', 'output_value': 'template_frame'}) return _state_machine
def create(self): # x:1195 y:433, x:132 y:431, x:750 y:42, x:991 y:471 _state_machine = OperatableStateMachine(outcomes=['Given', 'Person_not_found', 'No_object_in_hand', 'fail']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:130 y:365 _sm_lookat_0 = OperatableStateMachine(outcomes=['failed'], input_keys=['ID']) with _sm_lookat_0: # x:114 y:127 OperatableStateMachine.add('look', KeepLookingAt(), transitions={'failed': 'look'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:299 y:300, x:263 y:535 _sm_give_1 = OperatableStateMachine(outcomes=['failed', 'given'], input_keys=['Object']) with _sm_give_1: # x:67 y:27 OperatableStateMachine.add('SetPose', SetKey(Value="ShowGripper"), transitions={'done': 'say_give'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:53 y:413 OperatableStateMachine.add('read torque', ReadTorque(watchdog=5, Joint="right_elbow_pitch_joint", Threshold=2, min_time=1), transitions={'threshold': 'open gripper', 'watchdog': 'read torque', 'fail': 'failed'}, autonomy={'threshold': Autonomy.Off, 'watchdog': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'torque': 'torque'}) # x:52 y:500 OperatableStateMachine.add('open gripper', SetGripperState(width=0.15, effort=1), transitions={'object': 'given', 'no_object': 'given'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:64 y:248 OperatableStateMachine.add('say pull', SaraSay(sentence="You can pull on it", input_keys=[], emotion=1, block=False), transitions={'done': 'wait 1'}, autonomy={'done': Autonomy.Off}) # x:64 y:325 OperatableStateMachine.add('wait 1', WaitState(wait_time=1), transitions={'done': 'read torque'}, autonomy={'done': Autonomy.Off}) # x:57 y:175 OperatableStateMachine.add('moveArm', MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15), transitions={'done': 'say pull', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'target': 'target'}) # x:60 y:88 OperatableStateMachine.add('say_give', SaraSay(sentence=lambda x: "Hi. I'm giving you this "+str(x), input_keys=[], emotion=0, block=True), transitions={'done': 'moveArm'}, autonomy={'done': Autonomy.Off}) # x:596 y:480 _sm_follow_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID']) with _sm_follow_2: # x:180 y:123 OperatableStateMachine.add('follow', SaraFollow(distance=1.5, ReplanPeriod=0.5), transitions={'failed': 'follow'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:313 y:247, x:301 y:177, x:103 y:293, x:343 y:113, x:397 y:411, x:311 y:27, x:630 y:365 _sm_give_3 = ConcurrencyContainer(outcomes=['failed', 'given', 'continue'], input_keys=['ID', 'Object'], conditions=[ ('failed', [('Give', 'failed')]), ('given', [('Give', 'given')]), ('given', [('Follow', 'finished')]), ('failed', [('LookAt', 'failed')]) ]) with _sm_give_3: # x:91 y:50 OperatableStateMachine.add('Follow', _sm_follow_2, transitions={'finished': 'given'}, autonomy={'finished': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:84 y:164 OperatableStateMachine.add('Give', _sm_give_1, transitions={'failed': 'failed', 'given': 'given'}, autonomy={'failed': Autonomy.Inherit, 'given': Autonomy.Inherit}, remapping={'Object': 'Object'}) # x:175 y:371 OperatableStateMachine.add('LookAt', _sm_lookat_0, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}, remapping={'ID': 'ID'}) with _state_machine: # x:77 y:29 OperatableStateMachine.add('Get hand content', GetRosParam(ParamName="behavior/GripperContent"), transitions={'done': 'is object in hand?', 'failed': 'fail'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'Value': 'Object'}) # x:58 y:108 OperatableStateMachine.add('is object in hand?', CheckConditionState(predicate=lambda x: x), transitions={'true': 'name', 'false': 'log empty hand'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'Object'}) # x:70 y:277 OperatableStateMachine.add('list persons', list_entities_by_name(frontality_level=0.5, distance_max=10), transitions={'found': 'get id', 'none_found': 'Person_not_found'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'name', 'entity_list': 'People_list', 'number': 'number'}) # x:414 y:37 OperatableStateMachine.add('log empty hand', LogState(text="The hand is empty. Set the GripperContent rosParam", severity=Logger.REPORT_HINT), transitions={'done': 'No_object_in_hand'}, autonomy={'done': Autonomy.Off}) # x:754 y:223 OperatableStateMachine.add('log moveitfail', LogState(text="moveit failed", severity=Logger.REPORT_HINT), transitions={'done': 'fail'}, autonomy={'done': Autonomy.Off}) # x:606 y:371 OperatableStateMachine.add('log movebase fail', LogState(text="giving Failed", severity=Logger.REPORT_HINT), transitions={'done': 'fail'}, autonomy={'done': Autonomy.Off}) # x:402 y:133 OperatableStateMachine.add('set idle pose', SetKey(Value="IdlePose"), transitions={'done': 'say_good'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:751 y:108 OperatableStateMachine.add('moveArm2', MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15), transitions={'done': 'set none', 'failed': 'log moveitfail'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'target': 'target'}) # x:920 y:278 OperatableStateMachine.add('close gripper', SetGripperState(width=0, effort=1), transitions={'object': 'Given', 'no_object': 'Given'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:1048 y:236 OperatableStateMachine.add('remove gripper content', SetRosParam(ParamName="GripperContent"), transitions={'done': 'close gripper'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'none'}) # x:910 y:182 OperatableStateMachine.add('set none', SetKey(Value=None), transitions={'done': 'close gripper'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'none'}) # x:408 y:333 OperatableStateMachine.add('give', _sm_give_3, transitions={'failed': 'log movebase fail', 'given': 'set idle pose', 'continue': 'give'}, autonomy={'failed': Autonomy.Inherit, 'given': Autonomy.Inherit, 'continue': Autonomy.Inherit}, remapping={'ID': 'ID', 'Object': 'Object'}) # x:256 y:278 OperatableStateMachine.add('get id', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'give'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'People_list', 'output_value': 'ID'}) # x:68 y:192 OperatableStateMachine.add('name', SetKey(Value="person"), transitions={'done': 'list persons'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'name'}) # x:591 y:138 OperatableStateMachine.add('say_good', SaraSay(sentence=lambda x: "Good, enjoy your "+str(x), input_keys=[], emotion=0, block=True), transitions={'done': 'moveArm2'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): l_random = [-2.47557862791, -1.21674644795, -0.761510070214, -0.219080422969, -2.68076430953, 0.189685935246, -3.8825693651] r_random = [1.10260663602, 2.67164332545, 3.1286914453, 1.52042152218, -1.94191336135, 0.805351035619, -3.64437671794] # x:33 y:340, x:435 y:356 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.joint_values = [0] * 7 _state_machine.userdata.l_random = l_random _state_machine.userdata.r_random = r_random _state_machine.userdata.random = l_random if self.planning_group == "l_arm_group" else r_random _state_machine.userdata.hand_side = 'left' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:151 y:30 OperatableStateMachine.add('Set_Manipulate', ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE), transitions={'changed': 'Go_To_Stand', 'failed': 'failed'}, autonomy={'changed': Autonomy.Off, 'failed': Autonomy.Off}) # x:65 y:513 OperatableStateMachine.add('Turn_Torso_Left_Pose', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.TURN_TORSO_LEFT_POSE, vel_scaling=0.3, ignore_collisions=True, link_paddings={}), transitions={'done': 'Go_To_Open_Door_Pose', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) # x:312 y:565 OperatableStateMachine.add('Go_To_Open_Door_Pose', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.DOOR_OPEN_POSE_STRAIGHT, vel_scaling=0.1, ignore_collisions=False, link_paddings={}), transitions={'done': 'Back_To_Door_Ready_Pose', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) # x:647 y:328 OperatableStateMachine.add('Go_To_Final_Stand', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}), transitions={'done': 'Ask_If_Open', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) # x:622 y:189 OperatableStateMachine.add('Ask_If_Open', OperatorDecisionState(outcomes=['open','push_again'], hint='Is the door open?', suggestion='open'), transitions={'open': 'finished', 'push_again': 'Log_Try_Push_Again'}, autonomy={'open': Autonomy.High, 'push_again': Autonomy.Full}) # x:355 y:189 OperatableStateMachine.add('Log_Try_Push_Again', LogState(text='Move robot closer to door', severity=Logger.REPORT_HINT), transitions={'done': 'Go_To_Door_Ready_Pose'}, autonomy={'done': Autonomy.Full}) # x:623 y:452 OperatableStateMachine.add('Turn_Torso_Center_Pose', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.TURN_TORSO_CENTER_POSE, vel_scaling=0.3, ignore_collisions=True, link_paddings={}), transitions={'done': 'Go_To_Final_Stand', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) # x:126 y:316 OperatableStateMachine.add('Go_To_Door_Ready_Pose', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}), transitions={'done': 'Go_To_Open_Door_Pose', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) # x:100 y:203 OperatableStateMachine.add('Go_To_Stand', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.5, ignore_collisions=False, link_paddings={}), transitions={'done': 'Go_To_Door_Ready_Pose', 'failed': 'failed'}, autonomy={'done': Autonomy.High, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) # x:551 y:580 OperatableStateMachine.add('Back_To_Door_Ready_Pose', MoveitPredefinedPoseState(target_pose=MoveitPredefinedPoseState.DOOR_READY_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}), transitions={'done': 'Go_To_Final_Stand', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'side': 'hand_side'}) return _state_machine
def create(self): # x:821 y:66, x:331 y:203 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.both = 'same' _state_machine.userdata.left = 'left' _state_machine.userdata.right = 'right' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:100 y:355, x:321 y:250 _sm_prep_right_arm_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['hand_side']) with _sm_prep_right_arm_0: # x:61 y:89 OperatableStateMachine.add( 'Go_to_Camera_Pose', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState. CAR_DRIVE_CAMERA_POSE, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={ 'done': 'Open_Fingers', 'failed': 'Go_to_Camera_Pose' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'side': 'hand_side'}) # x:65 y:228 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState( hand_type='robotiq', configuration=0.0), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={'hand_side': 'hand_side'}) # x:329 y:513, x:476 y:175 _sm_prep_left_arm_1 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['hand_side']) with _sm_prep_left_arm_1: # x:79 y:59 OperatableStateMachine.add( 'Look_Down', TiltHeadState(desired_tilt=TiltHeadState.DOWN_60), transitions={ 'done': 'Go_to_Pre_Drive', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }) # x:60 y:166 OperatableStateMachine.add( 'Go_to_Pre_Drive', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState. CAR_PREDRIVE_LARM_POSE, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={ 'done': 'Open_Fingers', 'failed': 'Go_to_Pre_Drive' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'side': 'hand_side'}) # x:61 y:397 OperatableStateMachine.add( 'Go_to_Drive', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.CAR_DRIVE_LARM_POSE, vel_scaling=0.2, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={ 'done': 'Notify_Field_Team', 'failed': 'Go_to_Drive' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'side': 'hand_side'}) # x:65 y:283 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState( hand_type='robotiq', configuration=0.0), transitions={ 'done': 'Go_to_Drive', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={'hand_side': 'hand_side'}) # x:71 y:504 OperatableStateMachine.add( 'Notify_Field_Team', LogState(text='Ask field team to strap ATLAS', severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Full}) # x:409 y:455 _sm_prep_car_entry_2 = OperatableStateMachine( outcomes=['finished'], input_keys=['both', 'left', 'right']) with _sm_prep_car_entry_2: # x:60 y:65 OperatableStateMachine.add( 'Car_Entry_Arms', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.CAR_ENTRY_ARMS_POSE, vel_scaling=0.3, ignore_collisions=True, link_paddings={}, is_cartesian=False), transitions={ 'done': 'Car_Entry_Forearms', 'failed': 'Car_Entry_Arms' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'side': 'both'}) # x:60 y:217 OperatableStateMachine.add( 'Car_Entry_Forearms', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.CAR_ENTRY_FORE_POSE, vel_scaling=0.5, ignore_collisions=True, link_paddings={}, is_cartesian=False), transitions={ 'done': 'Car_Entry_Left_Leg', 'failed': 'Car_Entry_Forearms' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'side': 'both'}) # x:60 y:351 OperatableStateMachine.add( 'Car_Entry_Left_Leg', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.CAR_ENTRY_LEGS_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={ 'done': 'Car_Entry_Right_Leg', 'failed': 'Car_Entry_Left_Leg' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'side': 'left'}) # x:357 y:351 OperatableStateMachine.add( 'Car_Entry_Right_Leg', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.CAR_ENTRY_LEGS_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={ 'done': 'finished', 'failed': 'Car_Entry_Right_Leg' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Full }, remapping={'side': 'right'}) with _state_machine: # x:39 y:63 OperatableStateMachine.add('Start_in_FREEZE_HP', ChangeControlModeActionState( target_mode='freeze_high_pressure'), transitions={ 'changed': 'Go_to_VEHICLE_ENTRY', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Full, 'failed': Autonomy.High }) # x:51 y:446 OperatableStateMachine.add( 'Prep_Car_Entry', _sm_prep_car_entry_2, transitions={'finished': 'Go_to_FREEZE_HP'}, autonomy={'finished': Autonomy.Inherit}, remapping={ 'both': 'both', 'left': 'left', 'right': 'right' }) # x:278 y:451 OperatableStateMachine.add('Go_to_FREEZE_HP', ChangeControlModeActionState( target_mode='freeze_high_pressure'), transitions={ 'changed': 'Confirm_All_Clear', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.High, 'failed': Autonomy.High }) # x:547 y:226 OperatableStateMachine.add('Prep_Left_Arm', _sm_prep_left_arm_1, transitions={ 'finished': 'Prep_Right_Arm', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'hand_side': 'left'}) # x:523 y:16 OperatableStateMachine.add( 'Go_to_VEHICLE_DRIVE', ChangeControlModeActionState(target_mode='vehicle_drive'), transitions={ 'changed': 'finished', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.High }) # x:515 y:347 OperatableStateMachine.add( 'Back_to_VEHICLE_MANIPULATE', ChangeControlModeActionState(target_mode='vehicle_manipulate'), transitions={ 'changed': 'Prep_Left_Arm', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Off, 'failed': Autonomy.High }) # x:550 y:449 OperatableStateMachine.add( 'Confirm_All_Clear', LogState(text='Confirm that all personnel is clear!', severity=Logger.REPORT_HINT), transitions={'done': 'Back_to_VEHICLE_MANIPULATE'}, autonomy={'done': Autonomy.Full}) # x:39 y:252 OperatableStateMachine.add( 'Go_to_VEHICLE_ENTRY', ChangeControlModeActionState(target_mode='vehicle_entry'), transitions={ 'changed': 'Prep_Car_Entry', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.High }) # x:543 y:101 OperatableStateMachine.add('Prep_Right_Arm', _sm_prep_right_arm_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'hand_side': 'right'}) return _state_machine
def create(self): # x:119 y:369, x:527 y:258 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['my_input']) _state_machine.userdata.my_input = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add( 'Entry_Msg', LogState(text="Now at inner behavior...", severity=Logger.REPORT_INFO), transitions={'done': 'FlexBE Testprint Behavior'}, autonomy={'done': Autonomy.Off}) # x:34 y:240 OperatableStateMachine.add('Wait_A_Bit', WaitState(wait_time=2), transitions={'done': 'Print_State'}, autonomy={'done': Autonomy.High}) # x:279 y:327 OperatableStateMachine.add('Decide_Outcome', OperatorDecisionState( outcomes=['finished', 'failed'], hint="How should this end?", suggestion='finished'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Full, 'failed': Autonomy.Full }) # x:289 y:127 OperatableStateMachine.add( 'Print_State', CalculationState(calculation=self.print_number_and_inc), transitions={'done': 'Print_Param_Message'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'my_input', 'output_value': 'output_value' }) # x:273 y:212 OperatableStateMachine.add( 'Print_Param_Message', LogState(text="Wait, there is a message: " + self.message, severity=Logger.REPORT_HINT), transitions={'done': 'Decide_Outcome'}, autonomy={'done': Autonomy.Off}) # x:15 y:124 OperatableStateMachine.add('FlexBE Testprint Behavior', self.use_behavior( FlexBETestprintBehaviorSM, 'FlexBE Testprint Behavior'), transitions={'finished': 'Wait_A_Bit'}, autonomy={'finished': Autonomy.Inherit}) return _state_machine
def create(self): plug_in_affordance = "plug_in" plug_out_affordance = "plug_out" arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM # x:202 y:568, x:374 y:401 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.none = None _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.grasp_preference = 0 _state_machine.userdata.step_back_distance = 1.0 # meters # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:588 y:126, x:324 y:44 _sm_back_to_pregrasp_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'grasp_preference']) with _sm_back_to_pregrasp_0: # x:30 y:40 OperatableStateMachine.add('Get_Pregrasp', GetTemplatePregraspState(), transitions={ 'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'pre_grasp': 'grasp_pose' }) # x:242 y:292 OperatableStateMachine.add( 'Plan_To_Pregrasp', PlanEndeffectorCartesianWaypointsState( ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={ 'planned': 'Move_To_Pregrasp_Pose', 'incomplete': 'Move_To_Pregrasp_Pose', 'failed': 'Get_Pregrasp' }, autonomy={ 'planned': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.Full }, remapping={ 'waypoints': 'grasp_waypoints', 'hand': 'hand_side', 'frame_id': 'grasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction' }) # x:568 y:295 OperatableStateMachine.add( 'Move_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={ 'done': 'finished', 'failed': 'Get_Pregrasp' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'joint_trajectory': 'joint_trajectory'}) # x:41 y:178 OperatableStateMachine.add( 'Extract_Frame_Id', CalculationState( calculation=lambda pose: pose.header.frame_id), transitions={'done': 'Convert_Waypoints'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'grasp_pose', 'output_value': 'grasp_frame_id' }) # x:40 y:293 OperatableStateMachine.add( 'Convert_Waypoints', CalculationState(calculation=lambda msg: [msg.pose]), transitions={'done': 'Plan_To_Pregrasp'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'grasp_pose', 'output_value': 'grasp_waypoints' }) # x:598 y:30, x:220 y:148, x:1035 y:174 _sm_go_to_grasp_1 = OperatableStateMachine( outcomes=['finished', 'failed', 'again'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference']) with _sm_go_to_grasp_1: # x:33 y:49 OperatableStateMachine.add('Get_Grasp_Info', GetTemplateGraspState(), transitions={ 'done': 'Extract_Frame_Id', 'failed': 'failed', 'not_available': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'grasp': 'grasp_pose' }) # x:40 y:293 OperatableStateMachine.add( 'Convert_Waypoints', CalculationState(calculation=lambda msg: [msg.pose]), transitions={'done': 'Plan_To_Grasp'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'grasp_pose', 'output_value': 'grasp_waypoints' }) # x:242 y:292 OperatableStateMachine.add( 'Plan_To_Grasp', PlanEndeffectorCartesianWaypointsState( ignore_collisions=True, include_torso=False, keep_endeffector_orientation=False, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={ 'planned': 'Move_To_Grasp_Pose', 'incomplete': 'Move_To_Grasp_Pose', 'failed': 'Decide_Which_Grasp' }, autonomy={ 'planned': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full }, remapping={ 'waypoints': 'grasp_waypoints', 'hand': 'hand_side', 'frame_id': 'grasp_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction' }) # x:494 y:175 OperatableStateMachine.add( 'Move_To_Grasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={ 'done': 'Optional_Template_Adjustment', 'failed': 'Decide_Which_Grasp' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'joint_trajectory': 'joint_trajectory'}) # x:970 y:294 OperatableStateMachine.add( 'Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'again'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'grasp_preference', 'output_value': 'grasp_preference' }) # x:41 y:178 OperatableStateMachine.add( 'Extract_Frame_Id', CalculationState( calculation=lambda pose: pose.header.frame_id), transitions={'done': 'Convert_Waypoints'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'grasp_pose', 'output_value': 'grasp_frame_id' }) # x:727 y:50 OperatableStateMachine.add( 'Optional_Template_Adjustment', OperatorDecisionState( outcomes=["grasp", "pregrasp", "skip"], hint="Consider adjusting the template's pose", suggestion="skip"), transitions={ 'grasp': 'Get_Grasp_Info', 'pregrasp': 'again', 'skip': 'finished' }, autonomy={ 'grasp': Autonomy.Full, 'pregrasp': Autonomy.Full, 'skip': Autonomy.High }) # x:754 y:294 OperatableStateMachine.add( 'Decide_Which_Grasp', OperatorDecisionState( outcomes=["same", "next"], hint='Try the same grasp or the next one?', suggestion='same'), transitions={ 'same': 'Optional_Template_Adjustment', 'next': 'Increase_Preference_Index' }, autonomy={ 'same': Autonomy.High, 'next': Autonomy.High }) # x:596 y:113, x:351 y:62 _sm_go_to_pregrasp_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['hand_side', 'grasp_preference', 'template_id'], output_keys=['grasp_preference', 'pregrasp_pose']) with _sm_go_to_pregrasp_2: # x:27 y:68 OperatableStateMachine.add('Get_Pregrasp_Info', GetTemplatePregraspState(), transitions={ 'done': 'Plan_To_Pregrasp_Pose', 'failed': 'failed', 'not_available': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'preference': 'grasp_preference', 'pre_grasp': 'pregrasp_pose' }) # x:537 y:228 OperatableStateMachine.add( 'Move_To_Pregrasp_Pose', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={ 'done': 'finished', 'failed': 'Decide_Which_Pregrasp' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'joint_trajectory': 'joint_trajectory'}) # x:25 y:328 OperatableStateMachine.add( 'Increase_Preference_Index', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Get_Pregrasp_Info'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'grasp_preference', 'output_value': 'grasp_preference' }) # x:266 y:228 OperatableStateMachine.add( 'Plan_To_Pregrasp_Pose', PlanEndeffectorPoseState( ignore_collisions=False, include_torso=False, allowed_collisions=[], planner_id="RRTConnectkConfigDefault"), transitions={ 'planned': 'Move_To_Pregrasp_Pose', 'failed': 'Decide_Which_Pregrasp' }, autonomy={ 'planned': Autonomy.Low, 'failed': Autonomy.Full }, remapping={ 'target_pose': 'pregrasp_pose', 'hand': 'hand_side', 'joint_trajectory': 'joint_trajectory' }) # x:266 y:327 OperatableStateMachine.add( 'Decide_Which_Pregrasp', OperatorDecisionState( outcomes=["same", "next"], hint='Try the same pregrasp or the next one?', suggestion='same'), transitions={ 'same': 'Get_Pregrasp_Info', 'next': 'Increase_Preference_Index' }, autonomy={ 'same': Autonomy.High, 'next': Autonomy.Full }) # x:30 y:365, x:130 y:365 _sm_perform_step_back_3 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['step_back_distance']) with _sm_perform_step_back_3: # x:78 y:78 OperatableStateMachine.add( 'Plan_Steps_Back', FootstepPlanRelativeState( direction=FootstepPlanRelativeState.DIRECTION_BACKWARD), transitions={ 'planned': 'Do_Steps_Back', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Full }, remapping={ 'distance': 'step_back_distance', 'plan_header': 'plan_header' }) # x:74 y:228 OperatableStateMachine.add( 'Do_Steps_Back', ExecuteStepPlanActionState(), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'plan_header': 'plan_header'}) # x:550 y:574, x:130 y:365 _sm_plug_in_cable_4 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'target_template_id', 'hand_side', 'grasp_preference', 'template_id', 'template_pose' ]) with _sm_plug_in_cable_4: # x:82 y:59 OperatableStateMachine.add('Go_to_Pregrasp', _sm_go_to_pregrasp_2, transitions={ 'finished': 'Go_to_Grasp', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'target_template_id', 'pregrasp_pose': 'pregrasp_pose' }) # x:322 y:171 OperatableStateMachine.add('Go_to_Grasp', _sm_go_to_grasp_1, transitions={ 'finished': 'Detach_Cable', 'failed': 'failed', 'again': 'Go_to_Pregrasp' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'again': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'target_template_id' }) # x:516 y:306 OperatableStateMachine.add('Open_Fingers', FingerConfigurationState( hand_type=self.hand_type, configuration=0), transitions={ 'done': 'Back_To_Pregrasp', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'hand_side': 'hand_side'}) # x:472 y:406 OperatableStateMachine.add('Back_To_Pregrasp', _sm_back_to_pregrasp_0, transitions={ 'finished': 'Close_Fingers', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference' }) # x:314 y:524 OperatableStateMachine.add('Close_Fingers', FingerConfigurationState( hand_type=self.hand_type, configuration=1), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'hand_side': 'hand_side'}) # x:527 y:204 OperatableStateMachine.add('Detach_Cable', DetachObjectState(), transitions={ 'done': 'Open_Fingers', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'template_pose': 'template_pose' }) # x:30 y:365, x:115 y:197 _sm_take_cable_5 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['template_id', 'hand_side', 'none']) with _sm_take_cable_5: # x:81 y:54 OperatableStateMachine.add( 'Get_Plug_Out_Affordance', GetTemplateAffordanceState(identifier=plug_out_affordance), transitions={ 'done': 'Plan_Plug_Out', 'failed': 'failed', 'not_available': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full, 'not_available': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'affordance': 'affordance' }) # x:348 y:73 OperatableStateMachine.add( 'Plan_Plug_Out', PlanAffordanceState(vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={ 'done': 'Execute_Plug_Out', 'incomplete': 'Execute_Plug_Out', 'failed': 'failed' }, autonomy={ 'done': Autonomy.High, 'incomplete': Autonomy.High, 'failed': Autonomy.Full }, remapping={ 'affordance': 'affordance', 'hand': 'hand_side', 'reference_point': 'none', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction' }) # x:322 y:255 OperatableStateMachine.add( 'Execute_Plug_Out', ExecuteTrajectoryMsgState(controller=affordance_controller), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'joint_trajectory': 'joint_trajectory'}) with _state_machine: # x:73 y:78 OperatableStateMachine.add( 'Request_Cable_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place cable template"), transitions={ 'received': 'Decide_Walking', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed' }, autonomy={ 'received': Autonomy.Low, 'aborted': Autonomy.Full, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full }, remapping={'data': 'template_id'}) # x:330 y:172 OperatableStateMachine.add( 'Walk_To_Template', self.use_behavior(WalktoTemplateSM, 'Walk_To_Template'), transitions={ 'finished': 'Set_Manipulate', 'failed': 'failed', 'aborted': 'Set_Manipulate' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit }, remapping={ 'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side', 'template_id': 'template_id' }) # x:337 y:78 OperatableStateMachine.add('Decide_Walking', OperatorDecisionState( outcomes=["walk", "stand"], hint="Walk to template?", suggestion="walk"), transitions={ 'walk': 'Walk_To_Template', 'stand': 'Set_Manipulate' }, autonomy={ 'walk': Autonomy.High, 'stand': Autonomy.Full }) # x:839 y:72 OperatableStateMachine.add('Grasp Object', self.use_behavior( GraspObjectSM, 'Grasp Object'), transitions={ 'finished': 'Attach_Cable', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'template_id': 'template_id' }) # x:843 y:302 OperatableStateMachine.add('Take_Cable', _sm_take_cable_5, transitions={ 'finished': 'Request_Target_Template', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'none': 'none' }) # x:566 y:78 OperatableStateMachine.add( 'Set_Manipulate', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.MANIPULATE), transitions={ 'changed': 'Grasp Object', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Full }) # x:822 y:545 OperatableStateMachine.add('Plug_In_Cable', _sm_plug_in_cable_4, transitions={ 'finished': 'Warn_Stand', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'target_template_id': 'target_template_id', 'hand_side': 'hand_side', 'grasp_preference': 'grasp_preference', 'template_id': 'template_id', 'template_pose': 'template_pose' }) # x:826 y:185 OperatableStateMachine.add('Attach_Cable', AttachObjectState(), transitions={ 'done': 'Take_Cable', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'template_pose': 'template_pose' }) # x:811 y:400 OperatableStateMachine.add( 'Request_Target_Template', InputState(request=InputState.SELECTED_OBJECT_ID, message="Place target template"), transitions={ 'received': 'Plug_In_Cable', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed' }, autonomy={ 'received': Autonomy.Low, 'aborted': Autonomy.High, 'no_connection': Autonomy.Full, 'data_error': Autonomy.Full }, remapping={'data': 'target_template_id'}) # x:753 y:646 OperatableStateMachine.add( 'Warn_Stand', LogState(text="Going to stand pose", severity=Logger.REPORT_INFO), transitions={'done': 'Go_To_Stand_Pose'}, autonomy={'done': Autonomy.High}) # x:581 y:573 OperatableStateMachine.add( 'Go_To_Stand_Pose', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.1, ignore_collisions=False, link_paddings={}, is_cartesian=False), transitions={ 'done': 'Set_Stand', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Full }, remapping={'side': 'none'}) # x:446 y:653 OperatableStateMachine.add('Decide_Step_Back', OperatorDecisionState( outcomes=["walk", "stand"], hint="Step back?", suggestion="walk"), transitions={ 'walk': 'Perform_Step_Back', 'stand': 'finished' }, autonomy={ 'walk': Autonomy.High, 'stand': Autonomy.Full }) # x:100 y:661 OperatableStateMachine.add( 'Perform_Step_Back', _sm_perform_step_back_3, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'step_back_distance': 'step_back_distance'}) # x:383 y:527 OperatableStateMachine.add( 'Set_Stand', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND), transitions={ 'changed': 'Decide_Step_Back', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Full }) return _state_machine
def create(self): # x:807 y:293, x:1163 y:18 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:35 y:227, x:161 y:228, x:363 y:243, x:536 y:22, x:472 y:241, x:547 y:181, x:544 y:99, x:674 y:184, x:263 y:228 _sm_container_0 = ConcurrencyContainer( outcomes=['finished', 'failed', 'danger', 'preempted'], input_keys=['plan'], conditions=[('failed', [('DWA', 'failed')]), ('finished', [('DWA', 'done')]), ('preempted', [('DWA', 'preempted')]), ('danger', [('Safety', 'cliff')]), ('danger', [('Safety', 'bumper')])]) with _sm_container_0: # x:101 y:78 OperatableStateMachine.add( 'DWA', FollowPathState(topic="low_level_planner"), transitions={ 'done': 'finished', 'failed': 'failed', 'preempted': 'preempted' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off, 'preempted': Autonomy.Off }, remapping={'plan': 'plan'}) # x:343 y:91 OperatableStateMachine.add( 'Safety', TurtlebotStatusState(bumper_topic='mobile_base/events/bumper', cliff_topic='mobile_base/events/cliff'), transitions={ 'bumper': 'danger', 'cliff': 'danger' }, autonomy={ 'bumper': Autonomy.Off, 'cliff': Autonomy.Off }) with _state_machine: # x:193 y:26 OperatableStateMachine.add('ClearCostmap', ClearCostmapsState(costmap_topics=[ 'high_level_planner/clear_costmap', 'low_level_planner/clear_costmap' ], timeout=5.0), transitions={ 'done': 'Receive Goal', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }) # x:205 y:207 OperatableStateMachine.add( 'Receive Path', GetPathState(planner_topic="high_level_planner"), transitions={ 'planned': 'ExecutePlan', 'empty': 'Continue', 'failed': 'Continue' }, autonomy={ 'planned': Autonomy.Off, 'empty': Autonomy.Low, 'failed': Autonomy.Low }, remapping={ 'goal': 'goal', 'plan': 'plan' }) # x:194 y:301 OperatableStateMachine.add('ExecutePlan', OperatorDecisionState( outcomes=["yes", "no"], hint="Execute the current plan?", suggestion="yes"), transitions={ 'yes': 'Container', 'no': 'Continue' }, autonomy={ 'yes': Autonomy.High, 'no': Autonomy.Full }) # x:446 y:275 OperatableStateMachine.add('Container', _sm_container_0, transitions={ 'finished': 'Log Success', 'failed': 'AutoReplan', 'danger': 'EStop', 'preempted': 'Continue' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'danger': Autonomy.Inherit, 'preempted': Autonomy.Inherit }, remapping={'plan': 'plan'}) # x:435 y:146 OperatableStateMachine.add( 'Continue', OperatorDecisionState( outcomes=["yes", "no", "recover", "clearcostmap"], hint="Continue planning to new goal?", suggestion="yes"), transitions={ 'yes': 'Receive Goal', 'no': 'finished', 'recover': 'LogRecovery', 'clearcostmap': 'ClearCostmap' }, autonomy={ 'yes': Autonomy.High, 'no': Autonomy.Full, 'recover': Autonomy.Full, 'clearcostmap': Autonomy.Full }) # x:1052 y:227 OperatableStateMachine.add('Turtlebot Simple Recovery', self.use_behavior( TurtlebotSimpleRecoverySM, 'Turtlebot Simple Recovery'), transitions={ 'finished': 'AutoReplan', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:664 y:307 OperatableStateMachine.add('Log Success', LogState(text="Success!", severity=Logger.REPORT_HINT), transitions={'done': 'Continue'}, autonomy={'done': Autonomy.Off}) # x:664 y:374 OperatableStateMachine.add('Log Fail', LogState(text="Path execution failure", severity=Logger.REPORT_HINT), transitions={'done': 'Recover'}, autonomy={'done': Autonomy.Off}) # x:960 y:70 OperatableStateMachine.add('Log Recovered', LogState(text="Re-plan after recovery", severity=Logger.REPORT_HINT), transitions={'done': 'New Plan'}, autonomy={'done': Autonomy.Off}) # x:772 y:71 OperatableStateMachine.add( 'New Plan', GetPathState(planner_topic="high_level_planner"), transitions={ 'planned': 'Container', 'empty': 'Receive Goal', 'failed': 'Continue' }, autonomy={ 'planned': Autonomy.Off, 'empty': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'goal': 'goal', 'plan': 'plan' }) # x:203 y:113 OperatableStateMachine.add( 'Receive Goal', GetPoseState(topic='flex_nav_global/goal'), transitions={'done': 'Receive Path'}, autonomy={'done': Autonomy.Low}, remapping={'goal': 'goal'}) # x:866 y:374 OperatableStateMachine.add('Recover', OperatorDecisionState( outcomes=["yes", "no"], hint="Should we attempt recovery?", suggestion="yes"), transitions={ 'yes': 'LogRecovery', 'no': 'finished' }, autonomy={ 'yes': Autonomy.High, 'no': Autonomy.Full }) # x:887 y:236 OperatableStateMachine.add( 'LogRecovery', LogState(text="Starting recovery behavior", severity=Logger.REPORT_HINT), transitions={'done': 'Turtlebot Simple Recovery'}, autonomy={'done': Autonomy.Off}) # x:875 y:162 OperatableStateMachine.add('AutoReplan', OperatorDecisionState( outcomes=["yes", "no"], hint="Re-plan to current goal?", suggestion="yes"), transitions={ 'yes': 'Log Recovered', 'no': 'Continue' }, autonomy={ 'yes': Autonomy.High, 'no': Autonomy.Full }) # x:453 y:376 OperatableStateMachine.add( 'EStop', TimedStopState(timeout=0.25, cmd_topic='stamped_cmd_vel_mux/input/navi', odom_topic='mobile_base/odom'), transitions={ 'done': 'Log Fail', 'failed': 'Log Fail' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }) return _state_machine
def create(self): # x:702 y:576, x:764 y:158, x:766 y:33 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Place", "table"] _state_machine.userdata.Empty = None _state_machine.userdata.IdlePos = "IdlePose" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:44 y:28 OperatableStateMachine.add( 'gripper contain', GetRosParam(ParamName="behavior/GripperContent"), transitions={ 'done': 'if contain something', 'failed': 'cause1' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'content'}) # x:222 y:497 OperatableStateMachine.add('Action_place', self.use_behavior( Action_placeSM, 'Action_place'), transitions={ 'finished': 'idlearm', 'failed': 'cause3' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pos': 'MapPosition'}) # x:39 y:367 OperatableStateMachine.add( 'genPoseArm', GenPoseEuler(x=0.75, y=-0.25, z=0.85, roll=0, pitch=0, yaw=0), transitions={'done': 'referential from robot to map'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'position'}) # x:8 y:433 OperatableStateMachine.add('referential from robot to map', TF_transformation(in_ref="base_link", out_ref="map"), transitions={ 'done': 'log pose', 'fail': 'log tf error' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'in_pos': 'position', 'out_pos': 'MapPosition' }) # x:25 y:98 OperatableStateMachine.add( 'if contain something', CheckConditionState(predicate=lambda x: x != ''), transitions={ 'true': 'cond', 'false': 'say nothing in gripper' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'content'}) # x:209 y:98 OperatableStateMachine.add( 'say nothing in gripper', SaraSay(sentence="It seems I have nothing in my gripper", input_keys=[], emotion=1, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}) # x:28 y:236 OperatableStateMachine.add( 'construction phrase', FlexibleCalculationState( calculation=lambda x: "I will place this " + str(x[ 0]) + " on the " + str(x[1][1]), input_keys=["content", "Action"]), transitions={'done': 'Say_Place_object'}, autonomy={'done': Autonomy.Off}, remapping={ 'content': 'content', 'Action': 'Action', 'output_value': 'sentence' }) # x:33 y:167 OperatableStateMachine.add( 'cond', CheckConditionState(predicate=lambda x: x[1] != ''), transitions={ 'true': 'construction phrase', 'false': 'cause2' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'Action'}) # x:257 y:413 OperatableStateMachine.add('log tf error', LogState(text="tf error", severity=Logger.REPORT_HINT), transitions={'done': 'cause3'}, autonomy={'done': Autonomy.Off}) # x:42 y:502 OperatableStateMachine.add( 'log pose', LogKeyState(text="the placement pose will be: {}", severity=Logger.REPORT_HINT), transitions={'done': 'Action_place'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'MapPosition'}) # x:493 y:535 OperatableStateMachine.add( 'empty hand', SetRosParam(ParamName="behavior/GripperContent"), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Empty'}) # x:448 y:54 OperatableStateMachine.add( 'cause1', SetKey(Value="I didn't have any object in my gripper"), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:422 y:149 OperatableStateMachine.add( 'cause2', SetKey(Value="I didn't know where to place the object."), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:575 y:158 OperatableStateMachine.add( 'setrosparamcause', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:449 y:325 OperatableStateMachine.add( 'cause3', SetKey( Value="I was unable to calculate how to place the object." ), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:342 y:583 OperatableStateMachine.add('idlearm', MoveitMove(move=True, waitForExecution=False, group="RightArm", watchdog=15), transitions={ 'done': 'empty hand', 'failed': 'empty hand' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'IdlePos'}) # x:706 y:460 OperatableStateMachine.add( 'Say_Place_It_This_Place', SaraSay(sentence=lambda x: "I will place this " + x + " right there.", input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:35 y:301 OperatableStateMachine.add('Say_Place_object', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'genPoseArm'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:284 y:484, x:47 y:223 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:454, x:130 y:454 _sm_take_photo_container_(guess_age)_0 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_take_photo_container_(guess_age)_0: # x:124 y:43 OperatableStateMachine.add('ok if you are ready show me a big smile', TalkState(sentence_number=3), transitions={'continue': 'smile', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:699 y:57 OperatableStateMachine.add('wait as if take photo', WaitState(wait_time=3), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:481 y:41 OperatableStateMachine.add('smile', FacialExpressionState(expression_num=1), transitions={'continue': 'wait as if take photo', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) with _state_machine: # x:109 y:52 OperatableStateMachine.add('launch video', LaunchVideoStream(), transitions={'continue': 'starting object track talk', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:514 y:19 OperatableStateMachine.add('detect and track', LaunchObjDetectAndTrack(), transitions={'continue': 'subscriber_is_nearby_face_State', 'failed': 'stop face detect'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:734 y:20 OperatableStateMachine.add('subscriber_is_nearby_face_State', SubscriberState(topic='/is_person_nearby', blocking=True, clear=False), transitions={'received': 'check_if_nearby', 'unavailable': 'stop obj detect state'}, autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off}, remapping={'message': 'message'}) # x:732 y:157 OperatableStateMachine.add('check_if_nearby', CheckConditionState(predicate=lambda message:message.data == "yes"), transitions={'true': 'stop obj detect state', 'false': 'logger'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'message'}) # x:588 y:115 OperatableStateMachine.add('logger', LogState(text="false", severity=Logger.REPORT_HINT), transitions={'done': 'subscriber_is_nearby_face_State'}, autonomy={'done': Autonomy.Off}) # x:935 y:201 OperatableStateMachine.add('stop obj detect state', StopObjectDetectAndTrack(), transitions={'continue': 'face identified', 'failed': 'stop face detect'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:960 y:281 OperatableStateMachine.add('launch face detect and track', FaceDetTrack(), transitions={'continue': 'wait to show face trk', 'failed': 'stop face detect'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1178 y:285 OperatableStateMachine.add('wait to show face trk', WaitState(wait_time=5), transitions={'done': 'talk hello'}, autonomy={'done': Autonomy.Off}) # x:1165 y:361 OperatableStateMachine.add('talk hello', TalkState(sentence_number=2), transitions={'continue': 'listen_for_input_state', 'failed': 'stop face detect'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:342 y:308 OperatableStateMachine.add('stop face detect', StopFaceDetectAndTrack(), transitions={'continue': 'finished', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:967 y:38 OperatableStateMachine.add('face identified', TalkState(sentence_number=100), transitions={'continue': 'start face track', 'failed': 'stop obj detect state'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1171 y:33 OperatableStateMachine.add('start face track', TalkState(sentence_number=101), transitions={'continue': 'launch face detect and track', 'failed': 'stop obj detect state'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:273 y:126 OperatableStateMachine.add('starting object track talk', TalkState(sentence_number=99), transitions={'continue': 'detect and track', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1164 y:441 OperatableStateMachine.add('listen_for_input_state', ListeningState(), transitions={'continue': 'subscribe to user input1', 'failed': 'stop face detect'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1160 y:516 OperatableStateMachine.add('subscribe to user input1', SubscriberState(topic='/stt_topic', blocking=True, clear=False), transitions={'received': 'check user input1', 'unavailable': 'stop face detect'}, autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off}, remapping={'message': 'message_stt'}) # x:1180 y:590 OperatableStateMachine.add('check user input1', CheckConditionState(predicate=lambda message: message.query =="please take my photo"), transitions={'true': 'yes take photo', 'false': 'no take photo not received'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'message_stt'}) # x:1005 y:585 OperatableStateMachine.add('yes take photo', LogState(text="yes take photo recieved", severity=Logger.REPORT_HINT), transitions={'done': 'take photo container (guess age)'}, autonomy={'done': Autonomy.Off}) # x:1367 y:517 OperatableStateMachine.add('no take photo not received', LogState(text="no not recieved take photo", severity=Logger.REPORT_HINT), transitions={'done': 'listen_for_input_state'}, autonomy={'done': Autonomy.Off}) # x:727 y:532 OperatableStateMachine.add('take photo container (guess age)', _sm_take_photo_container_(guess_age)_0, transitions={'finished': 'left container', 'failed': 'stop face detect'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:585 y:606 OperatableStateMachine.add('left container', LogState(text="left container - now back on main", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:30 y:365, x:130 y:365 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:54 y:385, x:100 y:523 _sm_safety_monitor_0 = OperatableStateMachine( outcomes=['finished', 'failed']) with _sm_safety_monitor_0: # x:289 y:57 OperatableStateMachine.add('subscribe to safety topic 1', SubscriberState(topic='/chatter', blocking=True, clear=False), transitions={ 'received': 'check if topic publishes warning', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:719 y:64 OperatableStateMachine.add( 'fine continue log', LogState(text="continue log", severity=Logger.REPORT_HINT), transitions={'done': 'subscribe to safety topic 1'}, autonomy={'done': Autonomy.Off}) # x:690 y:202 OperatableStateMachine.add( 'check if topic publishes warning', CheckConditionState( predicate=lambda message: message.data == "1"), transitions={ 'true': 'fine continue log', 'false': 'stop now log' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'message'}) # x:451 y:481 OperatableStateMachine.add('stop now log', LogState( text="stopping - message receieved", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:30 y:456, x:130 y:456 _sm_normal_activity_1 = OperatableStateMachine( outcomes=['finished', 'failed']) with _sm_normal_activity_1: # x:338 y:227 OperatableStateMachine.add('normalactivity', LogState(text="normal activity", severity=Logger.REPORT_HINT), transitions={'done': 'wait1'}, autonomy={'done': Autonomy.Off}) # x:618 y:312 OperatableStateMachine.add('wait1', WaitState(wait_time=1), transitions={'done': 'normalactivity'}, autonomy={'done': Autonomy.Off}) # x:113 y:364, x:465 y:451, x:737 y:425, x:254 y:385, x:221 y:448 _sm_container_2 = ConcurrencyContainer( outcomes=['finished', 'failed'], conditions=[('failed', [('normal_activity', 'failed'), ('safety_monitor', 'failed')]), ('finished', [('normal_activity', 'finished')]), ('finished', [('safety_monitor', 'finished')])]) with _sm_container_2: # x:149 y:54 OperatableStateMachine.add('normal_activity', _sm_normal_activity_1, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:676 y:54 OperatableStateMachine.add('safety_monitor', _sm_safety_monitor_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) with _state_machine: # x:317 y:100 OperatableStateMachine.add('Container', _sm_container_2, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) return _state_machine
def create(self): # x:787 y:587, x:280 y:361 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.none = None _state_machine.userdata.grasp_preference = 0 _state_machine.userdata.step_back_distance = 0.5 # m # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:686 y:240, x:384 y:196 _sm_perform_walking_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['none', 'step_back_distance']) with _sm_perform_walking_0: # x:64 y:78 OperatableStateMachine.add( 'Plan_Realign_Feet', FootstepPlanRealignCenterState(), transitions={ 'planned': 'Execute_Realign_Feet', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Low }, remapping={'plan_header': 'plan_header'}) # x:624 y:378 OperatableStateMachine.add( 'Perform_Step_Back', ExecuteStepPlanActionState(), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'plan_header': 'plan_header'}) # x:328 y:378 OperatableStateMachine.add( 'Plan_Step_Back', FootstepPlanRelativeState( direction=FootstepPlanRelativeState.DIRECTION_BACKWARD), transitions={ 'planned': 'Perform_Step_Back', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Low }, remapping={ 'distance': 'step_back_distance', 'plan_header': 'plan_header' }) # x:74 y:190 OperatableStateMachine.add( 'Execute_Realign_Feet', ExecuteStepPlanActionState(), transitions={ 'finished': 'Wait_For_Stand', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'plan_header': 'plan_header'}) # x:66 y:316 OperatableStateMachine.add( 'Wait_For_Stand', CheckCurrentControlModeState( target_mode=CheckCurrentControlModeState.STAND, wait=True), transitions={ 'correct': 'Plan_Step_Back', 'incorrect': 'failed' }, autonomy={ 'correct': Autonomy.Low, 'incorrect': Autonomy.Full }, remapping={'control_mode': 'control_mode'}) # x:733 y:290, x:133 y:290 _sm_back_to_stand_1 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['none']) with _sm_back_to_stand_1: # x:66 y:78 OperatableStateMachine.add( 'Set_Manipulate', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.MANIPULATE), transitions={ 'changed': 'Stand_Posture', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.High, 'failed': Autonomy.Low }) # x:376 y:78 OperatableStateMachine.add( 'Stand_Posture', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.3, ignore_collisions=False, link_paddings={}), transitions={ 'done': 'Set_Stand', 'failed': 'Set_Stand' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'side': 'none'}) # x:666 y:78 OperatableStateMachine.add( 'Set_Stand', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND), transitions={ 'changed': 'finished', 'failed': 'finished' }, autonomy={ 'changed': Autonomy.Low, 'failed': Autonomy.Low }) # x:120 y:404, x:298 y:222 _sm_step_back_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=[ 'hand_side', 'none', 'template_id', 'grasp_preference', 'step_back_distance' ]) with _sm_step_back_2: # x:76 y:28 OperatableStateMachine.add( 'Go_To_Stand_Posture', MoveitPredefinedPoseState( target_pose=MoveitPredefinedPoseState.STAND_POSE, vel_scaling=0.1, ignore_collisions=False, link_paddings={}), transitions={ 'done': 'Set_To_Stand_Manipulate', 'failed': 'Set_To_Stand_Manipulate' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'side': 'none'}) # x:66 y:140 OperatableStateMachine.add( 'Set_To_Stand_Manipulate', ChangeControlModeActionState( target_mode=ChangeControlModeActionState.STAND_MANIPULATE), transitions={ 'changed': 'Perform_Walking', 'failed': 'failed' }, autonomy={ 'changed': Autonomy.High, 'failed': Autonomy.Low }) # x:84 y:272 OperatableStateMachine.add('Perform_Walking', _sm_perform_walking_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'none': 'none', 'step_back_distance': 'step_back_distance' }) # x:755 y:48, x:401 y:428 _sm_preparation_3 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['none'], output_keys=['template_id']) with _sm_preparation_3: # x:66 y:164 OperatableStateMachine.add( 'Head_Look_Straight', TiltHeadState(desired_tilt=TiltHeadState.STRAIGHT), transitions={ 'done': 'Place_Template', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }) # x:566 y:163 OperatableStateMachine.add( 'Request_Template_ID', InputState(request=InputState.SELECTED_OBJECT_ID, message="Provide the ID of the placed template."), transitions={ 'received': 'finished', 'aborted': 'failed', 'no_connection': 'Log_No_Connection', 'data_error': 'Log_Data_Error' }, autonomy={ 'received': Autonomy.High, 'aborted': Autonomy.High, 'no_connection': Autonomy.Low, 'data_error': Autonomy.Low }, remapping={'data': 'template_id'}) # x:287 y:204 OperatableStateMachine.add('Log_No_Connection', LogState( text="Have no connection to OCS!", severity=Logger.REPORT_ERROR), transitions={'done': 'Decide_Input'}, autonomy={'done': Autonomy.Off}) # x:344 y:144 OperatableStateMachine.add('Log_Data_Error', LogState( text="Received wrong data format!", severity=Logger.REPORT_ERROR), transitions={'done': 'Decide_Input'}, autonomy={'done': Autonomy.Off}) # x:483 y:44 OperatableStateMachine.add( 'Fake_Input', CalculationState(calculation=lambda x: 0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'none', 'output_value': 'template_id' }) # x:236 y:40 OperatableStateMachine.add( 'Decide_Input', OperatorDecisionState( outcomes=['fake_id', 'ocs_request'], hint="How do you want to provide the template?", suggestion=None), transitions={ 'fake_id': 'Fake_Input', 'ocs_request': 'Request_Template_ID' }, autonomy={ 'fake_id': Autonomy.Full, 'ocs_request': Autonomy.Full }) # x:78 y:40 OperatableStateMachine.add( 'Place_Template', LogState(text="Please place the drill template.", severity=Logger.REPORT_HINT), transitions={'done': 'Decide_Input'}, autonomy={'done': Autonomy.Full}) with _state_machine: # x:44 y:72 OperatableStateMachine.add('Preparation', _sm_preparation_3, transitions={ 'finished': 'Ask_Perform_Walking', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'none': 'none', 'template_id': 'template_id' }) # x:547 y:571 OperatableStateMachine.add('Step_Back', _sm_step_back_2, transitions={ 'finished': 'finished', 'failed': 'Back_To_Stand' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'none': 'none', 'template_id': 'template_id', 'grasp_preference': 'grasp_preference', 'step_back_distance': 'step_back_distance' }) # x:284 y:78 OperatableStateMachine.add( 'Ask_Perform_Walking', OperatorDecisionState( outcomes=['walk', 'stand'], hint="Does the robot need to walk to the table?", suggestion='walk'), transitions={ 'walk': 'Walk to Template', 'stand': 'Manipulation Config' }, autonomy={ 'walk': Autonomy.High, 'stand': Autonomy.Full }) # x:531 y:22 OperatableStateMachine.add( 'Walk to Template', self.use_behavior(WalktoTemplateSM, 'Walk to Template'), transitions={ 'finished': 'Manipulation Config', 'failed': 'Walk_Manually', 'aborted': 'Walk_Manually' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'aborted': Autonomy.Inherit }, remapping={ 'grasp_preference': 'grasp_preference', 'hand_side': 'hand_side', 'template_id': 'template_id' }) # x:525 y:143 OperatableStateMachine.add( 'Manipulation Config', self.use_behavior(ManipulationConfigSM, 'Manipulation Config'), transitions={ 'finished': 'Head_Look_Down', 'failed': 'Manipulation_Config_Manually' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:264 y:228 OperatableStateMachine.add( 'Manipulation_Config_Manually', OperatorDecisionState( outcomes=["done", "abort"], hint="Make sure the robot is ready to grasp", suggestion=None), transitions={ 'done': 'Pickup Object', 'abort': 'failed' }, autonomy={ 'done': Autonomy.Full, 'abort': Autonomy.Full }) # x:769 y:78 OperatableStateMachine.add( 'Walk_Manually', LogState(text="Guide the robot to the template manually.", severity=Logger.REPORT_HINT), transitions={'done': 'Manipulation Config'}, autonomy={'done': Autonomy.Full}) # x:245 y:485 OperatableStateMachine.add('Back_To_Stand', _sm_back_to_stand_1, transitions={ 'finished': 'failed', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'none': 'none'}) # x:538 y:353 OperatableStateMachine.add('Pickup Object', self.use_behavior( PickupObjectSM, 'Pickup Object'), transitions={ 'finished': 'Head_Look_Straight', 'failed': 'Back_To_Stand' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'template_id': 'template_id' }) # x:545 y:257 OperatableStateMachine.add( 'Head_Look_Down', TiltHeadState(desired_tilt=TiltHeadState.DOWN_45), transitions={ 'done': 'Pickup Object', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }) # x:540 y:473 OperatableStateMachine.add( 'Head_Look_Straight', TiltHeadState(desired_tilt=TiltHeadState.STRAIGHT), transitions={ 'done': 'Step_Back', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }) return _state_machine
def create(self): cart_lift = 0.20 # meters arm_controller = ExecuteTrajectoryMsgState.CONTROLLER_LEFT_ARM if self.hand_side == 'left' else ExecuteTrajectoryMsgState.CONTROLLER_RIGHT_ARM wrist_frame_id = self.hand_side[0] + '_hand' # x:220 y:518, x:463 y:233 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id']) _state_machine.userdata.hand_side = self.hand_side _state_machine.userdata.template_id = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:71 y:304, x:597 y:174 _sm_lift_object_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['hand_side', 'template_id']) with _sm_lift_object_0: # x:255 y:28 OperatableStateMachine.add('Get_Wrist_Pose', GetWristPoseState(), transitions={ 'done': 'Set_Target_for_Lifting', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'hand_side': 'hand_side', 'wrist_pose': 'wrist_pose' }) # x:525 y:29 OperatableStateMachine.add( 'Set_Target_for_Lifting', CalculationState( calculation=self.lift_template(cart_lift, False)), transitions={'done': 'Transform_to_Wrist_Frame'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'wrist_pose', 'output_value': 'lift_target' }) # x:887 y:296 OperatableStateMachine.add( 'Prepare_Plan_Frame_Wrist', CalculationState( calculation=lambda pose: pose.header.frame_id), transitions={'done': 'Plan_Cartesian_Lift'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'lift_target_hand', 'output_value': 'hand_frame_id' }) # x:523 y:296 OperatableStateMachine.add( 'Plan_Cartesian_Lift', PlanEndeffectorCartesianWaypointsState( ignore_collisions=False, include_torso=False, keep_endeffector_orientation=True, allow_incomplete_plans=True, vel_scaling=0.1, planner_id="RRTConnectkConfigDefault"), transitions={ 'planned': 'Perform_Lifting', 'incomplete': 'Perform_Lifting', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.Low, 'incomplete': Autonomy.High, 'failed': Autonomy.High }, remapping={ 'waypoints': 'lift_waypoints', 'hand': 'hand_side', 'frame_id': 'hand_frame_id', 'joint_trajectory': 'joint_trajectory', 'plan_fraction': 'plan_fraction' }) # x:886 y:29 OperatableStateMachine.add( 'Transform_to_Wrist_Frame', GetPoseInFrameState(target_frame=wrist_frame_id), transitions={ 'done': 'Convert_to_List_of_Poses', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'pose_in': 'lift_target', 'pose_out': 'lift_target_hand' }) # x:888 y:160 OperatableStateMachine.add( 'Convert_to_List_of_Poses', CalculationState(calculation=lambda pose: [pose.pose]), transitions={'done': 'Prepare_Plan_Frame_Wrist'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'lift_target_hand', 'output_value': 'lift_waypoints' }) # x:206 y:297 OperatableStateMachine.add( 'Perform_Lifting', ExecuteTrajectoryMsgState(controller=arm_controller), transitions={ 'done': 'finished', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={'joint_trajectory': 'joint_trajectory'}) with _state_machine: # x:187 y:25 OperatableStateMachine.add('Grasp_Object', self.use_behavior( GraspObjectSM, 'Grasp_Object'), transitions={ 'finished': 'Confirm-Successful_Grasp', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'template_id': 'template_id' }) # x:181 y:275 OperatableStateMachine.add('Attach_Object_to_Hand', AttachObjectState(), transitions={ 'done': 'Lift_Object', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.High }, remapping={ 'template_id': 'template_id', 'hand_side': 'hand_side', 'template_pose': 'template_pose' }) # x:190 y:383 OperatableStateMachine.add('Lift_Object', _sm_lift_object_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'hand_side': 'hand_side', 'template_id': 'template_id' }) # x:176 y:159 OperatableStateMachine.add( 'Confirm-Successful_Grasp', LogState(text='Confirm that the grasp was successful!', severity=Logger.REPORT_HINT), transitions={'done': 'Attach_Object_to_Hand'}, autonomy={'done': Autonomy.Full}) return _state_machine
def create(self): # x:456 y:478, x:347 y:237 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365, x:487 y:245 _sm_guess_age_interaction_0 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_guess_age_interaction_0: # x:153 y:68 OperatableStateMachine.add('smile', FacialExpressionState(expression_num=1), transitions={'continue': 'wait 1', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:378 y:48 OperatableStateMachine.add('wait 1', WaitState(wait_time=1), transitions={'done': 'blink'}, autonomy={'done': Autonomy.Off}) # x:605 y:69 OperatableStateMachine.add('blink', FacialExpressionState(expression_num=2), transitions={'continue': 'introduction', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:817 y:120 OperatableStateMachine.add('introduction', TalkState(sentence_number=1), transitions={'continue': 'listen for name', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:919 y:202 OperatableStateMachine.add('listen for name', ListeningState(), transitions={'continue': 'welcome to', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1004 y:291 OperatableStateMachine.add('welcome to', TalkState(sentence_number=2), transitions={'continue': 'sub please take my photo', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1027 y:401 OperatableStateMachine.add('sub please take my photo', SubscriberState(topic='/stt_topic', blocking=True, clear=False), transitions={'received': 'cond please take my photo', 'unavailable': 'failed'}, autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off}, remapping={'message': 'message'}) # x:1025 y:515 OperatableStateMachine.add('cond please take my photo', CheckConditionState(predicate=lambda message:message.data =="please take my photo"), transitions={'true': 'cool go ahead', 'false': 'not heard take photo'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'message'}) # x:823 y:458 OperatableStateMachine.add('not heard take photo', LogState(text=not yet heard "please take my photo", severity=Logger.REPORT_HINT), transitions={'done': 'sub please take my photo'}, autonomy={'done': Autonomy.Off}) # x:938 y:641 OperatableStateMachine.add('cool go ahead', TalkState(sentence_number=3), transitions={'continue': 'finished', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:161 y:371, x:268 y:170 _sm_start_face_det_and_trk_1 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_start_face_det_and_trk_1: # x:149 y:36 OperatableStateMachine.add('say face found', TalkState(sentence_number=100), transitions={'continue': 'face det trk', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:519 y:73 OperatableStateMachine.add('face det trk', FaceDetTrack(), transitions={'continue': 'say start face det trk', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:563 y:328 OperatableStateMachine.add('say start face det trk', TalkState(sentence_number=101), transitions={'continue': 'finished', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:128 y:223, x:130 y:365 _sm_stop_video_detects_and_tracks_2 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_stop_video_detects_and_tracks_2: # x:135 y:64 OperatableStateMachine.add('stop obj det trk', StopFaceDetectAndTrack(), transitions={'continue': 'stop face det trk', 'failed': 'stop face det trk'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:410 y:78 OperatableStateMachine.add('stop face det trk', StopFaceDetectAndTrack(), transitions={'continue': 'stop video stream', 'failed': 'stop video stream'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:466 y:250 OperatableStateMachine.add('stop video stream', StopCameraStream(), transitions={'continue': 'finished', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:438 y:355, x:121 y:296 _sm_check_face_nearby_3 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_check_face_nearby_3: # x:237 y:77 OperatableStateMachine.add('subscr face found topic', SubscriberState(topic='/is_person_nearby', blocking=True, clear=False), transitions={'received': 'face condition', 'unavailable': 'failed'}, autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off}, remapping={'message': 'message'}) # x:521 y:63 OperatableStateMachine.add('face condition', CheckConditionState(predicate=lambda message:message.data == "yes"), transitions={'true': 'finished', 'false': 'no face found yet'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'message'}) # x:297 y:218 OperatableStateMachine.add('no face found yet', LogState(text="no face found yet", severity=Logger.REPORT_HINT), transitions={'done': 'subscr face found topic'}, autonomy={'done': Autonomy.Off}) # x:30 y:365, x:132 y:205 _sm_launch_stream_and_object_detector_4 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_launch_stream_and_object_detector_4: # x:126 y:42 OperatableStateMachine.add('launch video stream', LaunchVideoStream(), transitions={'continue': 'say launching video', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:389 y:56 OperatableStateMachine.add('say launching video', TalkState(sentence_number=99), transitions={'continue': 'launch obj det trk', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:483 y:152 OperatableStateMachine.add('launch obj det trk', LaunchObjDetectAndTrack(), transitions={'continue': 'finished', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) with _state_machine: # x:135 y:38 OperatableStateMachine.add('launch stream and object detector', _sm_launch_stream_and_object_detector_4, transitions={'finished': 'check face nearby', 'failed': 'stop video detects and tracks'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:469 y:33 OperatableStateMachine.add('check face nearby', _sm_check_face_nearby_3, transitions={'finished': 'stop obj det trk', 'failed': 'stop video detects and tracks'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:546 y:220 OperatableStateMachine.add('stop video detects and tracks', _sm_stop_video_detects_and_tracks_2, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:701 y:69 OperatableStateMachine.add('stop obj det trk', StopObjectDetectAndTrack(), transitions={'continue': 'start face det and trk', 'failed': 'stop video detects and tracks'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:892 y:135 OperatableStateMachine.add('start face det and trk', _sm_start_face_det_and_trk_1, transitions={'finished': 'guess age interaction', 'failed': 'stop video detects and tracks'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:887 y:356 OperatableStateMachine.add('guess age interaction', _sm_guess_age_interaction_0, transitions={'finished': 'finished', 'failed': 'stop video detects and tracks'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) return _state_machine
def create(self): test = "test" hello = "hello World!" # x:1142 y:435, x:1117 y:356 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:104 y:43 OperatableStateMachine.add('Initial_Wait', WaitState(wait_time=self.waiting_time), transitions={'done': 'Print_Greeting'}, autonomy={'done': Autonomy.Off}) # x:920 y:287 OperatableStateMachine.add('Print_Bye', LogState(text="bye bye", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:285 y:53 OperatableStateMachine.add('Print_Greeting', LogState(text=hello, severity=Logger.REPORT_HINT), transitions={'done': 'state1'}, autonomy={'done': Autonomy.Off}) # x:785 y:217 OperatableStateMachine.add('Initial_Wait_2', WaitState(wait_time=5), transitions={'done': 'Print_Bye'}, autonomy={'done': Autonomy.Off}) # x:667 y:123 OperatableStateMachine.add('Log_test', LogState(text=test, severity=Logger.REPORT_HINT), transitions={'done': 'Initial_Wait_2'}, autonomy={'done': Autonomy.Off}) # x:446 y:78 OperatableStateMachine.add('state1', TestPublisherState(), transitions={'done': 'Show_Current_State_Page'}, autonomy={'done': Autonomy.Off}) # x:601 y:339 OperatableStateMachine.add('Show_Modal', tiago_flexbe_states__WebsiteDummyModalState(), transitions={'done': 'Log_test', 'failed': 'Show_Modal'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}) # x:388 y:280 OperatableStateMachine.add('Show_Current_State_Page', WebsiteDummyState1(), transitions={'done': 'Show_Modal', 'failed': 'Show_Current_State_Page'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}) return _state_machine
def create(self): # x:283 y:267 _state_machine = OperatableStateMachine(outcomes=['Shutdown']) _state_machine.userdata.Command = "no nothing" _state_machine.userdata.End = False # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:887 y:420 _sm_sara_brain_0 = OperatableStateMachine( outcomes=['error'], input_keys=['HighFIFO', 'LowFIFO', 'MedFIFO', 'DoNow', 'End']) with _sm_sara_brain_0: # x:270 y:346 OperatableStateMachine.add( 'sara_command_manager', self.use_behavior( sara_command_managerSM, 'Sara parallel Runtime/Sara brain/sara_command_manager'), transitions={ 'finished': 'if stop', 'failed': 'set end' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'DoNow': 'DoNow' }) # x:725 y:388 OperatableStateMachine.add( 'set end', CalculationState(calculation=lambda x: True), transitions={'done': 'error'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'End', 'output_value': 'End' }) # x:516 y:448 OperatableStateMachine.add( 'if stop', CheckConditionState(predicate=lambda x: x[0][0] == "Stop"), transitions={ 'true': 'set end', 'false': 'sara_command_manager' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'DoNow'}) # x:841 y:231 _sm_sara_action_executor_1 = OperatableStateMachine( outcomes=['shutdown'], input_keys=['HighFIFO', 'MedFIFO', 'LowFIFO', 'DoNow', 'End']) with _sm_sara_action_executor_1: # x:128 y:134 OperatableStateMachine.add( 'log', LogState(text="Start action executor", severity=Logger.REPORT_HINT), transitions={'done': 'Sara action executor'}, autonomy={'done': Autonomy.Off}) # x:636 y:111 OperatableStateMachine.add( 'Critical failure', LogState(text="Critical fail in action executer!", severity=Logger.REPORT_HINT), transitions={'done': 'shutdown'}, autonomy={'done': Autonomy.Off}) # x:366 y:149 OperatableStateMachine.add( 'Sara action executor', self.use_behavior( SaraactionexecutorSM, 'Sara parallel Runtime/Sara action executor/Sara action executor' ), transitions={ 'CriticalFail': 'Critical failure', 'Shutdown': 'shutdown' }, autonomy={ 'CriticalFail': Autonomy.Inherit, 'Shutdown': Autonomy.Inherit }, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'End': 'End' }) # x:30 y:365 _sm_sara_init_2 = OperatableStateMachine(outcomes=['done']) with _sm_sara_init_2: # x:30 y:40 OperatableStateMachine.add('set head', SaraSetHeadAngle(angle=0.1), transitions={'done': 'set face'}, autonomy={'done': Autonomy.Off}) # x:47 y:119 OperatableStateMachine.add('set arm', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'hello', 'failed': 'hello' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'target'}) # x:242 y:288 OperatableStateMachine.add( 'hello', SaraSay( sentence= "Good morning. I am Sara the robot. Please give me orders.", emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:239 y:21 OperatableStateMachine.add('set face', SetExpression(emotion=0, brightness=200), transitions={'done': 'www'}, autonomy={'done': Autonomy.Off}) # x:339 y:116 OperatableStateMachine.add('on face', SetExpression(emotion=1, brightness=-1), transitions={'done': 'setTarget'}, autonomy={'done': Autonomy.Off}) # x:484 y:41 OperatableStateMachine.add('www', WaitState(wait_time=2), transitions={'done': 'on face'}, autonomy={'done': Autonomy.Off}) # x:197 y:113 OperatableStateMachine.add('setTarget', SetKey(Value="IdlePose"), transitions={'done': 'set arm'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:55 y:366 _sm_create_fifos_3 = OperatableStateMachine( outcomes=['done'], output_keys=['HighFIFO', 'MedFIFO', 'LowFIFO', 'DoNow']) with _sm_create_fifos_3: # x:33 y:40 OperatableStateMachine.add('Create HighFIFO', FIFO_New(), transitions={'done': 'Create MedFIFO'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'HighFIFO'}) # x:30 y:114 OperatableStateMachine.add('Create MedFIFO', FIFO_New(), transitions={'done': 'Create LowFIFO'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'MedFIFO'}) # x:30 y:189 OperatableStateMachine.add('Create LowFIFO', FIFO_New(), transitions={'done': 'Create DoNow'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'LowFIFO'}) # x:34 y:260 OperatableStateMachine.add('Create DoNow', FIFO_New(), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'DoNow'}) # x:30 y:322 _sm_sara_shutdown_4 = OperatableStateMachine(outcomes=['finished']) with _sm_sara_shutdown_4: # x:57 y:86 OperatableStateMachine.add('log', LogState(text="shutdown", severity=Logger.REPORT_HINT), transitions={'done': 'say'}, autonomy={'done': Autonomy.Off}) # x:122 y:228 OperatableStateMachine.add( 'say', SaraSay(sentence="I'm goint to shutdown for safety reasons", emotion=1, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:336 y:314, x:315 y:433, x:276 y:271 _sm_sara_parallel_runtime_5 = ConcurrencyContainer( outcomes=['Shutdown'], input_keys=['HighFIFO', 'MedFIFO', 'LowFIFO', 'DoNow', 'End'], conditions=[('Shutdown', [('Sara brain', 'error')]), ('Shutdown', [('Sara action executor', 'shutdown')])]) with _sm_sara_parallel_runtime_5: # x:52 y:416 OperatableStateMachine.add('Sara action executor', _sm_sara_action_executor_1, transitions={'shutdown': 'Shutdown'}, autonomy={'shutdown': Autonomy.Inherit}, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'DoNow': 'DoNow', 'End': 'End' }) # x:57 y:333 OperatableStateMachine.add('Sara brain', _sm_sara_brain_0, transitions={'error': 'Shutdown'}, autonomy={'error': Autonomy.Inherit}, remapping={ 'HighFIFO': 'HighFIFO', 'LowFIFO': 'LowFIFO', 'MedFIFO': 'MedFIFO', 'DoNow': 'DoNow', 'End': 'End' }) with _state_machine: # x:43 y:60 OperatableStateMachine.add('log', LogState(text="Start Sara", severity=Logger.REPORT_HINT), transitions={'done': 'low head'}, autonomy={'done': Autonomy.Off}) # x:306 y:365 OperatableStateMachine.add( 'Sara parallel Runtime', _sm_sara_parallel_runtime_5, transitions={'Shutdown': 'Sara shutdown'}, autonomy={'Shutdown': Autonomy.Inherit}, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'DoNow': 'DoNow', 'End': 'End' }) # x:315 y:462 OperatableStateMachine.add('Sara shutdown', _sm_sara_shutdown_4, transitions={'finished': 'low head'}, autonomy={'finished': Autonomy.Inherit}) # x:328 y:284 OperatableStateMachine.add( 'Create_FIFOs', _sm_create_fifos_3, transitions={'done': 'Sara parallel Runtime'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'DoNow': 'DoNow' }) # x:150 y:207 OperatableStateMachine.add('listen', GetSpeech(watchdog=10), transitions={ 'done': 'check hello', 'nothing': 'listen', 'fail': 'Shutdown' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'words'}) # x:345 y:195 OperatableStateMachine.add('Sara init', _sm_sara_init_2, transitions={'done': 'Create_FIFOs'}, autonomy={'done': Autonomy.Inherit}) # x:355 y:96 OperatableStateMachine.add( 'check hello', RegexTester( regex= ".*((wake up)|(sarah?)|(shut up)|(hello)(robot)|(hi)|(morning)|(greet)).*" ), transitions={ 'true': 'Sara init', 'false': 'listen' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'text': 'words', 'result': 'result' }) # x:15 y:220 OperatableStateMachine.add('close face', SetExpression(emotion=0, brightness=20), transitions={'done': 'listen'}, autonomy={'done': Autonomy.Off}) # x:102 y:115 OperatableStateMachine.add('low head', SaraSetHeadAngle(angle=0.8), transitions={'done': 'close face'}, autonomy={'done': Autonomy.Off}) return _state_machine