def create(self): # x:42 y:502, x:623 y:168 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['EntityId'], output_keys=['ContainerId']) _state_machine.userdata.EntityId = 45 _state_machine.userdata.ContainerId = "" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:111 y:46 OperatableStateMachine.add('GetWonderlandEntity', WonderlandGetEntityByID(), transitions={'found': 'GetContainer', 'not_found': 'failed', 'error': 'failed'}, autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off, 'error': Autonomy.Off}, remapping={'id': 'EntityId', 'entity': 'Entity', 'depth_position': 'depth_position', 'depth_waypoint': 'depth_waypoint'}) # x:257 y:303 OperatableStateMachine.add('GetWonderlandEntity_2', WonderlandGetEntityByID(), transitions={'found': 'ClimbALevel', 'not_found': 'ThisIsTheContainer', 'error': 'ThisIsTheContainer'}, autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off, 'error': Autonomy.Off}, remapping={'id': 'ContainerId', 'entity': 'Entity', 'depth_position': 'depth_position', 'depth_waypoint': 'depth_waypoint'}) # x:245 y:175 OperatableStateMachine.add('ClimbALevel', CalculationState(calculation=lambda x: x), transitions={'done': 'GetWonderlandEntity'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'ContainerId', 'output_value': 'EntityId'}) # x:126 y:432 OperatableStateMachine.add('ThisIsTheContainer', CalculationState(calculation=lambda x: x), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'EntityId', 'output_value': 'ContainerId'}) # x:29 y:172 OperatableStateMachine.add('GetContainer', CalculationState(calculation=lambda x: x.containerId), transitions={'done': 'if as container'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Entity', 'output_value': 'ContainerId'}) # x:39 y:291 OperatableStateMachine.add('if as container', CheckConditionState(predicate=lambda x: x.containerId), transitions={'true': 'GetWonderlandEntity_2', 'false': 'ThisIsTheContainer'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'Entity'}) return _state_machine
def create(self): # x:572 y:333, x:130 y:365 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) _state_machine.userdata.unused = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:170 y:138 OperatableStateMachine.add('SelectAction', CalculationState(calculation=lambda x: random.choice(['brohoof', 'menace'])), transitions={'done': 'ExecuteCompoundAction'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'unused', 'output_value': 'action_param'}) # x:330 y:170 OperatableStateMachine.add('ExecuteCompoundAction', CompoundActionParamKey(action_ns='saved_msgs/compound_action'), transitions={'success': 'finished', 'invalid_pose': 'failed', 'failure': 'failed'}, autonomy={'success': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off}, remapping={'action_param': 'action_param'}) return _state_machine
def create(self): # x:30 y:365, x:599 y:427 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:378 y:183 OperatableStateMachine.add('start', InputState(request=integer, message=eeeee), transitions={'received': 'eeeeeeeeeee', 'aborted': 'failed', 'no_connection': 'eeeeeeeeeee', 'data_error': 'finished'}, autonomy={'received': Autonomy.Off, 'aborted': Autonomy.Off, 'no_connection': Autonomy.Off, 'data_error': Autonomy.Off}, remapping={'data': 'data'}) # x:1115 y:242 OperatableStateMachine.add('eeeeeeeeeee', DecisionState(outcomes="LOL", conditions=input_value > 0), transitions={'LOL': 'failed'}, autonomy={'LOL': Autonomy.Off}, remapping={'input_value': 'data'}) # x:808 y:383 OperatableStateMachine.add('lmFAOR', CalculationState(calculation=54x-5w), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'input_value', 'output_value': 'output_value'}) return _state_machine
def create(self): # x:67 y:463, x:336 y:160 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['data']) _state_machine.userdata.data = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:30 y:40 OperatableStateMachine.add('Wait', WaitState(wait_time=0.5), transitions={'done': 'Calculate'}, autonomy={'done': Autonomy.Off}) # x:36 y:240 OperatableStateMachine.add('Log Param', flexbe_states__LogState(text=self.param, severity=2), transitions={'done': 'Verify Input'}, autonomy={'done': Autonomy.Off}) # x:32 y:340 OperatableStateMachine.add('Verify Input', DecisionState( outcomes=['accepted', 'rejected'], conditions=lambda x: 'accepted' if x > 3 else 'rejected'), transitions={ 'accepted': 'finished', 'rejected': 'failed' }, autonomy={ 'accepted': Autonomy.Off, 'rejected': Autonomy.Off }, remapping={'input_value': 'data'}) # x:28 y:136 OperatableStateMachine.add( 'Calculate', CalculationState(calculation=self._calculate), transitions={'done': 'Log Param'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'data', 'output_value': 'data' }) return _state_machine
def create(self): # x:30 y:365, x:130 y:365 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['data'], output_keys=['result']) _state_machine.userdata.data = None _state_machine.userdata.result = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:40 y:73 OperatableStateMachine.add( 'Modify Data', CalculationState(calculation=lambda x: x * 2), transitions={'done': 'Decide Param'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'data', 'output_value': 'result' }) # x:37 y:201 OperatableStateMachine.add( 'Decide Param', DecisionState(outcomes=['finished', 'failed'], conditions=lambda x: 'finished' if self.value == 'correct' else 'failed'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'input_value': 'data'}) return _state_machine
def create(self): # x:67 y:337, x:285 y:147 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], output_keys=['words']) _state_machine.userdata.words = "" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:38 y:133 OperatableStateMachine.add('subber', SubscriberState(topic="/sara_command", blocking=True, clear=True), transitions={ 'received': 'get speech', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:40 y:234 OperatableStateMachine.add( 'get speech', CalculationState(calculation=lambda x: x.data), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'message', 'output_value': 'words' }) return _state_machine
def create(self): # x:72 y:443, x:514 y:143 _state_machine = OperatableStateMachine(outcomes=['done', 'pas_done'], input_keys=['className'], output_keys=['entity']) _state_machine.userdata.className = "person" _state_machine.userdata.entity = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:706 y:647 _sm_rotation_0 = OperatableStateMachine(outcomes=['end']) with _sm_rotation_0: # x:51 y:38 OperatableStateMachine.add('Set 180 degres', SetKey(Value=3.1416), transitions={'done': 'Look Center'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:613 y:470 OperatableStateMachine.add('action_turn', self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'Container/Rotation/action_turn'), transitions={'finished': 'Look Right', 'failed': 'Look Right'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'rotation': 'rotation'}) # x:421 y:54 OperatableStateMachine.add('Look Right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:265 y:56 OperatableStateMachine.add('w1', WaitState(wait_time=4), transitions={'done': 'Look Right'}, autonomy={'done': Autonomy.Off}) # x:630 y:56 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'center'}, autonomy={'done': Autonomy.Off}) # x:250 y:177 OperatableStateMachine.add('Look Center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:618 y:304 OperatableStateMachine.add('Look Left 2', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'w4'}, autonomy={'done': Autonomy.Off}) # x:612 y:138 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:635 y:214 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'Look Left 2'}, autonomy={'done': Autonomy.Off}) # x:636 y:394 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}) # x:683 y:188 _sm_find_entity_1 = OperatableStateMachine(outcomes=['found'], input_keys=['className'], output_keys=['entity']) with _sm_find_entity_1: # x:181 y:178 OperatableStateMachine.add('find_entity', list_entities_by_name(frontality_level=0.5, distance_max=4), transitions={'found': 'Get Entity', 'none_found': 'find_entity'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'className', 'entity_list': 'entity_list', 'number': 'number'}) # x:454 y:178 OperatableStateMachine.add('Get Entity', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity_list', 'output_value': 'entity'}) # x:194 y:40 OperatableStateMachine.add('WaitState', WaitState(wait_time=1), transitions={'done': 'find_entity'}, autonomy={'done': Autonomy.Off}) # x:372 y:27, x:370 y:220, x:368 y:100, x:330 y:458 _sm_container_2 = ConcurrencyContainer(outcomes=['found', 'not_found'], input_keys=['className'], output_keys=['entity'], conditions=[ ('not_found', [('Rotation', 'end')]), ('found', [('Find Entity', 'found')]) ]) with _sm_container_2: # x:131 y:44 OperatableStateMachine.add('Find Entity', _sm_find_entity_1, transitions={'found': 'found'}, autonomy={'found': Autonomy.Inherit}, remapping={'className': 'className', 'entity': 'entity'}) # x:129 y:197 OperatableStateMachine.add('Rotation', _sm_rotation_0, transitions={'end': 'not_found'}, autonomy={'end': Autonomy.Inherit}) with _state_machine: # x:67 y:42 OperatableStateMachine.add('Look Center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'Container'}, autonomy={'done': Autonomy.Off}) # x:278 y:138 OperatableStateMachine.add('Look Center Not Found', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'pas_done'}, autonomy={'done': Autonomy.Off}) # x:58 y:326 OperatableStateMachine.add('Log Entity', LogKeyState(text="Found entity: {}", severity=Logger.REPORT_HINT), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'entity'}) # x:63 y:126 OperatableStateMachine.add('Container', _sm_container_2, transitions={'found': 'WaitState', 'not_found': 'Look Center Not Found'}, autonomy={'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit}, remapping={'className': 'className', 'entity': 'entity'}) # x:67 y:222 OperatableStateMachine.add('WaitState', WaitState(wait_time=1), transitions={'done': 'Log Entity'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): joy_topic = '/hmi/joystick' # x:33 y:423, x:683 y:568 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['be_evil']) _state_machine.userdata.cycle_counter = 0 _state_machine.userdata.be_evil = self.be_evil # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:356, x:644 y:383 _sm_detectjoyevent_0 = OperatableStateMachine( outcomes=['received', 'unavailable']) with _sm_detectjoyevent_0: # x:229 y:154 OperatableStateMachine.add('JoyEvent', SubscriberState(topic=joy_topic, blocking=True, clear=False), transitions={ 'received': 'CheckEvent', 'unavailable': 'unavailable' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:229 y:296 OperatableStateMachine.add( 'CheckEvent', DecisionState( outcomes=['yes', 'no'], conditions=lambda msg: 'yes' if any(msg.buttons) or any(msg.axes[0:5]) else 'no'), transitions={ 'yes': 'received', 'no': 'JoyEvent' }, autonomy={ 'yes': Autonomy.Off, 'no': Autonomy.Off }, remapping={'input_value': 'message'}) # x:778 y:316, x:75 y:354, x:294 y:358, x:776 y:358, x:777 y:406, x:530 y:356 _sm_checkjoy_1 = ConcurrencyContainer( outcomes=['no_activity', 'activity_detected', 'failed'], conditions=[('no_activity', [('WaitForJoy', 'done')]), ('failed', [('DetectJoyEvent', 'unavailable')]), ('activity_detected', [('DetectJoyEvent', 'received')]) ]) with _sm_checkjoy_1: # x:219 y:132 OperatableStateMachine.add('DetectJoyEvent', _sm_detectjoyevent_0, transitions={ 'received': 'activity_detected', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Inherit, 'unavailable': Autonomy.Inherit }) # x:624 y:152 OperatableStateMachine.add('WaitForJoy', WaitState(wait_time=self.wait_time), transitions={'done': 'no_activity'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:646 y:31 OperatableStateMachine.add( 'RandomChoose', DecisionState(outcomes=['greeting', 'play', 'cheer', 'bad'], conditions=self.select_behavior), transitions={ 'greeting': 'Greeting', 'play': 'Play', 'cheer': 'Cheer', 'bad': 'Bad' }, autonomy={ 'greeting': Autonomy.Low, 'play': Autonomy.Low, 'cheer': Autonomy.Low, 'bad': Autonomy.Low }, remapping={'input_value': 'cycle_counter'}) # x:383 y:460 OperatableStateMachine.add('CheckJoy', _sm_checkjoy_1, transitions={ 'no_activity': 'ResetCounter', 'activity_detected': 'AddCycleCounter', 'failed': 'failed' }, autonomy={ 'no_activity': Autonomy.Inherit, 'activity_detected': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:221 y:134 OperatableStateMachine.add( 'AddCycleCounter', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'RandomChoose'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'cycle_counter', 'output_value': 'cycle_counter' }) # x:483 y:171 OperatableStateMachine.add('Greeting', self.use_behavior( GreetingSM, 'Greeting'), transitions={ 'finished': 'CheckJoy', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'be_evil': 'be_evil'}) # x:849 y:197 OperatableStateMachine.add('Cheer', self.use_behavior(CheerSM, 'Cheer'), transitions={ 'finished': 'CheckJoy', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'be_evil': 'be_evil'}) # x:687 y:197 OperatableStateMachine.add('Play', self.use_behavior(PlaySM, 'Play'), transitions={ 'finished': 'CheckJoy', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'be_evil': 'be_evil'}) # x:991 y:203 OperatableStateMachine.add('Bad', self.use_behavior(BadSM, 'Bad'), transitions={ 'finished': 'CheckJoy', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'be_evil': 'be_evil'}) # x:115 y:364 OperatableStateMachine.add( 'ResetCounter', CalculationState(calculation=lambda x: 0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'cycle_counter', 'output_value': 'cycle_counter' }) return _state_machine
def create(self): # x:267 y:194, x:706 y:131, x:92 y:292 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Move", "zone"] _state_machine.userdata.relative = False # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:131 y:459, x:447 y:470 _sm_try_to_reach_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['Action']) with _sm_try_to_reach_0: # x:84 y:67 OperatableStateMachine.add( 'get destination', CalculationState(calculation=lambda x: x[1:]), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Action', 'output_value': 'destination' }) # x:258 y:149 OperatableStateMachine.add( 'Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'Try to reach/Action_Move'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'destination'}) with _state_machine: # x:65 y:108 OperatableStateMachine.add('Try to reach', _sm_try_to_reach_0, transitions={ 'finished': 'finished', 'failed': 'gen fail cause' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'Action': 'Action'}) # x:532 y:113 OperatableStateMachine.add( 'paramoffailure', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'FailureCause'}) # x:290 y:110 OperatableStateMachine.add( 'gen fail cause', CalculationState(calculation=lambda x: "I couldn't reach the " + x[1] + "."), transitions={'done': 'paramoffailure'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Action', 'output_value': 'FailureCause' }) return _state_machine
def create(self): waypoints = [(1.0, 0.0)] # list of (x,y) orientations = [0] # list of angles (degrees) # x:1329 y:218, x:1324 y:61 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.target_pose = None _state_machine.userdata.target_pose_index = 0 _state_machine.userdata.bagfile_name = '' # calculated # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # If the user doesn't specify any waypoints, default to these if not waypoints: waypoints = [(1.0, 0.0), (2.0, 0.26795), (2.73205, 1.0), (2.0, 0.26795)] orientations = [0, 30, 60, 30] orientations = [ang * math.pi / 180 for ang in orientations] # convert to radians # Prepare a list of pose stamped waypoints for use by the footstep planning state self._poses_to_visit = list() for i in range(0, len(waypoints)): pt = Point(x=waypoints[i][0], y=waypoints[i][1]) qt = tf.transformations.quaternion_from_euler( 0, 0, orientations[i]) p = Pose(position=pt, orientation=Quaternion(*qt)) pose_stamped = PoseStamped(header=Header(frame_id='/world'), pose=p) self._poses_to_visit.append(pose_stamped) # Topics of interest will be rosbag recorded and the bagfiles will be stored in the folder below logs_folder = os.path.expanduser('~/footstep_tests/') if not os.path.exists(logs_folder): os.makedirs(logs_folder) _state_machine.userdata.bagfile_name = logs_folder + "run_" + time.strftime( "%Y-%m-%d-%H_%M") + ".bag" # [/MANUAL_CREATE] # x:730 y:38, x:328 y:117 _sm_perform_walking_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['target_pose']) with _sm_perform_walking_0: # x:157 y:27 OperatableStateMachine.add( 'Create_Step_Goal', CreateStepGoalState(pose_is_pelvis=False), transitions={ 'done': 'Plan_To_Next_Pose', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={ 'target_pose': 'target_pose', 'step_goal': 'step_goal' }) # x:401 y:188 OperatableStateMachine.add( 'Print_The_Footstep_Plan', CalculationState(calculation=self.print_plan), transitions={'done': 'Execute_Footstep_Plan'}, autonomy={'done': Autonomy.High}, remapping={ 'input_value': 'plan_header', 'output_value': 'output_value' }) # x:398 y:30 OperatableStateMachine.add( 'Execute_Footstep_Plan', ExecuteStepPlanActionState(), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'plan_header': 'plan_header'}) # x:156 y:188 OperatableStateMachine.add( 'Plan_To_Next_Pose', PlanFootstepsState( mode=PlanFootstepsState.MODE_STEP_NO_COLLISION), transitions={ 'planned': 'Print_The_Footstep_Plan', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'step_goal': 'step_goal', 'plan_header': 'plan_header' }) # x:1253 y:294, x:470 y:218 _sm_test_ocs_modified_plan_1 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['target_pose_index']) with _sm_test_ocs_modified_plan_1: # x:116 y:56 OperatableStateMachine.add( 'Set_Target_First_Pose', CalculationState( calculation=lambda x: self._poses_to_visit[0]), transitions={'done': 'Create_Goal'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'target_pose_index', 'output_value': 'target_pose' }) # x:707 y:54 OperatableStateMachine.add( 'Request_Plan', PlanFootstepsState( mode=PlanFootstepsState.MODE_STEP_NO_COLLISION), transitions={ 'planned': 'Print_Original_Plan', 'failed': 'failed' }, autonomy={ 'planned': Autonomy.High, 'failed': Autonomy.Low }, remapping={ 'step_goal': 'step_goal', 'plan_header': 'plan_header' }) # x:1018 y:128 OperatableStateMachine.add( 'Modify', InputState( request=InputState.FOOTSTEP_PLAN_HEADER, message='Modify plan (if necessary) and then confirm.'), transitions={ 'received': 'Print_Modified_Plan', 'aborted': 'failed', 'no_connection': 'failed', 'data_error': 'failed' }, autonomy={ 'received': Autonomy.High, 'aborted': Autonomy.Low, 'no_connection': Autonomy.Low, 'data_error': Autonomy.Low }, remapping={'data': 'plan_header_new'}) # x:971 y:358 OperatableStateMachine.add( 'Execute', ExecuteStepPlanActionState(), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Low, 'failed': Autonomy.Low }, remapping={'plan_header': 'plan_header_new'}) # x:431 y:56 OperatableStateMachine.add( 'Create_Goal', CreateStepGoalState(pose_is_pelvis=False), transitions={ 'done': 'Request_Plan', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Low, 'failed': Autonomy.Low }, remapping={ 'target_pose': 'target_pose', 'step_goal': 'step_goal' }) # x:1005 y:228 OperatableStateMachine.add( 'Print_Modified_Plan', CalculationState(calculation=self.print_plan), transitions={'done': 'Execute'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'plan_header_new', 'output_value': 'output_value' }) # x:989 y:28 OperatableStateMachine.add( 'Print_Original_Plan', CalculationState(calculation=self.print_plan), transitions={'done': 'Modify'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'plan_header', 'output_value': 'output_value' }) # x:8 y:123, x:561 y:146 _sm_test_waypoint_following_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['target_pose_index']) with _sm_test_waypoint_following_2: # x:361 y:28 OperatableStateMachine.add( 'Decide_Next_Target_Pose', DecisionState(outcomes=['continue', 'finished'], conditions=lambda x: 'continue' if x < len(waypoints) else 'finished'), transitions={ 'continue': 'Set_Next_Target_Pose', 'finished': 'finished' }, autonomy={ 'continue': Autonomy.Low, 'finished': Autonomy.High }, remapping={'input_value': 'target_pose_index'}) # x:349 y:254 OperatableStateMachine.add( 'Increment_To_Next_Target_Pose', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Decide_Next_Target_Pose'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'target_pose_index', 'output_value': 'target_pose_index' }) # x:699 y:30 OperatableStateMachine.add( 'Set_Next_Target_Pose', CalculationState( calculation=lambda x: self._poses_to_visit[x]), transitions={'done': 'Perform_Walking'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'target_pose_index', 'output_value': 'target_pose' }) # x:679 y:254 OperatableStateMachine.add( 'Wait_For_Stand', CheckCurrentControlModeState( target_mode=CheckCurrentControlModeState.STAND, wait=True), transitions={ 'correct': 'Increment_To_Next_Target_Pose', 'incorrect': 'failed' }, autonomy={ 'correct': Autonomy.High, 'incorrect': Autonomy.Low }, remapping={'control_mode': 'control_mode'}) # x:699 y:132 OperatableStateMachine.add( 'Perform_Walking', _sm_perform_walking_0, transitions={ 'finished': 'Wait_For_Stand', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'target_pose': 'target_pose'}) with _state_machine: # x:82 y:28 OperatableStateMachine.add( 'Start_Recording', StartRecordLogsState(topics_to_record=self.topics_to_record), transitions={'logging': 'Wait_For_Rosbag_Process'}, autonomy={'logging': Autonomy.Off}, remapping={ 'bagfile_name': 'bagfile_name', 'rosbag_process': 'rosbag_process' }) # x:1050 y:211 OperatableStateMachine.add( 'Stop_Recording_If_Finished', StopRecordLogsState(), transitions={'stopped': 'finished'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process'}) # x:1035 y:52 OperatableStateMachine.add( 'Stop_Recording_Before_Failed', StopRecordLogsState(), transitions={'stopped': 'failed'}, autonomy={'stopped': Autonomy.Off}, remapping={'rosbag_process': 'rosbag_process'}) # x:74 y:110 OperatableStateMachine.add( 'Wait_For_Rosbag_Process', WaitState(wait_time=2), transitions={'done': 'Decide_Test_Type'}, autonomy={'done': Autonomy.Off}) # x:570 y:28 OperatableStateMachine.add( 'Test_Waypoint_Following', _sm_test_waypoint_following_2, transitions={ 'finished': 'Stop_Recording_If_Finished', 'failed': 'Stop_Recording_Before_Failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'target_pose_index': 'target_pose_index'}) # x:357 y:110 OperatableStateMachine.add('Decide_Test_Type', OperatorDecisionState(outcomes=[ 'waypoints', 'wide_stance', 'modified' ], hint=None, suggestion=None), transitions={ 'waypoints': 'Test_Waypoint_Following', 'wide_stance': 'Manipulation Config', 'modified': 'Test_OCS_Modified_Plan' }, autonomy={ 'waypoints': Autonomy.High, 'wide_stance': Autonomy.High, 'modified': Autonomy.High }) # x:454 y:250 OperatableStateMachine.add( 'Manipulation Config', self.use_behavior(ManipulationConfigSM, 'Manipulation Config'), transitions={ 'finished': 'Locomotion Config', 'failed': 'Stop_Recording_Before_Failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:704 y:252 OperatableStateMachine.add( 'Locomotion Config', self.use_behavior(LocomotionConfigSM, 'Locomotion Config'), transitions={ 'finished': 'Stop_Recording_If_Finished', 'failed': 'Stop_Recording_Before_Failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:572 y:148 OperatableStateMachine.add( 'Test_OCS_Modified_Plan', _sm_test_ocs_modified_plan_1, transitions={ 'finished': 'Stop_Recording_If_Finished', 'failed': 'Stop_Recording_Before_Failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'target_pose_index': 'target_pose_index'}) return _state_machine
def create(self): # x:808 y:575, x:78 y:552, x:477 y:566 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = [ "Ask", "How are you today?", "Answer" ] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:675 y:219, x:771 y:50 _sm_confirm_0 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['answer']) with _sm_confirm_0: # x:70 y:60 OperatableStateMachine.add( 'Heard', SaraSay( sentence=lambda x: "I heard " + x + ". Is that correct?", input_keys=[], emotion=0, block=True), transitions={'done': 'get speech'}, autonomy={'done': Autonomy.Off}) # x:501 y:98 OperatableStateMachine.add( 'check yes', CheckConditionState( predicate=lambda x: "yes" in x and not "no" in x), transitions={ 'true': 'finished', 'false': 'failed' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'words'}) # x:259 y:92 OperatableStateMachine.add('get speech', GetSpeech(watchdog=5), transitions={ 'done': 'check yes', 'nothing': 'failed', 'fail': 'finished' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'words'}) with _state_machine: # x:43 y:162 OperatableStateMachine.add( 'SetPerson', SetKey(Value="person"), transitions={'done': 'Action_findPerson'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'personKey'}) # x:194 y:99 OperatableStateMachine.add( 'trouveLaQuestion', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'AskTheQuestion'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Action', 'output_value': 'question' }) # x:545 y:161 OperatableStateMachine.add('GetTheResponse', GetSpeech(watchdog=7), transitions={ 'done': 'Confirm', 'nothing': 'looping', 'fail': 'looping' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'response'}) # x:372 y:262 OperatableStateMachine.add( 'NotUnderstand', SaraSay(sentence="Soory, I did not understand.", input_keys=[], emotion=1, block=True), transitions={'done': 'AskTheQuestion'}, autonomy={'done': Autonomy.Off}) # x:23 y:285 OperatableStateMachine.add('Action_findPerson', self.use_behavior( Action_findPersonSM, 'Action_findPerson'), transitions={ 'done': 'fisrtSentence', 'pas_done': 'NoPerson' }, autonomy={ 'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit }, remapping={ 'className': 'personKey', 'entity': 'entity' }) # x:224 y:301 OperatableStateMachine.add( 'NoPerson', SaraSay(sentence="I did not find any person. ", input_keys=[], emotion=1, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}) # x:169 y:194 OperatableStateMachine.add( 'fisrtSentence', SaraSay(sentence="Hello, I have a question for you.", input_keys=[], emotion=1, block=True), transitions={'done': 'trouveLaQuestion'}, autonomy={'done': Autonomy.Off}) # x:539 y:332 OperatableStateMachine.add('looping', ForLoop(repeat=2), transitions={ 'do': 'NotUnderstand', 'end': 'saraSorry' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:540 y:473 OperatableStateMachine.add( 'saraSorry', SaraSay(sentence="Sorry, I can't understand your response.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause2'}, autonomy={'done': Autonomy.Off}) # x:776 y:209 OperatableStateMachine.add( 'getRosparmName', CalculationState(calculation=lambda x: x[2]), transitions={'done': 'SetRosParamKey'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Action', 'output_value': 'RosParamName' }) # x:778 y:301 OperatableStateMachine.add('SetRosParamKey', SetRosParamKey(), transitions={'done': 'log'}, autonomy={'done': Autonomy.Off}, remapping={ 'Value': 'response', 'ParamName': 'RosParamName' }) # x:787 y:482 OperatableStateMachine.add( 'thank you', SaraSay(sentence="Thank you for your answer.", input_keys=[], emotion=1, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:789 y:390 OperatableStateMachine.add('log', LogKeyState( text="{}", severity=Logger.REPORT_HINT), transitions={'done': 'thank you'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'response'}) # x:221 y:384 OperatableStateMachine.add( 'cause1', SetKey(Value="I didn't find any person"), transitions={'done': 'set cause failure'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:326 y:503 OperatableStateMachine.add( 'cause2', SetKey(Value="I did not understand the answer"), transitions={'done': 'set cause failure'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:159 y:468 OperatableStateMachine.add( 'set cause failure', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:769 y:88 OperatableStateMachine.add('Confirm', _sm_confirm_0, transitions={ 'finished': 'getRosparmName', 'failed': 'AskTheQuestion' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'answer': 'response'}) # x:367 y:117 OperatableStateMachine.add('AskTheQuestion', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'GetTheResponse'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:1023 y:473, x:791 y:291, x:797 y:103 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Guide", 'table', 'kitchen'] _state_machine.userdata.relative = False # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:458 _sm_groupwait_0 = OperatableStateMachine(outcomes=['end']) with _sm_groupwait_0: # x:30 y:40 OperatableStateMachine.add('waitwait', WaitState(wait_time=20), transitions={'done': 'end'}, autonomy={'done': Autonomy.Off}) # x:534 y:319 _sm_move_head_and_base_at_the_end_1 = OperatableStateMachine( outcomes=['failed']) with _sm_move_head_and_base_at_the_end_1: # x:52 y:31 OperatableStateMachine.add('setkeyorientation', SetKey(Value=1.5), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:51 y:114 OperatableStateMachine.add( 'action_turn', self.use_behavior( action_turnSM, 'operator is still there/Move head and base end /move head and base at the end/action_turn' ), transitions={ 'finished': 'turn right head', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:48 y:272 OperatableStateMachine.add('wait while head turn', WaitState(wait_time=4), transitions={'done': 'action_turn_2'}, autonomy={'done': Autonomy.Off}) # x:47 y:199 OperatableStateMachine.add( 'turn right head', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'wait while head turn'}, autonomy={'done': Autonomy.Off}) # x:45 y:348 OperatableStateMachine.add( 'action_turn_2', self.use_behavior( action_turnSM, 'operator is still there/Move head and base end /move head and base at the end/action_turn_2' ), transitions={ 'finished': 'left to rigth', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:43 y:592 OperatableStateMachine.add('left to rigth', SaraSetHeadAngle(pitch=0, yaw=-1.57), transitions={'done': 'waitwait1'}, autonomy={'done': Autonomy.Off}) # x:420 y:602 OperatableStateMachine.add('right to left', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'waitwait2'}, autonomy={'done': Autonomy.Off}) # x:266 y:541 OperatableStateMachine.add('waitwait1', WaitState(wait_time=8), transitions={'done': 'right to left'}, autonomy={'done': Autonomy.Off}) # x:236 y:674 OperatableStateMachine.add('waitwait2', WaitState(wait_time=8), transitions={'done': 'left to rigth'}, autonomy={'done': Autonomy.Off}) # x:231 y:538 _sm_find_a_human_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID']) with _sm_find_a_human_2: # x:78 y:275 OperatableStateMachine.add('find the human', GetEntityByID(), transitions={ 'found': 'finished', 'not_found': 'find the human' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Entity' }) # x:415 y:99, x:318 y:246, x:442 y:295 _sm_move_head_and_base_end__3 = ConcurrencyContainer( outcomes=['failed'], conditions=[('failed', [('move head and base at the end', 'failed') ]), ('failed', [('Groupwait', 'end')])]) with _sm_move_head_and_base_end__3: # x:132 y:57 OperatableStateMachine.add('move head and base at the end', _sm_move_head_and_base_at_the_end_1, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:121 y:218 OperatableStateMachine.add('Groupwait', _sm_groupwait_0, transitions={'end': 'failed'}, autonomy={'end': Autonomy.Inherit}) # x:67 y:227 _sm_export_no_waypoint_4 = OperatableStateMachine( outcomes=['done'], output_keys=['waipoint', 'area_name']) with _sm_export_no_waypoint_4: # x:30 y:40 OperatableStateMachine.add('noWaypoint', SetKey(Value=None), transitions={'done': 'NoName'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'waipoint'}) # x:43 y:129 OperatableStateMachine.add('NoName', SetKey(Value=None), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'area_name'}) # x:30 y:458 _sm_export_waypoint_5 = OperatableStateMachine( outcomes=['done'], input_keys=['entity'], output_keys=['waipoint', 'area_name']) with _sm_export_waypoint_5: # x:58 y:107 OperatableStateMachine.add( 'Extract Wayppoint', CalculationState(calculation=lambda x: x.waypoint), transitions={'done': 'Extract Name'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'waipoint' }) # x:67 y:257 OperatableStateMachine.add( 'Extract Name', CalculationState(calculation=lambda x: x.name), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'area_name' }) # x:379 y:221 _sm_wait_to_compte_6 = OperatableStateMachine(outcomes=['finished']) with _sm_wait_to_compte_6: # x:77 y:195 OperatableStateMachine.add('one more wait', WaitState(wait_time=60), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:535 y:314 _sm_turn_around_7 = OperatableStateMachine(outcomes=['failed']) with _sm_turn_around_7: # x:47 y:45 OperatableStateMachine.add('set orientation', SetKey(Value=1.57), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:42 y:213 OperatableStateMachine.add('move head', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'waitwait'}, autonomy={'done': Autonomy.Off}) # x:53 y:294 OperatableStateMachine.add('waitwait', WaitState(wait_time=10), transitions={'done': 'action_turn_2'}, autonomy={'done': Autonomy.Off}) # x:33 y:122 OperatableStateMachine.add( 'action_turn', self.use_behavior( action_turnSM, 'Try to reach/check person behind/move head and base/turn around/action_turn' ), transitions={ 'finished': 'move head', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:31 y:434 OperatableStateMachine.add( 'action_turn_2', self.use_behavior( action_turnSM, 'Try to reach/check person behind/move head and base/turn around/action_turn_2' ), transitions={ 'finished': 'head left right', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:35 y:595 OperatableStateMachine.add('head left right', SaraSetHeadAngle(pitch=0, yaw=1.57), transitions={'done': 'wait turn head'}, autonomy={'done': Autonomy.Off}) # x:308 y:537 OperatableStateMachine.add('wait turn head', WaitState(wait_time=10), transitions={'done': 'head right left'}, autonomy={'done': Autonomy.Off}) # x:495 y:604 OperatableStateMachine.add('head right left', SaraSetHeadAngle(pitch=0, yaw=-1.57), transitions={'done': 'wait wait wait'}, autonomy={'done': Autonomy.Off}) # x:271 y:711 OperatableStateMachine.add('wait wait wait', WaitState(wait_time=10), transitions={'done': 'head left right'}, autonomy={'done': Autonomy.Off}) # x:845 y:395 _sm_find_human_8 = OperatableStateMachine(outcomes=['finished'], input_keys=['ID']) with _sm_find_human_8: # x:223 y:137 OperatableStateMachine.add('find person', GetEntityByID(), transitions={ 'found': 'finished', 'not_found': 'find person' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Entity' }) # x:549 y:135, x:555 y:269, x:230 y:458 _sm_move_head_and_base_9 = ConcurrencyContainer( outcomes=['failed'], conditions=[('failed', [('turn around', 'failed')]), ('failed', [('wait to compte', 'finished')])]) with _sm_move_head_and_base_9: # x:268 y:77 OperatableStateMachine.add('turn around', _sm_turn_around_7, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:263 y:246 OperatableStateMachine.add('wait to compte', _sm_wait_to_compte_6, transitions={'finished': 'failed'}, autonomy={'finished': Autonomy.Inherit}) # x:493 y:206 _sm_container_10 = OperatableStateMachine(outcomes=['check']) with _sm_container_10: # x:230 y:160 OperatableStateMachine.add('wait long', WaitState(wait_time=40), transitions={'done': 'check'}, autonomy={'done': Autonomy.Off}) # x:30 y:458, x:706 y:447 _sm_navigate_to_the_point_11 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['pose']) with _sm_navigate_to_the_point_11: # x:174 y:122 OperatableStateMachine.add('set relative', SetKey(Value=False), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'relative'}) # x:347 y:191 OperatableStateMachine.add( 'Action_Move', self.use_behavior( Action_MoveSM, 'Try to reach/Container/navigate to the point/Action_Move' ), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'pose'}) # x:728 y:335, x:792 y:103, x:738 y:249, x:724 y:448 _sm_check_person_behind_12 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['ID'], conditions=[('failed', [('move head and base', 'failed')]), ('finished', [('find human', 'finished')])]) with _sm_check_person_behind_12: # x:250 y:72 OperatableStateMachine.add('move head and base', _sm_move_head_and_base_9, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:261 y:238 OperatableStateMachine.add('find human', _sm_find_human_8, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:635 y:61, x:634 y:159, x:597 y:300, x:330 y:458, x:430 y:458, x:530 y:458 _sm_container_13 = ConcurrencyContainer( outcomes=['finished', 'failed', 'check'], input_keys=['waypoint'], conditions=[('check', [('Container', 'check')]), ('finished', [('navigate to the point', 'finished')]), ('failed', [('navigate to the point', 'failed')])]) with _sm_container_13: # x:315 y:51 OperatableStateMachine.add('navigate to the point', _sm_navigate_to_the_point_11, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'waypoint'}) # x:322 y:257 OperatableStateMachine.add('Container', _sm_container_10, transitions={'check': 'check'}, autonomy={'check': Autonomy.Inherit}) # x:98 y:451 _sm_get_area_containers_14 = OperatableStateMachine( outcomes=['done'], input_keys=['command'], output_keys=['containers']) with _sm_get_area_containers_14: # x:46 y:31 OperatableStateMachine.add( 'Set Empty Array', SetKey(Value=[]), transitions={'done': 'Set initial index'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'containers'}) # x:50 y:110 OperatableStateMachine.add('Set initial index', SetKey(Value=2), transitions={'done': 'Set Loop Max'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'iLoop'}) # x:49 y:192 OperatableStateMachine.add( 'Set Loop Max', CalculationState(calculation=lambda x: len(x)), transitions={'done': 'Check Loop End'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'command', 'output_value': 'iLoopMax' }) # x:514 y:278 OperatableStateMachine.add( 'Increment', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'Print container array'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'iLoop', 'output_value': 'iLoop' }) # x:271 y:154 OperatableStateMachine.add( 'Add Container to Array', FlexibleCalculationState( calculation=lambda x: x[2] + [x[0][x[1]]], input_keys=['command', 'iLoop', 'containers']), transitions={'done': 'Increment'}, autonomy={'done': Autonomy.Off}, remapping={ 'command': 'command', 'iLoop': 'iLoop', 'containers': 'containers', 'output_value': 'containers' }) # x:42 y:287 OperatableStateMachine.add('Check Loop End', FlexibleCheckConditionState( predicate=lambda x: x[0] < x[1], input_keys=['iLoop', 'iLoopMax']), transitions={ 'true': 'Add Container to Array', 'false': 'done' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'iLoop': 'iLoop', 'iLoopMax': 'iLoopMax' }) # x:279 y:401 OperatableStateMachine.add('Print container array', LogKeyState( text="Containers: \n {}", severity=Logger.REPORT_HINT), transitions={'done': 'Check Loop End'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'containers'}) # x:626 y:228, x:607 y:71, x:230 y:458, x:330 y:458 _sm_operator_is_still_there_15 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['ID'], conditions=[('finished', [('find a human', 'finished')]), ('failed', [('Move head and base end ', 'failed')])]) with _sm_operator_is_still_there_15: # x:207 y:54 OperatableStateMachine.add('Move head and base end ', _sm_move_head_and_base_end__3, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) # x:222 y:190 OperatableStateMachine.add('find a human', _sm_find_a_human_2, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:1620 y:109, x:1648 y:375 _sm_try_to_find_area_16 = OperatableStateMachine( outcomes=['found', 'not_found'], input_keys=['area_to_search', 'containers'], output_keys=['area_name', 'waypoint']) with _sm_try_to_find_area_16: # x:517 y:67 OperatableStateMachine.add( 'WonderlandUniqueEnity', self.use_behavior(WonderlandUniqueEnitySM, 'Try to find area/WonderlandUniqueEnity'), transitions={ 'found': 'Export Waypoint', 'not_found': 'Export No Waypoint' }, autonomy={ 'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit }, remapping={ 'name': 'area_to_search', 'containers': 'containers', 'entity': 'entity' }) # x:966 y:71 OperatableStateMachine.add('Export Waypoint', _sm_export_waypoint_5, transitions={'done': 'say_going'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'entity': 'entity', 'waipoint': 'waypoint', 'area_name': 'area_name' }) # x:1247 y:295 OperatableStateMachine.add('Export No Waypoint', _sm_export_no_waypoint_4, transitions={'done': 'not_found'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'waipoint': 'waypoint', 'area_name': 'area_name' }) # x:1273 y:106 OperatableStateMachine.add( 'say_going', SaraSay(sentence=lambda x: "I'm going to the " + x, input_keys=[], emotion=0, block=True), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}) # x:323 y:632, x:638 y:631 _sm_try_to_reach_17 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['waypoint', 'relative', 'areaName', 'ID']) with _sm_try_to_reach_17: # x:252 y:161 OperatableStateMachine.add('Container', _sm_container_13, transitions={ 'finished': 'say_reached', 'failed': 'Say_not_reached', 'check': 'say check' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'check': Autonomy.Inherit }, remapping={'waypoint': 'waypoint'}) # x:581 y:163 OperatableStateMachine.add('check person behind', _sm_check_person_behind_12, transitions={ 'finished': 'say found', 'failed': 'say lost' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'ID': 'ID'}) # x:706 y:484 OperatableStateMachine.add( 'say lost', SaraSay(sentence="Oh no! I lost my operator!", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:445 y:69 OperatableStateMachine.add( 'say check', SaraSay(sentence="I check if my operator is still there", input_keys=[], emotion=1, block=True), transitions={'done': 'check person behind'}, autonomy={'done': Autonomy.Off}) # x:443 y:187 OperatableStateMachine.add( 'say found', SaraSay(sentence="Great. You are still there.", input_keys=[], emotion=1, block=True), transitions={'done': 'Container'}, autonomy={'done': Autonomy.Off}) # x:463 y:398 OperatableStateMachine.add( 'Say_not_reached', SaraSay(sentence=lambda x: "I have not reach the " + x + "!", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:61 y:366 OperatableStateMachine.add( 'say_reached', SaraSay(sentence=lambda x: "I have reach the " + x + "!", input_keys=[], emotion=1, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:871 y:59 _sm_decompose_command_18 = OperatableStateMachine( outcomes=['done'], input_keys=['command'], output_keys=['containers', 'area']) with _sm_decompose_command_18: # x:163 y:34 OperatableStateMachine.add('Set State Command', Set_a_step(step=0), transitions={'done': 'Get area name'}, autonomy={'done': Autonomy.Off}) # x:387 y:58 OperatableStateMachine.add( 'Get area name', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Get area containers'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'command', 'output_value': 'area' }) # x:597 y:94 OperatableStateMachine.add('Get area containers', _sm_get_area_containers_14, transitions={'done': 'done'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'command': 'command', 'containers': 'containers' }) with _state_machine: # x:54 y:24 OperatableStateMachine.add( 'Get Person Id', GetRosParam(ParamName="behavior/FoundPerson/Id"), transitions={ 'done': 'GetPerson', 'failed': 'Cant Find Person' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'ID'}) # x:19 y:152 OperatableStateMachine.add( 'Decompose Command', _sm_decompose_command_18, transitions={'done': 'Try to find area'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'command': 'Action', 'containers': 'containers', 'area': 'area' }) # x:43 y:358 OperatableStateMachine.add('Try to reach', _sm_try_to_reach_17, transitions={ 'finished': 'operator is still there', 'failed': 'cause2' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'waypoint': 'waypoint', 'relative': 'relative', 'areaName': 'area_name', 'ID': 'ID' }) # x:360 y:109 OperatableStateMachine.add('Cant Find Person', SaraSay( sentence="I can't find a person.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}) # x:52 y:82 OperatableStateMachine.add('GetPerson', GetEntityByID(), transitions={ 'found': 'Decompose Command', 'not_found': 'Cant Find Person' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Entity' }) # x:36 y:219 OperatableStateMachine.add('Try to find area', _sm_try_to_find_area_16, transitions={ 'found': 'sayfollowme', 'not_found': 'Cant Find Person' }, autonomy={ 'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit }, remapping={ 'area_to_search': 'area', 'containers': 'containers', 'area_name': 'area_name', 'waypoint': 'waypoint' }) # x:27 y:438 OperatableStateMachine.add('operator is still there', _sm_operator_is_still_there_15, transitions={ 'finished': 'getentitybyID', 'failed': 'say lost operator' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'ID': 'ID'}) # x:347 y:370 OperatableStateMachine.add( 'say lost operator', SaraSay( sentence= "I have reach my goal but I lost the person I was guiding.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause3'}, autonomy={'done': Autonomy.Off}) # x:49 y:291 OperatableStateMachine.add('sayfollowme', SaraSay(sentence="Follow me please.", input_keys=[], emotion=1, block=True), transitions={'done': 'Try to reach'}, autonomy={'done': Autonomy.Off}) # x:808 y:451 OperatableStateMachine.add('head to middle', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:48 y:525 OperatableStateMachine.add('getentitybyID', GetEntityByID(), transitions={ 'found': 'get entity to point', 'not_found': 'say_reach_the_entity' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Entity' }) # x:249 y:562 OperatableStateMachine.add('get entity to point', WonderlandGetEntityVerbal(), transitions={ 'one': 'find the point', 'multiple': 'say_reach_the_entity', 'none': 'say_reach_the_entity', 'error': 'say_reach_the_entity' }, autonomy={ 'one': Autonomy.Off, 'multiple': Autonomy.Off, 'none': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'name': 'area_name', 'containers': 'containers', 'entities': 'entities', 'firstEntity': 'firstEntity' }) # x:708 y:529 OperatableStateMachine.add( 'Action_point_at', self.use_behavior(Action_point_atSM, 'Action_point_at'), transitions={ 'finished': 'say_reach_the_entity', 'failed': 'say_reach_the_entity' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'targetPoint': 'targetPoint'}) # x:539 y:558 OperatableStateMachine.add( 'find the point', CalculationState(calculation=lambda x: x.position), transitions={'done': 'Action_point_at'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entities', 'output_value': 'targetPoint' }) # x:547 y:121 OperatableStateMachine.add( 'cause1', SetKey(Value="I didn't find any persone"), transitions={'done': 'setrosparamfail'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:507 y:220 OperatableStateMachine.add( 'cause2', SetKey(Value="I did not reach the area"), transitions={'done': 'setrosparamfail'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:559 y:337 OperatableStateMachine.add( 'cause3', SetKey(Value="I lost the person I was guiding."), transitions={'done': 'setrosparamfail'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:656 y:250 OperatableStateMachine.add( 'setrosparamfail', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:523 y:462 OperatableStateMachine.add( 'say_reach_the_entity', SaraSay(sentence=lambda x: "Here is the " + x, input_keys=[], emotion=1, block=True), transitions={'done': 'head to middle'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:392 y:613, x:739 y:627, x:59 y:602, x:34 y:328 _state_machine = OperatableStateMachine( outcomes=['timeout', 'failed', 'invalid_pose', 'unknown_keys'], input_keys=['key_pressed_msg'], output_keys=['key_pressed_msg']) _state_machine.userdata.key_pressed_msg = KeyPressed() # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:353, x:130 y:353, x:626 y:359, x:330 y:353, x:430 y:353, x:530 y:353 _sm_waitkey_0 = ConcurrencyContainer( outcomes=['received', 'timeout', 'failed'], output_keys=['key_pressed_msg'], conditions=[('received', [('WaitKeyPressed', 'received')]), ('timeout', [('WaitTimeout', 'done')]), ('failed', [('WaitKeyPressed', 'unavailable')])]) with _sm_waitkey_0: # x:281 y:146 OperatableStateMachine.add( 'WaitKeyPressed', WaitForMessageState(topic='/hmi/joy_decoder/keys_pressed', condition=self.trigger, buffered=False, clear=True), transitions={ 'received': 'received', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'key_pressed_msg'}) # x:509 y:151 OperatableStateMachine.add('WaitTimeout', WaitState(wait_time=self.timeout), transitions={'done': 'timeout'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:51 y:31 OperatableStateMachine.add( 'CheckExitEnter', DecisionState( outcomes=['animation', 'exit', 'walk', 'unknown'], conditions=self.decision), transitions={ 'animation': 'unknown_keys', 'exit': 'unknown_keys', 'walk': 'PrepareForWalk', 'unknown': 'unknown_keys' }, autonomy={ 'animation': Autonomy.Off, 'exit': Autonomy.Off, 'walk': Autonomy.Off, 'unknown': Autonomy.Off }, remapping={'input_value': 'key_pressed_msg'}) # x:326 y:259 OperatableStateMachine.add( 'CheckExit', DecisionState( outcomes=['animation', 'walk', 'exit', 'unknown'], conditions=self.decision), transitions={ 'animation': 'EndWalkUnknownKeys', 'walk': 'ProcessKeyMsg', 'exit': 'EndWalkUnknownKeys', 'unknown': 'WaitKey' }, autonomy={ 'animation': Autonomy.Off, 'walk': Autonomy.Off, 'exit': Autonomy.Off, 'unknown': Autonomy.Off }, remapping={'input_value': 'key_pressed_msg'}) # x:703 y:410 OperatableStateMachine.add( 'ExecuteStepSeq', ExecuteStepSequenceKey( controller='motion/controller/step_sequence', trajectory_ns='saved_msgs/step_sequence'), transitions={ 'success': 'WaitKey', 'unavailable': 'WaitKey', 'partial_movement': 'invalid_pose', 'invalid_pose': 'invalid_pose', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'unavailable': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'trajectory_param': 'motion_param'}) # x:431 y:381 OperatableStateMachine.add( 'WaitKey', _sm_waitkey_0, transitions={ 'received': 'CheckExit', 'timeout': 'EndWalkTimeout', 'failed': 'failed' }, autonomy={ 'received': Autonomy.Inherit, 'timeout': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'key_pressed_msg': 'key_pressed_msg'}) # x:719 y:246 OperatableStateMachine.add( 'CheckNone', CheckConditionState(predicate=lambda x: x == None), transitions={ 'true': 'WaitKey', 'false': 'ExecuteStepSeq' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'motion_param'}) # x:108 y:289 OperatableStateMachine.add( 'EndWalkUnknownKeys', ExecuteJointTrajectory( action_topic='motion/controller/joint_trajectory', trajectory_param='crouch_end', trajectory_ns='saved_msgs/joint_trajectory'), transitions={ 'success': 'unknown_keys', 'partial_movement': 'invalid_pose', 'invalid_pose': 'invalid_pose', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }) # x:379 y:491 OperatableStateMachine.add( 'EndWalkTimeout', ExecuteJointTrajectory( action_topic='motion/controller/joint_trajectory', trajectory_param='crouch_end', trajectory_ns='saved_msgs/joint_trajectory'), transitions={ 'success': 'timeout', 'partial_movement': 'invalid_pose', 'invalid_pose': 'invalid_pose', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }) # x:408 y:158 OperatableStateMachine.add( 'ProcessKeyMsg', CalculationState(calculation=self.process_walk), transitions={'done': 'CheckNone'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'key_pressed_msg', 'output_value': 'motion_param' }) # x:211 y:72 OperatableStateMachine.add( 'PrepareForWalk', ExecuteJointTrajectory( action_topic='motion/controller/joint_trajectory', trajectory_param='crouch_begin', trajectory_ns='saved_msgs/joint_trajectory'), transitions={ 'success': 'ProcessKeyMsg', 'partial_movement': 'invalid_pose', 'invalid_pose': 'invalid_pose', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }) return _state_machine
def create(self): joint_trajectory_action = 'motion/controller/joint_trajectory' storage = 'joint_trajectory/' voice_topic = 'voice/voice' eye_cmd_topic = 'control' # x:1034 y:292, x:622 y:639 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['be_evil', 'new_be_evil'], output_keys=['be_evil']) _state_machine.userdata.be_evil = self.be_evil _state_machine.userdata.new_be_evil = self.new_be_evil _state_machine.userdata.head_crooked_pose = [0, 0, 0, 0.4] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:96 y:163 OperatableStateMachine.add('CheckEvil', DecisionState( outcomes=['good', 'evil'], conditions=lambda x: 'good' if not x else 'evil'), transitions={ 'good': 'CheckChangeToEvil', 'evil': 'CheckChangeToGood' }, autonomy={ 'good': Autonomy.Off, 'evil': Autonomy.Off }, remapping={'input_value': 'be_evil'}) # x:252 y:50 OperatableStateMachine.add( 'CheckChangeToEvil', DecisionState(outcomes=['yes', 'no'], conditions=lambda x: 'yes' if x else 'no'), transitions={ 'yes': 'RedEyes', 'no': 'NormalEyes' }, autonomy={ 'yes': Autonomy.Off, 'no': Autonomy.Off }, remapping={'input_value': 'new_be_evil'}) # x:426 y:48 OperatableStateMachine.add('RedEyes', TextCommandState(type='eyes/emotion', command='red_eyes', topic=eye_cmd_topic), transitions={'done': 'Seizure'}, autonomy={'done': Autonomy.Off}) # x:737 y:30 OperatableStateMachine.add( 'Seizure', ExecuteJointTrajectory(action_topic=joint_trajectory_action, trajectory_param='seizure_evil', trajectory_ns=storage), transitions={ 'success': 'SetEvil', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) # x:961 y:162 OperatableStateMachine.add( 'SetEvil', CalculationState(calculation=lambda x: True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'be_evil', 'output_value': 'be_evil' }) # x:419 y:135 OperatableStateMachine.add('NormalEyes', TextCommandState(type='eyes/emotion', command='normal', topic=eye_cmd_topic), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:195 y:281 OperatableStateMachine.add( 'CheckChangeToGood', DecisionState(outcomes=['yes', 'no'], conditions=lambda x: 'no' if x else 'yes'), transitions={ 'yes': 'NormalEyes2', 'no': 'RedEyes2' }, autonomy={ 'yes': Autonomy.Off, 'no': Autonomy.Off }, remapping={'input_value': 'new_be_evil'}) # x:416 y:331 OperatableStateMachine.add( 'NormalEyes2', TextCommandState(type='eyes/emotion', command='normal', topic=eye_cmd_topic), transitions={'done': 'HeadAssumeBasicPose'}, autonomy={'done': Autonomy.Off}) # x:613 y:321 OperatableStateMachine.add( 'ShakeHead', ExecuteJointTrajectory(action_topic=joint_trajectory_action, trajectory_param='head_shake', trajectory_ns=storage), transitions={ 'success': 'LookAround', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) # x:894 y:326 OperatableStateMachine.add( 'SetGood', CalculationState(calculation=lambda x: False), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'be_evil', 'output_value': 'be_evil' }) # x:671 y:437 OperatableStateMachine.add( 'LookAround', ExecuteJointTrajectory(action_topic=joint_trajectory_action, trajectory_param='look_around', trajectory_ns=storage), transitions={ 'success': 'SetGood', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'result': 'result'}) # x:420 y:200 OperatableStateMachine.add('RedEyes2', TextCommandState(type='eyes/emotion', command='red_eyes', topic=eye_cmd_topic), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:444 y:420 OperatableStateMachine.add('HeadAssumeBasicPose', SrdfStateToMoveit( config_name='head_basic', move_group='head', action_topic='move_group', robot_name=''), transitions={ 'reached': 'ShakeHead', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed' }, autonomy={ 'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off }, remapping={ 'config_name': 'config_name', 'move_group': 'move_group', 'robot_name': 'robot_name', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names' }) return _state_machine
def create(self): # x:712 y:612, x:820 y:97 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.name = "person" _state_machine.userdata.distance = 1.05 _state_machine.userdata.taxi = "taxi" _state_machine.userdata.umbrella = "umbrella" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365, x:130 y:365, x:230 y:365 _sm_get_closer_0 = ConcurrencyContainer( outcomes=['finished'], input_keys=['ID'], conditions=[('finished', [('follow', 'failed')]), ('finished', [('wait 3', 'done')])]) with _sm_get_closer_0: # x:46 y:131 OperatableStateMachine.add('follow', SaraFollow(distance=1.15, ReplanPeriod=0.5), transitions={'failed': 'finished'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:223 y:134 OperatableStateMachine.add('wait 3', WaitState(wait_time=4), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:571 y:592 _sm_scan_1 = OperatableStateMachine(outcomes=['finished']) with _sm_scan_1: # x:42 y:66 OperatableStateMachine.add( 'Looking', SaraSay( sentence= "I am trying to find who wants to leave. Please raise your hand.", input_keys=[], emotion=0, block=False), transitions={'done': 'center'}, autonomy={'done': Autonomy.Off}) # x:394 y:262 OperatableStateMachine.add('right', SaraSetHeadAngle(pitch=0.1, yaw=-0.3), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:206 y:372 OperatableStateMachine.add('center2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w4'}, autonomy={'done': Autonomy.Off}) # x:34 y:589 OperatableStateMachine.add('w1', WaitState(wait_time=3), transitions={'done': 'center3'}, autonomy={'done': Autonomy.Off}) # x:413 y:159 OperatableStateMachine.add('w2', WaitState(wait_time=3), transitions={'done': 'right'}, autonomy={'done': Autonomy.Off}) # x:415 y:370 OperatableStateMachine.add('w3', WaitState(wait_time=3), transitions={'done': 'center2'}, autonomy={'done': Autonomy.Off}) # x:39 y:371 OperatableStateMachine.add('w4', WaitState(wait_time=3), transitions={'done': 'left'}, autonomy={'done': Autonomy.Off}) # x:16 y:475 OperatableStateMachine.add('left', SaraSetHeadAngle(pitch=0.1, yaw=0.3), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:215 y:156 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:213 y:579 OperatableStateMachine.add('center3', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'wait'}, autonomy={'done': Autonomy.Off}) # x:393 y:576 OperatableStateMachine.add('wait', WaitState(wait_time=4), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:32 y:494, x:627 y:411 _sm_filtregender_2 = OperatableStateMachine( outcomes=['none_found', 'found person'], input_keys=['name'], output_keys=['pronoun', 'person']) with _sm_filtregender_2: # x:109 y:51 OperatableStateMachine.add('List', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'FiltreWave', 'none_found': 'none_found' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'name', 'entity_list': 'entity_list', 'number': 'number' }) # x:180 y:301 OperatableStateMachine.add( 'FiltreExitingwomen', Filter(filter=lambda x: x.face.gender == "female"), transitions={'done': 'no female?'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_list': 'persons', 'output_list': 'female' }) # x:174 y:386 OperatableStateMachine.add( 'no female?', CheckConditionState(predicate=lambda x: len(x) > 0), transitions={ 'true': 'set pronoun female', 'false': 'set pronoun male' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'female'}) # x:374 y:312 OperatableStateMachine.add( 'set pronoun female', SetKey(Value="miss"), transitions={'done': 'get first female'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'pronoun'}) # x:181 y:476 OperatableStateMachine.add('set pronoun male', SetKey(Value=""), transitions={'done': 'get first male'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'pronoun'}) # x:599 y:290 OperatableStateMachine.add( 'get first female', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'found person'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'female', 'output_value': 'person' }) # x:381 y:408 OperatableStateMachine.add( 'get first male', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'found person'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'persons', 'output_value': 'person' }) # x:151 y:135 OperatableStateMachine.add( 'FiltreWave', Filter(filter=lambda x: x.pose.right_arm_up or x.pose. left_arm_up), transitions={'done': 'if more than 0'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_list': 'entity_list', 'output_list': 'persons' }) # x:157 y:217 OperatableStateMachine.add( 'if more than 0', CheckConditionState(predicate=lambda x: len(x) > 0), transitions={ 'true': 'FiltreExitingwomen', 'false': 'none_found' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'persons'}) # x:772 y:208, x:360 y:516 _sm_confirm_3 = OperatableStateMachine( outcomes=['false', 'done'], input_keys=['Person', 'pronoun']) with _sm_confirm_3: # x:66 y:83 OperatableStateMachine.add( 'Confirm', SaraSay(sentence=lambda x: "would you like to leave, " + x[0] + "?", input_keys=["pronoun"], emotion=0, block=True), transitions={'done': 'GetSpeech'}, autonomy={'done': Autonomy.Off}, remapping={'pronoun': 'pronoun'}) # x:82 y:504 OperatableStateMachine.add('GetID', SetRosParam(ParamName="OpeID"), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'ID'}) # x:67 y:298 OperatableStateMachine.add('if yes', RegexTester(regex=".*((yes)|(I do)).*"), transitions={ 'true': 'get id', 'false': 'test no' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'text': 'text', 'result': 'result' }) # x:84 y:409 OperatableStateMachine.add('get id', GetAttribute(attributes=["ID"]), transitions={'done': 'GetID'}, autonomy={'done': Autonomy.Off}, remapping={ 'object': 'Person', 'ID': 'ID' }) # x:87 y:188 OperatableStateMachine.add('GetSpeech', GetSpeech(watchdog=10), transitions={ 'done': 'if yes', 'nothing': 'sayno', 'fail': 'if yes' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'text'}) # x:259 y:193 OperatableStateMachine.add( 'sayno', SaraSay( sentence="Sorry, did you say that you wanted to leave?", input_keys=[], emotion=0, block=True), transitions={'done': 'getno'}, autonomy={'done': Autonomy.Off}) # x:439 y:179 OperatableStateMachine.add('getno', GetSpeech(watchdog=10), transitions={ 'done': 'getNO', 'nothing': 'false', 'fail': 'false' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'text'}) # x:603 y:337 OperatableStateMachine.add('getNO', RegexTester(regex=".*((yes)|(I do)).*"), transitions={ 'true': 'get id', 'false': 'false' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'text': 'text', 'result': 'result' }) # x:246 y:298 OperatableStateMachine.add('test no', RegexTester(regex=".*((no)|(not)).*"), transitions={ 'true': 'false', 'false': 'sayno' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'text': 'text', 'result': 'result' }) # x:228 y:419 _sm_find_raising_arm_4 = OperatableStateMachine( outcomes=['finished'], output_keys=['umbrella']) with _sm_find_raising_arm_4: # x:33 y:48 OperatableStateMachine.add('setperson', SetKey(Value="person"), transitions={'done': 'gettaxiperson'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'person'}) # x:183 y:120 OperatableStateMachine.add('gettaxiperson', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'taxi', 'none_found': 'gettaxiperson' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'person', 'entity_list': 'entity_list', 'number': 'number' }) # x:408 y:270 OperatableStateMachine.add( 'if', CheckConditionState(predicate=lambda x: len(x) > 0), transitions={ 'true': 'get first', 'false': 'gettaxiperson' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'output_list'}) # x:344 y:184 OperatableStateMachine.add( 'taxi', Filter(filter=lambda x: x.pose.right_arm_up or x.pose. left_arm_up), transitions={'done': 'if'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_list': 'entity_list', 'output_list': 'output_list' }) # x:405 y:359 OperatableStateMachine.add( 'get first', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'output_list', 'output_value': 'umbrella' }) # x:30 y:365 _sm_rotation_5 = OperatableStateMachine(outcomes=['end']) with _sm_rotation_5: # x:51 y:38 OperatableStateMachine.add('Set 180 degres', SetKey(Value=3.1416), transitions={'done': 'Look Center'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:613 y:470 OperatableStateMachine.add( 'action_turn', self.use_behavior( sara_flexbe_behaviors__action_turnSM, 'GetTaxi/Find umbrella/gettaxihuman/Rotation/action_turn'), transitions={ 'finished': 'Look Right', 'failed': 'Look Right' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:421 y:54 OperatableStateMachine.add('Look Right', SaraSetHeadAngle(pitch=0, yaw=-1.5), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:265 y:56 OperatableStateMachine.add('w1', WaitState(wait_time=4), transitions={'done': 'Look Right'}, autonomy={'done': Autonomy.Off}) # x:630 y:56 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'center'}, autonomy={'done': Autonomy.Off}) # x:250 y:177 OperatableStateMachine.add('Look Center', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:618 y:304 OperatableStateMachine.add('Look Left 2', SaraSetHeadAngle(pitch=0, yaw=1.5), transitions={'done': 'w4'}, autonomy={'done': Autonomy.Off}) # x:612 y:138 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:635 y:214 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'Look Left 2'}, autonomy={'done': Autonomy.Off}) # x:636 y:394 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}) # x:30 y:365 _sm_find_entity_6 = OperatableStateMachine(outcomes=['found'], input_keys=['className'], output_keys=['entity']) with _sm_find_entity_6: # x:181 y:178 OperatableStateMachine.add('find_entity', list_entities_by_name( frontality_level=0.5, distance_max=4), transitions={ 'found': 'Get Entity', 'none_found': 'find_entity' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'className', 'entity_list': 'entity_list', 'number': 'number' }) # x:454 y:178 OperatableStateMachine.add( 'Get Entity', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'entity' }) # x:194 y:40 OperatableStateMachine.add('WaitState', WaitState(wait_time=1), transitions={'done': 'find_entity'}, autonomy={'done': Autonomy.Off}) # x:34 y:496, x:130 y:365, x:475 y:291, x:330 y:365, x:430 y:365 _sm_gettaxihuman_7 = ConcurrencyContainer( outcomes=['found', 'not_found'], input_keys=['umbrella'], output_keys=['umbrella'], conditions=[('not_found', [('Rotation', 'end')]), ('found', [('Find Entity', 'found')]), ('found', [('FInd raising arm', 'finished')])]) with _sm_gettaxihuman_7: # x:127 y:67 OperatableStateMachine.add('Find Entity', _sm_find_entity_6, transitions={'found': 'found'}, autonomy={'found': Autonomy.Inherit}, remapping={ 'className': 'umbrella', 'entity': 'umbrella' }) # x:129 y:180 OperatableStateMachine.add('Rotation', _sm_rotation_5, transitions={'end': 'not_found'}, autonomy={'end': Autonomy.Inherit}) # x:377 y:127 OperatableStateMachine.add('FInd raising arm', _sm_find_raising_arm_4, transitions={'finished': 'found'}, autonomy={'finished': Autonomy.Inherit}, remapping={'umbrella': 'umbrella'}) # x:73 y:441, x:586 y:51 _sm_find_umbrella_8 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['umbrella'], output_keys=['umbrella']) with _sm_find_umbrella_8: # x:67 y:42 OperatableStateMachine.add('Look Center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'gettaxihuman'}, autonomy={'done': Autonomy.Off}) # x:278 y:138 OperatableStateMachine.add('Look Center Not Found', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:58 y:326 OperatableStateMachine.add('Log Entity', LogKeyState( text="Found entity: {}", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'umbrella'}) # x:63 y:126 OperatableStateMachine.add('gettaxihuman', _sm_gettaxihuman_7, transitions={ 'found': 'WaitState', 'not_found': 'Look Center Not Found' }, autonomy={ 'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit }, remapping={'umbrella': 'umbrella'}) # x:67 y:222 OperatableStateMachine.add('WaitState', WaitState(wait_time=1), transitions={'done': 'Log Entity'}, autonomy={'done': Autonomy.Off}) # x:83 y:284 _sm_lift_head_9 = OperatableStateMachine(outcomes=['finished']) with _sm_lift_head_9: # x:53 y:42 OperatableStateMachine.add('lift head', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'wait 2'}, autonomy={'done': Autonomy.Off}) # x:57 y:152 OperatableStateMachine.add('wait 2', WaitState(wait_time=3), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:30 y:365 _sm_get_closer_10 = OperatableStateMachine(outcomes=['finished'], input_keys=['person']) with _sm_get_closer_10: # x:151 y:63 OperatableStateMachine.add('get ID', GetAttribute(attributes=["ID"]), transitions={'done': 'Get closer'}, autonomy={'done': Autonomy.Off}, remapping={ 'object': 'person', 'ID': 'ID' }) # x:135 y:194 OperatableStateMachine.add('Get closer', _sm_get_closer_0, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:607 y:98, x:558 y:330 _sm_get_gender_11 = OperatableStateMachine( outcomes=['none_found', 'done'], input_keys=['name', 'distance'], output_keys=['person', 'pronoun']) with _sm_get_gender_11: # x:30 y:40 OperatableStateMachine.add('set head', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'Scan'}, autonomy={'done': Autonomy.Off}) # x:441 y:109 OperatableStateMachine.add('FiltreGender', _sm_filtregender_2, transitions={ 'none_found': 'none_found', 'found person': 'done' }, autonomy={ 'none_found': Autonomy.Inherit, 'found person': Autonomy.Inherit }, remapping={ 'name': 'name', 'pronoun': 'pronoun', 'person': 'person' }) # x:211 y:93 OperatableStateMachine.add( 'Scan', _sm_scan_1, transitions={'finished': 'FiltreGender'}, autonomy={'finished': Autonomy.Inherit}) # x:30 y:373, x:130 y:373 _sm_nevermind_12 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['poseOrigin']) with _sm_nevermind_12: # x:30 y:40 OperatableStateMachine.add('say ok', SaraSay(sentence="I will try again", input_keys=[], emotion=0, block=False), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:26 y:192 OperatableStateMachine.add( 'Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'nevermind/Action_Move'), transitions={ 'finished': 'done', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'poseOrigin'}) # x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458 _sm_confirm_13 = ConcurrencyContainer( outcomes=['false', 'done'], input_keys=['person', 'pronoun'], conditions=[('false', [('Confirm', 'false')]), ('done', [('Confirm', 'done')]), ('false', [('LookAtClosest', 'failed')])]) with _sm_confirm_13: # x:95 y:163 OperatableStateMachine.add('Confirm', _sm_confirm_3, transitions={ 'false': 'false', 'done': 'done' }, autonomy={ 'false': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'Person': 'person', 'pronoun': 'pronoun' }) # x:309 y:162 OperatableStateMachine.add( 'LookAtClosest', self.use_behavior(sara_flexbe_behaviors__LookAtClosestSM, 'confirm/LookAtClosest'), transitions={'failed': 'false'}, autonomy={'failed': Autonomy.Inherit}) # x:65 y:581, x:688 y:449 _sm_gettaxi_14 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['distance', 'taxi', 'umbrella']) with _sm_gettaxi_14: # x:103 y:28 OperatableStateMachine.add( 'Action_Move_to taxi', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'GetTaxi/Action_Move_to taxi'), transitions={ 'finished': 'Lift head', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'taxi'}) # x:220 y:546 OperatableStateMachine.add( 'Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'GetTaxi/Action_Move'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'pose_out'}) # x:80 y:475 OperatableStateMachine.add('NotTooClose', Get_Reacheable_Waypoint(), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={ 'pose_in': 'position', 'distance': 'distance', 'pose_out': 'pose_out' }) # x:81 y:400 OperatableStateMachine.add('GEtUmbrellaPosition', GetAttribute(attributes=["position"]), transitions={'done': 'NotTooClose'}, autonomy={'done': Autonomy.Off}, remapping={ 'object': 'umbrella', 'position': 'position' }) # x:108 y:131 OperatableStateMachine.add( 'Lift head', _sm_lift_head_9, transitions={'finished': 'Find umbrella'}, autonomy={'finished': Autonomy.Inherit}) # x:150 y:315 OperatableStateMachine.add( 'log', LogKeyState(text="found umbrella: {}", severity=Logger.REPORT_HINT), transitions={'done': 'GEtUmbrellaPosition'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'umbrella'}) # x:267 y:312 OperatableStateMachine.add( 'say see taxy', SaraSay( sentence= "I see the taxi. Over there. The person with the umbrella.", input_keys=[], emotion=0, block=True), transitions={'done': 'log'}, autonomy={'done': Autonomy.Off}) # x:111 y:204 OperatableStateMachine.add('Find umbrella', _sm_find_umbrella_8, transitions={ 'finished': 'say see taxy', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'umbrella': 'umbrella'}) with _state_machine: # x:46 y:16 OperatableStateMachine.add('GetOrigin', Get_Robot_Pose(), transitions={'done': 'Init_Sequence'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'poseOrigin'}) # x:63 y:574 OperatableStateMachine.add('GetTaxi', _sm_gettaxi_14, transitions={ 'finished': 'say succeed', 'failed': 'say fail' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'distance': 'distance', 'taxi': 'taxi', 'umbrella': 'umbrella' }) # x:103 y:369 OperatableStateMachine.add('confirm', _sm_confirm_13, transitions={ 'false': 'nevermind', 'done': 'say taxi' }, autonomy={ 'false': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'person': 'person', 'pronoun': 'pronoun' }) # x:289 y:142 OperatableStateMachine.add( 'say nobody', SaraSay(sentence="I don't see any raised hand.", input_keys=[], emotion=0, block=True), transitions={'done': 'Get Gender'}, autonomy={'done': Autonomy.Off}) # x:571 y:104 OperatableStateMachine.add( 'say fail', SaraSay(sentence="I failed this scenario. Sorry.", input_keys=[], emotion=3, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:245 y:595 OperatableStateMachine.add('say succeed', SaraSay( sentence="here is the taxy driver.", input_keys=[], emotion=5, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:103 y:483 OperatableStateMachine.add( 'say taxi', SaraSay(sentence="Ok, follow me to the taxi then.", input_keys=[], emotion=0, block=True), transitions={'done': 'GetTaxi'}, autonomy={'done': Autonomy.Off}) # x:49 y:101 OperatableStateMachine.add( 'Init_Sequence', self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM, 'Init_Sequence'), transitions={ 'finished': 'Get Gender', 'failed': 'say fail' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:17 y:288 OperatableStateMachine.add('nevermind', _sm_nevermind_12, transitions={ 'done': 'Get Gender', 'failed': 'say fail' }, autonomy={ 'done': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'poseOrigin': 'poseOrigin'}) # x:30 y:192 OperatableStateMachine.add('Get Gender', _sm_get_gender_11, transitions={ 'none_found': 'say nobody', 'done': 'get closer' }, autonomy={ 'none_found': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'name': 'name', 'distance': 'distance', 'person': 'person', 'pronoun': 'pronoun' }) # x:259 y:310 OperatableStateMachine.add('get closer', _sm_get_closer_10, transitions={'finished': 'confirm'}, autonomy={'finished': Autonomy.Inherit}, remapping={'person': 'person'}) 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): # x:860 y:786, x:837 y:171, x:828 y:43 _state_machine = OperatableStateMachine( outcomes=['found', 'failed', 'not_found'], input_keys=['question'], output_keys=['entityFound']) _state_machine.userdata.question = "" _state_machine.userdata.entityFound = "" _state_machine.userdata.personKey = "person" _state_machine.userdata.index = -1 _state_machine.userdata.rotation180degres = -3.14 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:458, x:130 y:458 _sm_keep_looking_at_person_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['personID']) with _sm_keep_looking_at_person_0: # x:79 y:77 OperatableStateMachine.add('keep looking', KeepLookingAt(), transitions={'failed': 'keep looking'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'personID'}) # x:30 y:458, x:130 y:458 _sm_ask_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['question'], output_keys=['answer']) with _sm_ask_1: # x:57 y:66 OperatableStateMachine.add('wait', WaitState(wait_time=3), transitions={'done': 'Action_Ask'}, autonomy={'done': Autonomy.Off}) # x:46 y:155 OperatableStateMachine.add( 'Action_Ask', self.use_behavior( sara_flexbe_behaviors__Action_AskSM, 'ask while looking at person/ask/Action_Ask'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'question': 'question', 'answer': 'answer' }) # x:871 y:712, x:874 y:127 _sm_get_real_id_2 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['entity', 'personKey', 'FIFO'], output_keys=['personID']) with _sm_get_real_id_2: # x:47 y:32 OperatableStateMachine.add( 'get posittion face or entity', CalculationState(calculation=lambda x: x.face.boundingBox. Center if x.face.id != '' else x.position), transitions={'done': 'get direction to point'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'pointToLook' }) # x:27 y:351 OperatableStateMachine.add('look at point', SaraSetHeadAngleKey(), transitions={'done': 'wait 1'}, autonomy={'done': Autonomy.Off}, remapping={ 'yaw': 'yaw', 'pitch': 'pitch' }) # x:40 y:423 OperatableStateMachine.add( 'wait 1', WaitState(wait_time=1), transitions={'done': 'list entity in front'}, autonomy={'done': Autonomy.Off}) # x:24 y:491 OperatableStateMachine.add( 'list entity in front', list_entities_by_name(frontality_level=0.5, distance_max=3), transitions={ 'found': 'fitler the entity list to remove id already checked', 'none_found': 'set key' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'personKey', 'entity_list': 'entity_list', 'number': 'number' }) # x:42 y:632 OperatableStateMachine.add( 'calcul first entity ID', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'say real id'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'filteredEntityList', 'output_value': 'personID' }) # x:518 y:578 OperatableStateMachine.add( 'say real id', SaraSay(sentence=lambda x: "The real ID is " + str(x[0]) + ".", input_keys=["personID"], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'personID': 'personID'}) # x:539 y:164 OperatableStateMachine.add('set key', SetKey(Value=0), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'personID'}) # x:34 y:234 OperatableStateMachine.add('pitch to 0.1 if no face', FlexibleCalculationState( calculation=lambda x: x[0] if x[1].face.id != '' else 0.1, input_keys=["pitch", "entity"]), transitions={'done': 'look at point'}, autonomy={'done': Autonomy.Off}, remapping={ 'pitch': 'pitch', 'entity': 'entity', 'output_value': 'pitch' }) # x:37 y:149 OperatableStateMachine.add('get direction to point', Get_direction_to_point( frame_origin="base_link", frame_reference="head_link"), transitions={ 'done': 'pitch to 0.1 if no face', 'fail': 'set key' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'targetPoint': 'pointToLook', 'yaw': 'yaw', 'pitch': 'pitch' }) # x:29 y:560 OperatableStateMachine.add( 'fitler the entity list to remove id already checked', FilterKey(filter_function=lambda x: x[0].ID not in x[1], input_keys=["input_list", "FIFO"]), transitions={ 'not_empty': 'calcul first entity ID', 'empty': 'set key' }, autonomy={ 'not_empty': Autonomy.Off, 'empty': Autonomy.Off }, remapping={ 'input_list': 'entity_list', 'FIFO': 'FIFO', 'output_list': 'filteredEntityList' }) # x:30 y:458, x:710 y:24 _sm_tourne_tete_et_base_3 = OperatableStateMachine( outcomes=['done', 'failed'], input_keys=['index', 'rotation180degres']) with _sm_tourne_tete_et_base_3: # x:57 y:27 OperatableStateMachine.add( 'decide', DecisionState(outcomes=["_0", "_1", "_2", "_3", "_4"], conditions=lambda x: "_" + str(x)), transitions={ '_0': 'look center', '_1': 'look center_2', '_2': 'look center_3', '_3': 'action_turn', '_4': 'look center_5' }, autonomy={ '_0': Autonomy.Off, '_1': Autonomy.Off, '_2': Autonomy.Off, '_3': Autonomy.Off, '_4': Autonomy.Off }, remapping={'input_value': 'index'}) # x:186 y:236 OperatableStateMachine.add('look right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'wait right'}, autonomy={'done': Autonomy.Off}) # x:208 y:307 OperatableStateMachine.add('wait right', WaitState(wait_time=3), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:322 y:109 OperatableStateMachine.add('look center_3', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'wait center_3'}, autonomy={'done': Autonomy.Off}) # x:325 y:174 OperatableStateMachine.add('wait center_3', WaitState(wait_time=3), transitions={'done': 'look right_2'}, autonomy={'done': Autonomy.Off}) # x:318 y:240 OperatableStateMachine.add('look right_2', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'wait right_2'}, autonomy={'done': Autonomy.Off}) # x:345 y:310 OperatableStateMachine.add('wait right_2', WaitState(wait_time=3), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:489 y:178 OperatableStateMachine.add('look left_2_2', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'waitleft_2_2'}, autonomy={'done': Autonomy.Off}) # x:506 y:247 OperatableStateMachine.add('waitleft_2_2', WaitState(wait_time=3), transitions={'done': 'look center_4'}, autonomy={'done': Autonomy.Off}) # x:501 y:319 OperatableStateMachine.add('look center_4', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'wait center_4'}, autonomy={'done': Autonomy.Off}) # x:520 y:407 OperatableStateMachine.add('wait center_4', WaitState(wait_time=3), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:476 y:109 OperatableStateMachine.add( 'action_turn', self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'tourne tete et base/action_turn'), transitions={ 'finished': 'look left_2_2', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation180degres'}) # x:199 y:108 OperatableStateMachine.add('look center_2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'wait center_2'}, autonomy={'done': Autonomy.Off}) # x:51 y:113 OperatableStateMachine.add('look center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'wait center'}, autonomy={'done': Autonomy.Off}) # x:56 y:174 OperatableStateMachine.add('wait center', WaitState(wait_time=4), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:206 y:173 OperatableStateMachine.add('wait center_2', WaitState(wait_time=2), transitions={'done': 'look right'}, autonomy={'done': Autonomy.Off}) # x:730 y:110 OperatableStateMachine.add('look center_5', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'wait center_5'}, autonomy={'done': Autonomy.Off}) # x:741 y:193 OperatableStateMachine.add('wait center_5', WaitState(wait_time=3), transitions={'done': 'look right_5'}, autonomy={'done': Autonomy.Off}) # x:733 y:296 OperatableStateMachine.add('look right_5', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'wait right_5'}, autonomy={'done': Autonomy.Off}) # x:731 y:422 OperatableStateMachine.add('wait right_5', WaitState(wait_time=3), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:449 y:45, x:454 y:135, x:447 y:195, x:446 y:252, x:430 y:458, x:530 y:458 _sm_ask_while_looking_at_person_4 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['personID', 'question'], output_keys=['answer'], conditions=[('finished', [('ask', 'finished')]), ('failed', [('ask', 'failed')]), ('finished', [('keep looking at person', 'finished')]), ('failed', [('keep looking at person', 'failed')])]) with _sm_ask_while_looking_at_person_4: # x:97 y:55 OperatableStateMachine.add('ask', _sm_ask_1, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'question': 'question', 'answer': 'answer' }) # x:65 y:197 OperatableStateMachine.add('keep looking at person', _sm_keep_looking_at_person_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'personID': 'personID'}) with _state_machine: # x:30 y:115 OperatableStateMachine.add( 'Init_Sequence', self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM, 'Init_Sequence'), transitions={ 'finished': 'create fifo', 'failed': 'create fifo' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:164 y:62 OperatableStateMachine.add('for loop', ForLoopWithInput(repeat=4), transitions={ 'do': 'tourne tete et base', 'end': 'not_found' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={ 'index_in': 'index', 'index_out': 'index' }) # x:260 y:557 OperatableStateMachine.add('ask while looking at person', _sm_ask_while_looking_at_person_4, transitions={ 'finished': 'answer contains yes', 'failed': 'add id' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'personID': 'personID', 'question': 'question', 'answer': 'answer' }) # x:289 y:480 OperatableStateMachine.add( 'get personID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'ask while looking at person'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'personEntity', 'output_value': 'personID' }) # x:270 y:643 OperatableStateMachine.add( 'answer contains yes', RegexTester( regex=".*((yes)|(Yes)|(yep)|(sure)|(of course)).*"), transitions={ 'true': 'say ty', 'false': 'add id' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'text': 'answer', 'result': 'result' }) # x:173 y:260 OperatableStateMachine.add('look center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'for loop'}, autonomy={'done': Autonomy.Off}) # x:372 y:89 OperatableStateMachine.add('tourne tete et base', _sm_tourne_tete_et_base_3, transitions={ 'done': 'get list of person', 'failed': 'not_found' }, autonomy={ 'done': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'index': 'index', 'rotation180degres': 'rotation180degres' }) # x:263 y:322 OperatableStateMachine.add( 'fitler the entity list to remove id already checked', FilterKey(filter_function=lambda x: x[0].ID not in x[1], input_keys=["input_list", "FIFO"]), transitions={ 'not_empty': 'get first entity', 'empty': 'look center' }, autonomy={ 'not_empty': Autonomy.Off, 'empty': Autonomy.Off }, remapping={ 'input_list': 'entityList', 'FIFO': 'FIFO', 'output_list': 'filteredEntityList' }) # x:268 y:388 OperatableStateMachine.add( 'get first entity', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'get real id'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'filteredEntityList', 'output_value': 'personEntity' }) # x:296 y:168 OperatableStateMachine.add( 'get list of person', list_entities_by_name(frontality_level=0.5, distance_max=3), transitions={ 'found': 'fitler the entity list to remove id already checked', 'none_found': 'for loop' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'personKey', 'entity_list': 'entityList', 'number': 'numberOfEntity' }) # x:111 y:533 OperatableStateMachine.add( 'add id', FIFO_Add(), transitions={'done': 'say keep looking'}, autonomy={'done': Autonomy.Off}, remapping={ 'Entry': 'personID', 'FIFO': 'FIFO' }) # x:609 y:651 OperatableStateMachine.add('say ty', SaraSay(sentence="Thank you.", input_keys=[], emotion=0, block=True), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}) # x:113 y:386 OperatableStateMachine.add('say keep looking', SaraSay(sentence="Ok, never mind.", input_keys=[], emotion=0, block=True), transitions={'done': 'for loop'}, autonomy={'done': Autonomy.Off}) # x:472 y:438 OperatableStateMachine.add('get real id', _sm_get_real_id_2, transitions={ 'finished': 'ask while looking at person', 'failed': 'get personID' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'entity': 'personEntity', 'personKey': 'personKey', 'FIFO': 'FIFO', 'personID': 'personID' }) # x:35 y:53 OperatableStateMachine.add('create fifo', FIFO_New(), transitions={'done': 'for loop'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'FIFO'}) return _state_machine
def create(self): joy_topic = '/hmi/joystick' # x:30 y:365, x:846 y:376 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.unused = None _state_machine.userdata.joy_msg = Joy() _state_machine.userdata.text_msg = TextCommand() # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:353 _sm_waituntiltextcmd_0 = OperatableStateMachine( outcomes=['received'], output_keys=['text_msg']) with _sm_waituntiltextcmd_0: # x:87 y:101 OperatableStateMachine.add( 'WaitTextMsg', WaitForMessageState( topic='/control', condition=lambda x: x.type == 'flexbe/action', buffered=False, clear=True), transitions={ 'received': 'received', 'unavailable': 'Wait1s' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'text_msg'}) # x:314 y:97 OperatableStateMachine.add('Wait1s', WaitState(wait_time=1), transitions={'done': 'WaitTextMsg'}, autonomy={'done': Autonomy.Off}) # x:397 y:287, x:85 y:291, x:504 y:228, x:330 y:297, x:459 y:297, x:530 y:297 _sm_joystickmovements_1 = ConcurrencyContainer( outcomes=['failed', 'button1234', 'timeout'], input_keys=['joy_msg'], output_keys=['joy_msg'], conditions=[('timeout', [('JoystickControl', 'done')]), ('failed', [('WaitButton1234Pressed', 'unavailable')]), ('button1234', [('WaitButton1234Pressed', 'received')]) ]) with _sm_joystickmovements_1: # x:385 y:129 OperatableStateMachine.add( 'JoystickControl', JoystickJointControl( joints_topic='/joint_states', goal_joints_topic= '/motion/controller/joint_state/out_joints_src_reset', joy_topic=joy_topic, buttons=[(4, 0.6, 'ear_l_joint', -3.0, 1.0), (6, -0.6, 'ear_l_joint', -3.0, 1.0), (5, 0.6, 'ear_r_joint', -3.0, 1.0), (7, -0.6, 'ear_r_joint', -3.0, 1.0)], axes=[(0, 0, 'eyes_yaw', -1.5, 1.5), (1, 0, 'eyes_pitch', -1.5, 1.5), (4, -0.4, 'head_joint4', -1.5, 1.5), (5, 0.4, 'head_joint2', -1.5, 1.5), (3, 0.5, 'mouth_joint', -0.6, 0)], timeout=5.0), transitions={'done': 'timeout'}, autonomy={'done': Autonomy.Off}) # x:130 y:136 OperatableStateMachine.add( 'WaitButton1234Pressed', WaitForMessageState( topic=joy_topic, condition=lambda msg: any(msg.buttons[0:4]), buffered=False, clear=True), transitions={ 'received': 'button1234', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'joy_msg'}) # x:205 y:274, x:300 y:274, x:68 y:275, x:449 y:266, x:595 y:272, x:833 y:323, x:638 y:392, x:730 y:353, x:949 y:274 _sm_randmovements_2 = ConcurrencyContainer( outcomes=['failed', 'timeout', 'joy_msg', 'text_msg'], input_keys=['unused', 'joy_msg', 'text_msg'], output_keys=['joy_msg', 'text_msg', 'unused'], conditions=[('failed', [('WaitJoystick', 'unavailable')]), ('failed', [('RandHeadMoves', 'failed')]), ('joy_msg', [('WaitJoystick', 'received')]), ('timeout', [('RandHeadMoves', 'done')]), ('text_msg', [('WaitUntilTextCmd', 'received')])]) with _sm_randmovements_2: # x:88 y:74 OperatableStateMachine.add( 'WaitJoystick', WaitForMessageState( topic=joy_topic, condition=lambda msg: any(msg.buttons) or any(msg.axes), buffered=False, clear=True), transitions={ 'received': 'joy_msg', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'joy_msg'}) # x:332 y:60 OperatableStateMachine.add( 'RandHeadMoves', RandJointsMovements(controller='joint_state_head', duration=10, interval=[2.0, 5.0], joints=['head_joint2', 'head_joint4'], minimal=[-0.2, -0.3], maximal=[0.3, 0.3]), transitions={ 'done': 'timeout', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'config': 'unused'}) # x:647 y:50 OperatableStateMachine.add('WaitUntilTextCmd', _sm_waituntiltextcmd_0, transitions={'received': 'text_msg'}, autonomy={'received': Autonomy.Inherit}, remapping={'text_msg': 'text_msg'}) with _state_machine: # x:12 y:169 OperatableStateMachine.add( 'SetNominalPose', SetJointState(controller='motion/controller/joint_state_head', pose_param='nominal', pose_ns='saved_msgs/joint_state', tolerance=0.017, timeout=10.0, joint_topic="joint_states"), transitions={ 'done': 'RandMovements', 'failed': 'failed', 'timeout': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off, 'timeout': Autonomy.Off }) # x:127 y:370 OperatableStateMachine.add('JoystickMovements', _sm_joystickmovements_1, transitions={ 'failed': 'failed', 'button1234': 'ProcessButton', 'timeout': 'RandMovements' }, autonomy={ 'failed': Autonomy.Inherit, 'button1234': Autonomy.Inherit, 'timeout': Autonomy.Inherit }, remapping={'joy_msg': 'joy_msg'}) # x:248 y:186 OperatableStateMachine.add( 'CheckButtonPressed', DecisionState(outcomes=['button1234', 'other'], conditions=lambda msg: 'button1234' if any(msg.buttons[0:4]) else 'other'), transitions={ 'button1234': 'ProcessButton', 'other': 'JoystickMovements' }, autonomy={ 'button1234': Autonomy.Off, 'other': Autonomy.Off }, remapping={'input_value': 'joy_msg'}) # x:690 y:123 OperatableStateMachine.add( 'SetHeadNominalPose', SetJointState(controller='motion/controller/joint_state_head', pose_param='head_nominal', pose_ns='saved_msgs/joint_state', tolerance=0.017, timeout=10.0, joint_topic="joint_states"), transitions={ 'done': 'ExecuteAction', 'failed': 'failed', 'timeout': 'failed' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off, 'timeout': Autonomy.Off }) # x:425 y:181 OperatableStateMachine.add( 'ProcessButton', CalculationState(calculation=self.process_joy_msg), transitions={'done': 'SetHeadNominalPose'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'joy_msg', 'output_value': 'action_name' }) # x:272 y:100 OperatableStateMachine.add( 'ProcessTextMsg', CalculationState(calculation=self.process_text_msg), transitions={'done': 'SetHeadNominalPose'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'text_msg', 'output_value': 'action_name' }) # x:457 y:12 OperatableStateMachine.add( 'RandomAction', CalculationState(calculation=self.random_action), transitions={'done': 'SetHeadNominalPose'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'unused', 'output_value': 'action_name' }) # x:383 y:325 OperatableStateMachine.add( 'ExecuteAction', ExecuteJointTrajectoryKey( controller='motion/controller/joint_trajectory', trajectory_ns='saved_msgs/joint_trajectory'), transitions={ 'success': 'JoystickMovements', 'unavalible': 'JoystickMovements', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed' }, autonomy={ 'success': Autonomy.Off, 'unavalible': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off }, remapping={'trajectory_param': 'action_name'}) # x:308 y:7 OperatableStateMachine.add( 'RandomChoice', DecisionState(outcomes=['move', 'continue'], conditions=lambda x: 'continue' if random.random() < 0.4 else 'move'), transitions={ 'move': 'RandomAction', 'continue': 'RandMovements' }, autonomy={ 'move': Autonomy.Off, 'continue': Autonomy.Off }, remapping={'input_value': 'unused'}) # x:117 y:39 OperatableStateMachine.add('RandMovements', _sm_randmovements_2, transitions={ 'failed': 'failed', 'timeout': 'RandomChoice', 'joy_msg': 'CheckButtonPressed', 'text_msg': 'ProcessTextMsg' }, autonomy={ 'failed': Autonomy.Inherit, 'timeout': Autonomy.Inherit, 'joy_msg': Autonomy.Inherit, 'text_msg': Autonomy.Inherit }, remapping={ 'unused': 'unused', 'joy_msg': 'joy_msg', 'text_msg': 'text_msg' }) return _state_machine
def create(self): # x:860 y:152, x:755 y:568 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['pose']) _state_machine.userdata.pose = "crowd" _state_machine.userdata.name = "destination" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:130 y:365 _sm_look_around_0 = OperatableStateMachine(outcomes=['failed']) with _sm_look_around_0: # x:78 y:40 OperatableStateMachine.add('set pitch', SetKey(Value=0.9), transitions={'done': 'get angle'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'pitch'}) # x:348 y:210 OperatableStateMachine.add('set head', SaraSetHeadAngleKey(), transitions={'done': 'get angle'}, autonomy={'done': Autonomy.Off}, remapping={ 'yaw': 'yaw', 'pitch': 'pitch' }) # x:204 y:248 OperatableStateMachine.add( 'limit yaw', CalculationState(calculation=lambda x: max(min(x, 1), -1)), transitions={'done': 'set head'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'yaw', 'output_value': 'yaw' }) # x:191 y:126 OperatableStateMachine.add('get angle', GetClosestObstacle(topic="/scan", maximumDistance=2), transitions={'done': 'limit yaw'}, autonomy={'done': Autonomy.Off}, remapping={'Angle': 'yaw'}) # x:30 y:365, x:130 y:365 _sm_move_1 = OperatableStateMachine(outcomes=['arrived', 'failed'], input_keys=['pose']) with _sm_move_1: # x:95 y:122 OperatableStateMachine.add('move', SaraMoveBase(reference="map"), transitions={ 'arrived': 'arrived', 'failed': 'failed' }, autonomy={ 'arrived': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'pose': 'pose'}) # x:259 y:573, x:491 y:362, x:491 y:453 _sm_manage_name_2 = OperatableStateMachine( outcomes=['done', 'too much', 'not found'], input_keys=['pose'], output_keys=['pose', 'name']) with _sm_manage_name_2: # x:39 y:48 OperatableStateMachine.add( 'check if Pose', CheckConditionState(predicate=lambda x: type(x) is type([])), transitions={ 'true': 'getname', 'false': 'check if string' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'pose'}) # x:257 y:264 OperatableStateMachine.add( 'getcontainers', CalculationState(calculation=lambda x: x[1:]), transitions={'done': 'get wonderland entity'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'pose', 'output_value': 'containers' }) # x:232 y:354 OperatableStateMachine.add('get wonderland entity', WonderlandGetEntityVerbal(), transitions={ 'one': 'get waypoint', 'multiple': 'too much', 'none': 'not found', 'error': 'not found' }, autonomy={ 'one': Autonomy.Off, 'multiple': Autonomy.Off, 'none': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'name': 'name', 'containers': 'containers', 'entities': 'entities', 'firstEntity': 'firstEntity' }) # x:238 y:442 OperatableStateMachine.add('get waypoint', GetAttribute(attributes=["waypoint"]), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={ 'object': 'firstEntity', 'waypoint': 'pose' }) # x:256 y:166 OperatableStateMachine.add( 'getname', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'getcontainers'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'pose', 'output_value': 'name' }) # x:42 y:180 OperatableStateMachine.add( 'check if string', CheckConditionState(predicate=lambda x: type(x) is type("")), transitions={ 'true': 'remap to name', 'false': 'done' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'pose'}) # x:40 y:251 OperatableStateMachine.add( 'remap to name', CalculationState(calculation=lambda x: x), transitions={'done': 'set containers empty'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'pose', 'output_value': 'name' }) # x:30 y:326 OperatableStateMachine.add( 'set containers empty', SetKey(Value=[]), transitions={'done': 'get wonderland entity'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'containers'}) # x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365 _sm_move_concurent_3 = ConcurrencyContainer( outcomes=['arrived', 'failed'], input_keys=['pose'], conditions=[('arrived', [('Move', 'arrived')]), ('failed', [('Move', 'failed')]), ('failed', [('Look around', 'failed')])]) with _sm_move_concurent_3: # x:30 y:40 OperatableStateMachine.add('Move', _sm_move_1, transitions={ 'arrived': 'arrived', 'failed': 'failed' }, autonomy={ 'arrived': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'pose'}) # x:268 y:74 OperatableStateMachine.add('Look around', _sm_look_around_0, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}) with _state_machine: # x:54 y:27 OperatableStateMachine.add('SetCount', SetKey(Value=2), transitions={'done': 'manage name'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Count'}) # x:258 y:250 OperatableStateMachine.add('stuck', SaraSay(sentence="I'm getting stuck.", input_keys=[], emotion=2, block=True), transitions={'done': 'Count--'}, autonomy={'done': Autonomy.Off}) # x:49 y:251 OperatableStateMachine.add('try again', SaraSay(sentence="But I'm still going.", input_keys=[], emotion=1, block=False), transitions={'done': 'Move concurent'}, autonomy={'done': Autonomy.Off}) # x:360 y:508 OperatableStateMachine.add( 'sorry', SaraSay(sentence="Well. It seem's I can't go there.", input_keys=[], emotion=2, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:672 y:147 OperatableStateMachine.add('set blink', SetExpression(emotion=6, brightness=-1), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:252 y:146 OperatableStateMachine.add('Move concurent', _sm_move_concurent_3, transitions={ 'arrived': 'reset head', 'failed': 'stuck' }, autonomy={ 'arrived': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'pose'}) # x:254 y:353 OperatableStateMachine.add( 'Count--', CalculationState(calculation=lambda x: x - 1), transitions={'done': 'check count'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Count', 'output_value': 'Count' }) # x:37 y:351 OperatableStateMachine.add( 'check count', CheckConditionState(predicate=lambda x: x >= 0), transitions={ 'true': 'try again', 'false': 'sorry' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'Count'}) # x:497 y:149 OperatableStateMachine.add('reset head', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'set blink'}, autonomy={'done': Autonomy.Off}) # x:269 y:21 OperatableStateMachine.add('manage name', _sm_manage_name_2, transitions={ 'done': 'set head', 'too much': 'say too much', 'not found': 'say not known' }, autonomy={ 'done': Autonomy.Inherit, 'too much': Autonomy.Inherit, 'not found': Autonomy.Inherit }, remapping={ 'pose': 'pose', 'name': 'name' }) # x:46 y:147 OperatableStateMachine.add('set head', SaraSetHeadAngle(pitch=0.8, yaw=0), transitions={'done': 'Move concurent'}, autonomy={'done': Autonomy.Off}) # x:477 y:333 OperatableStateMachine.add( 'say too much', SaraSay( sentence=lambda x: "There is more than one " + x[0] + ".", input_keys=["poseName"], emotion=3, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'poseName': 'name'}) # x:445 y:418 OperatableStateMachine.add( 'say not known', SaraSay(sentence=lambda x: "I don't know where the " + x[0] + " is.", input_keys=["poseName"], emotion=0, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'poseName': 'name'}) return _state_machine
def create(self): # x:84 y:465, x:727 y:360 _state_machine = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['className'], output_keys=['entity']) _state_machine.userdata.className = "bottle" _state_machine.userdata.entity = None # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365 _sm_look_at_0 = OperatableStateMachine(outcomes=['end'], input_keys=['ID']) with _sm_look_at_0: # x:75 y:135 OperatableStateMachine.add('look at', KeepLookingAt(), transitions={'failed': 'look at'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:798 y:597 _sm_rotation360_1 = OperatableStateMachine(outcomes=['end']) with _sm_rotation360_1: # x:42 y:34 OperatableStateMachine.add('Set 180 degres', SetKey(Value=3.1416), transitions={'done': 'rotate center'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:610 y:293 OperatableStateMachine.add('Look Right', SaraSetHeadAngle(pitch=0.5, yaw=1), transitions={'done': 'Rotate Right'}, autonomy={'done': Autonomy.Off}) # x:406 y:58 OperatableStateMachine.add('Rotate Left', WaitState(wait_time=5), transitions={'done': 'rotate center'}, autonomy={'done': Autonomy.Off}) # x:410 y:313 OperatableStateMachine.add('Rotate Right', WaitState(wait_time=5), transitions={'done': 'look center 2'}, autonomy={'done': Autonomy.Off}) # x:209 y:50 OperatableStateMachine.add('Look Left', SaraSetHeadAngle(pitch=0.5, yaw=-1), transitions={'done': 'Rotate Left'}, autonomy={'done': Autonomy.Off}) # x:216 y:186 OperatableStateMachine.add('Rotate Left 2', WaitState(wait_time=5), transitions={'done': 'Look Left'}, autonomy={'done': Autonomy.Off}) # x:630 y:170 OperatableStateMachine.add('wait 5', WaitState(wait_time=5), transitions={'done': 'Look Right'}, autonomy={'done': Autonomy.Off}) # x:603 y:47 OperatableStateMachine.add('rotate center', SaraSetHeadAngle(pitch=0.5, yaw=0), transitions={'done': 'wait 5'}, autonomy={'done': Autonomy.Off}) # x:208 y:308 OperatableStateMachine.add('look center 2', SaraSetHeadAngle(pitch=0.5, yaw=0), transitions={'done': 'Rotate Left 2'}, autonomy={'done': Autonomy.Off}) # x:683 y:188 _sm_find_entity_2 = OperatableStateMachine(outcomes=['found'], input_keys=['className'], output_keys=['entity']) with _sm_find_entity_2: # x:181 y:178 OperatableStateMachine.add('find_entity', list_entities_by_name( frontality_level=0.5, distance_max=2), transitions={ 'found': 'Get Entity', 'none_found': 'find_entity' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'className', 'entity_list': 'entity_list', 'number': 'number' }) # x:454 y:178 OperatableStateMachine.add( 'Get Entity', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'entity' }) # x:371 y:306, x:130 y:365, x:230 y:365 _sm_look_for_2_sec_3 = ConcurrencyContainer( outcomes=['done'], input_keys=['ID'], conditions=[('done', [('WaitState 2', 'done')]), ('done', [('Look at', 'end')])]) with _sm_look_for_2_sec_3: # x:84 y:166 OperatableStateMachine.add('Look at', _sm_look_at_0, transitions={'end': 'done'}, autonomy={'end': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:345 y:187 OperatableStateMachine.add('WaitState 2', WaitState(wait_time=3), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:372 y:27, x:370 y:220, x:368 y:100, x:352 y:305, x:460 y:465 _sm_find_entity_while_turning360_4 = ConcurrencyContainer( outcomes=['found', 'not_found'], input_keys=['className'], output_keys=['entity'], conditions=[('not_found', [('Rotation360', 'end')]), ('found', [('Find Entity', 'found')]), ('not_found', [('wait', 'done')])]) with _sm_find_entity_while_turning360_4: # x:131 y:44 OperatableStateMachine.add('Find Entity', _sm_find_entity_2, transitions={'found': 'found'}, autonomy={'found': Autonomy.Inherit}, remapping={ 'className': 'className', 'entity': 'entity' }) # x:129 y:197 OperatableStateMachine.add('Rotation360', _sm_rotation360_1, transitions={'end': 'not_found'}, autonomy={'end': Autonomy.Inherit}) # x:149 y:306 OperatableStateMachine.add('wait', WaitState(wait_time=30), transitions={'done': 'not_found'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:55 y:41 OperatableStateMachine.add( 'Look Front Center', SaraSetHeadAngle(pitch=0.7, yaw=0), transitions={'done': 'Find Entity WHILE Turning360'}, autonomy={'done': Autonomy.Off}) # x:345 y:156 OperatableStateMachine.add('Look Center Not Found', SaraSetHeadAngle(pitch=0.7, yaw=0), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:59 y:376 OperatableStateMachine.add('Log Entity', LogKeyState( text="Found entity: {}", severity=Logger.REPORT_HINT), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'entity'}) # x:26 y:121 OperatableStateMachine.add('Find Entity WHILE Turning360', _sm_find_entity_while_turning360_4, transitions={ 'found': 'get ID', 'not_found': 'Look Center Not Found' }, autonomy={ 'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit }, remapping={ 'className': 'className', 'entity': 'entity' }) # x:45 y:290 OperatableStateMachine.add('look for 2 sec', _sm_look_for_2_sec_3, transitions={'done': 'Log Entity'}, autonomy={'done': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:62 y:211 OperatableStateMachine.add( 'get ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'look for 2 sec'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'ID' }) return _state_machine
def create(self): # x:254 y:640, x:565 y:232, x:530 y:448 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Count", "bottle", "behavior/Count/CountedObject"] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:41 y:32 OperatableStateMachine.add('get name', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Say_Start'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Action', 'output_value': 'className'}) # x:22 y:213 OperatableStateMachine.add('Action_count', self.use_behavior(sara_flexbe_behaviors__Action_countSM, 'Action_count'), transitions={'done': 'get paramname', 'failed': 'failed'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'className': 'className', 'Count': 'Count'}) # x:14 y:462 OperatableStateMachine.add('concat', FlexibleCalculationState(calculation=lambda x: "I counted "+str(x[0])+" "+str(x[1])+".", input_keys=["Count", "className"]), transitions={'done': 'Say_Count'}, autonomy={'done': Autonomy.Off}, remapping={'Count': 'Count', 'className': 'className', 'output_value': 'sentence'}) # x:28 y:388 OperatableStateMachine.add('store param', SetRosParamKey(), transitions={'done': 'concat'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Count', 'ParamName': 'ParamName'}) # x:45 y:290 OperatableStateMachine.add('get paramname', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'store param'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Action', 'output_value': 'ParamName'}) # x:17 y:633 OperatableStateMachine.add('set head back', SaraSetHeadAngle(pitch=-0.3, yaw=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:36 y:120 OperatableStateMachine.add('Say_Start', SaraSay(sentence=lambda x: "I'm starting to count the "+str(x)+"s.", input_keys=[], emotion=0, block=True), transitions={'done': 'Action_count'}, autonomy={'done': Autonomy.Off}) # x:27 y:546 OperatableStateMachine.add('Say_Count', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'set head back'}, autonomy={'done': Autonomy.Off}) 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): # x:660 y:509, x:880 y:203, x:715 y:440 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Pick", "bottle"] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365 _sm_group_0 = ConcurrencyContainer(outcomes=['done'], input_keys=['pose_out'], conditions=[ ('done', [('move', 'arrived')]), ('done', [('move', 'failed')]), ('done', [('3', 'done')]) ]) with _sm_group_0: # x:30 y:40 OperatableStateMachine.add('move', SaraMoveBase(reference="map"), transitions={ 'arrived': 'done', 'failed': 'done' }, autonomy={ 'arrived': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'pose': 'pose_out'}) # x:179 y:91 OperatableStateMachine.add('3', WaitState(wait_time=2), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:40 y:700 _sm_get_closer_1 = OperatableStateMachine(outcomes=['done'], input_keys=['Object']) with _sm_get_closer_1: # x:59 y:36 OperatableStateMachine.add('set targetpose', SetKey(Value="PreGripPose"), transitions={'done': 'say closer'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:88 y:374 OperatableStateMachine.add('set dist', SetKey(Value=0.8), transitions={'done': 'get close pos'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distance'}) # x:26 y:448 OperatableStateMachine.add('get close pos', Get_Reacheable_Waypoint(), transitions={'done': 'Group'}, autonomy={'done': Autonomy.Off}, remapping={ 'pose_in': 'Pos', 'distance': 'distance', 'pose_out': 'pose_out' }) # x:47 y:213 OperatableStateMachine.add( 'get pos', CalculationState(calculation=lambda x: x.position), transitions={'done': 'move head'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Object', 'output_value': 'Pos' }) # x:88 y:290 OperatableStateMachine.add('move head', SaraSetHeadAngle(pitch=0.7, yaw=0), transitions={'done': 'set dist'}, autonomy={'done': Autonomy.Off}) # x:201 y:156 OperatableStateMachine.add('move arm', MoveitMove(move=True, waitForExecution=True, group="RightArm", watchdog=15), transitions={ 'done': 'get pos', 'failed': 'get pos' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'target'}) # x:60 y:106 OperatableStateMachine.add( 'say closer', SaraSay(sentence="I need to get a bit closer.", input_keys=[], emotion=1, block=False), transitions={'done': 'move arm'}, autonomy={'done': Autonomy.Off}) # x:26 y:541 OperatableStateMachine.add('Group', _sm_group_0, transitions={'done': 'wait'}, autonomy={'done': Autonomy.Inherit}, remapping={'pose_out': 'pose_out'}) # x:33 y:625 OperatableStateMachine.add('wait', WaitState(wait_time=2), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:59 y:308, x:447 y:59, x:384 y:162 _sm_check_form_2 = OperatableStateMachine( outcomes=['done', 'fail_full', 'full_no_object'], input_keys=['Action']) with _sm_check_form_2: # x:31 y:40 OperatableStateMachine.add( 'check if gripper full', GetRosParam(ParamName="behavior/Gripper_Content"), transitions={ 'done': 'Say_Full', 'failed': 'cond' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'ObjectInGripper'}) # x:30 y:121 OperatableStateMachine.add( 'cond', CheckConditionState(predicate=lambda x: x[1] == ''), transitions={ 'true': 'not told', 'false': 'done' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'Action'}) # x:222 y:119 OperatableStateMachine.add( 'not told', SaraSay(sentence="Hum! They didn't told me what to pick", input_keys=[], emotion=1, block=True), transitions={'done': 'full_no_object'}, autonomy={'done': Autonomy.Off}) # x:242 y:31 OperatableStateMachine.add( 'Say_Full', SaraSay(sentence=lambda x: "Wait. There is already a " + x + "in my gripper.", input_keys=[], emotion=0, block=True), transitions={'done': 'fail_full'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:84 y:30 OperatableStateMachine.add('Check Form', _sm_check_form_2, transitions={ 'done': 'get name', 'fail_full': 'cause1', 'full_no_object': 'cause2' }, autonomy={ 'done': Autonomy.Inherit, 'fail_full': Autonomy.Inherit, 'full_no_object': Autonomy.Inherit }, remapping={'Action': 'Action'}) # x:28 y:452 OperatableStateMachine.add('Action_pick', self.use_behavior( Action_pickSM, 'Action_pick'), transitions={ 'success': 'Got_It', 'unreachable': 'for 1', 'not found': 'Say_lost', 'dropped': 'say missed' }, autonomy={ 'success': Autonomy.Inherit, 'unreachable': Autonomy.Inherit, 'not found': Autonomy.Inherit, 'dropped': Autonomy.Inherit }, remapping={'objectID': 'ID'}) # x:261 y:239 OperatableStateMachine.add('Get closer', _sm_get_closer_1, transitions={'done': 'Action_find'}, autonomy={'done': Autonomy.Inherit}, remapping={'Object': 'Object'}) # x:275 y:333 OperatableStateMachine.add('for 1', ForLoop(repeat=1), transitions={ 'do': 'Get closer', 'end': 'say giveup' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:416 y:264 OperatableStateMachine.add('say giveup', SaraSay(sentence="I give up", input_keys=[], emotion=1, block=True), transitions={'done': 'cause4'}, autonomy={'done': Autonomy.Off}) # x:284 y:496 OperatableStateMachine.add('say missed', SaraSay(sentence="Oops! I missed.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause4'}, autonomy={'done': Autonomy.Off}) # x:469 y:495 OperatableStateMachine.add( 'set param', SetRosParam(ParamName="behavior/GripperContent"), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'ObjectName'}) # x:82 y:115 OperatableStateMachine.add( 'get name', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Action_find'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Action', 'output_value': 'ObjectName' }) # x:511 y:20 OperatableStateMachine.add( 'cause1', SetKey(Value="My gripper was already full."), transitions={'done': 'setrosparam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:512 y:81 OperatableStateMachine.add( 'cause2', SetKey(Value="I didn't know what to pick."), transitions={'done': 'setrosparam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:511 y:143 OperatableStateMachine.add( 'cause3', SetKey(Value="I didn't found the object."), transitions={'done': 'setrosparam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:690 y:197 OperatableStateMachine.add( 'setrosparam', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:605 y:312 OperatableStateMachine.add( 'cause4', SetKey(Value="I was unable to pick the object."), transitions={'done': 'setrosparam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:30 y:188 OperatableStateMachine.add('Action_find', self.use_behavior( Action_findSM, 'Action_find'), transitions={ 'done': 'getID', 'failed': 'cause3' }, autonomy={ 'done': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'className': 'ObjectName', 'entity': 'Object' }) # x:49 y:322 OperatableStateMachine.add( 'getID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'Action_pick'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Object', 'output_value': 'ID' }) # x:284 y:422 OperatableStateMachine.add( 'Say_lost', SaraSay(sentence=lambda x: "Hum! I lost sight of the " + x, input_keys=[], emotion=0, block=True), transitions={'done': 'cause4'}, autonomy={'done': Autonomy.Off}) # x:281 y:572 OperatableStateMachine.add( 'Got_It', SaraSay(sentence=lambda x: "I have the " + x, input_keys=[], emotion=0, block=True), transitions={'done': 'set param'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:1103 y:529, x:321 y:227 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.Pose_Init = "IdlePose" _state_machine.userdata.Closed_Gripper_Width = 1 _state_machine.userdata.Open_Gripper_Width = 255 _state_machine.userdata.ID = 0 _state_machine.userdata.PoseStart = "PoseStart" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365 _sm_look_in_the_eyes_0 = OperatableStateMachine(outcomes=['finished']) with _sm_look_in_the_eyes_0: # x:189 y:48 OperatableStateMachine.add('wait 3', WaitState(wait_time=3), transitions={'done': 'say ready'}, autonomy={'done': Autonomy.Off}) # x:171 y:288 OperatableStateMachine.add('reset head', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:164 y:163 OperatableStateMachine.add( 'say ready', SaraSay( sentence= "Good, I see you want this bag. But, could you hand it to me please?", input_keys=[], emotion=0, block=True), transitions={'done': 'reset head'}, autonomy={'done': Autonomy.Off}) # x:30 y:365 _sm_look_at_it_1 = OperatableStateMachine(outcomes=['fail'], input_keys=['position']) with _sm_look_at_it_1: # x:126 y:99 OperatableStateMachine.add('look', LookAtPos(), transitions={ 'failed': 'look', 'done': 'look' }, autonomy={ 'failed': Autonomy.Off, 'done': Autonomy.Off }, remapping={'pos': 'position'}) # x:30 y:365 _sm_point_at_it_2 = OperatableStateMachine(outcomes=['finished'], input_keys=['position']) with _sm_point_at_it_2: # x:112 y:202 OperatableStateMachine.add( 'Action_point_at', self.use_behavior( Action_point_atSM, 'recevoir sac/Recevoir sac/look at bag/designate bag/point at it/Action_point_at' ), transitions={ 'finished': 'Action_point_at', 'failed': 'Action_point_at' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'targetPoint': 'position'}) # x:30 y:365 _sm_move_to_it_3 = OperatableStateMachine(outcomes=['arrived'], input_keys=['position']) with _sm_move_to_it_3: # x:204 y:52 OperatableStateMachine.add('set distance', SetKey(Value=1), transitions={'done': 'get pose'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distance'}) # x:211 y:185 OperatableStateMachine.add('get pose', Get_Reacheable_Waypoint(), transitions={'done': 'move to bag'}, autonomy={'done': Autonomy.Off}, remapping={ 'pose_in': 'position', 'distance': 'distance', 'pose_out': 'pose' }) # x:188 y:279 OperatableStateMachine.add('move to bag', SaraMoveBase(reference="map"), transitions={ 'arrived': 'arrived', 'failed': 'arrived' }, autonomy={ 'arrived': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'pose': 'pose'}) # x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365 _sm_designate_bag_4 = ConcurrencyContainer( outcomes=['finished'], input_keys=['position'], conditions=[('finished', [('look at it', 'fail')]), ('finished', [('point at it', 'finished')]), ('finished', [('move to it', 'arrived')])]) with _sm_designate_bag_4: # x:499 y:118 OperatableStateMachine.add('move to it', _sm_move_to_it_3, transitions={'arrived': 'finished'}, autonomy={'arrived': Autonomy.Inherit}, remapping={'position': 'position'}) # x:239 y:126 OperatableStateMachine.add('point at it', _sm_point_at_it_2, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'position': 'position'}) # x:45 y:121 OperatableStateMachine.add('look at it', _sm_look_at_it_1, transitions={'fail': 'finished'}, autonomy={'fail': Autonomy.Inherit}, remapping={'position': 'position'}) # x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365 _sm_receive_it_5 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['Closed_Gripper_Width', 'Open_Gripper_Width'], conditions=[('finished', [('Action_Receive_Bag', 'finished'), ('look in the eyes', 'finished')]), ('failed', [('Action_Receive_Bag', 'failed')])]) with _sm_receive_it_5: # x:30 y:149 OperatableStateMachine.add( 'Action_Receive_Bag', self.use_behavior( Action_Receive_BagSM, 'recevoir sac/Recevoir sac/receive it/Action_Receive_Bag'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'Closed_Gripper_Width': 'Closed_Gripper_Width', 'Open_Gripper_Width': 'Open_Gripper_Width', 'Closed_Gripper_Width': 'Closed_Gripper_Width' }) # x:259 y:150 OperatableStateMachine.add('look in the eyes', _sm_look_in_the_eyes_0, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}) # x:30 y:322 _sm_look_at_bag_6 = OperatableStateMachine(outcomes=['finished']) with _sm_look_at_bag_6: # x:35 y:70 OperatableStateMachine.add('setName', SetKey(Value="person"), transitions={'done': 'say 1'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'personName'}) # x:233 y:109 OperatableStateMachine.add( 'say 1', SaraSay(sentence="Please, point at the bag you want.", input_keys=[], emotion=0, block=True), transitions={'done': 'list'}, autonomy={'done': Autonomy.Off}) # x:28 y:193 OperatableStateMachine.add('list', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'calc', 'none_found': 'list' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'personName', 'entity_list': 'entity_list', 'number': 'number' }) # x:230 y:363 OperatableStateMachine.add( 'get bag position from pointing', GetPointedPositionOnPlane(planeHeight=0.2), transitions={ 'done': 'designate bag', 'not_pointing': 'say 1', 'pointing_up': 'say 1', 'failed': 'list' }, autonomy={ 'done': Autonomy.Off, 'not_pointing': Autonomy.Off, 'pointing_up': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'entity': 'entity', 'position': 'position' }) # x:34 y:362 OperatableStateMachine.add( 'calc', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'get bag position from pointing'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'entity' }) # x:241 y:554 OperatableStateMachine.add('designate bag', _sm_designate_bag_4, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'position': 'position'}) # x:30 y:365, x:182 y:454 _sm_recevoir_sac_7 = OperatableStateMachine( outcomes=['failed', 'done'], input_keys=['Closed_Gripper_Width', 'Open_Gripper_Width']) with _sm_recevoir_sac_7: # x:66 y:44 OperatableStateMachine.add('look at bag', _sm_look_at_bag_6, transitions={'finished': 'close'}, autonomy={'finished': Autonomy.Inherit}) # x:41 y:267 OperatableStateMachine.add('receive it', _sm_receive_it_5, transitions={ 'finished': 'sayCAr', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'Closed_Gripper_Width': 'Closed_Gripper_Width', 'Open_Gripper_Width': 'Open_Gripper_Width' }) # x:81 y:157 OperatableStateMachine.add( 'close', SetGripperState(width=0, effort=0), transitions={ 'object': 'receive it', 'no_object': 'receive it' }, autonomy={ 'object': Autonomy.Off, 'no_object': Autonomy.Off }, remapping={'object_size': 'object_size'}) # x:139 y:334 OperatableStateMachine.add( 'sayCAr', SaraSay(sentence="PLease tell me when we get to the car.", input_keys=[], emotion=0, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:30 y:365 _sm_listen_8 = OperatableStateMachine(outcomes=['done']) with _sm_listen_8: # x:50 y:76 OperatableStateMachine.add( 'SayFollow', SaraSay( sentence= "I will follow you to the car now. Tell me when we get to the car.", input_keys=[], emotion=0, block=False), transitions={'done': 'LIsten'}, autonomy={'done': Autonomy.Off}) # x:161 y:148 OperatableStateMachine.add('LIsten', GetSpeech(watchdog=10), transitions={ 'done': 'Listen2', 'nothing': 'LIsten', 'fail': 'LIsten' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'words'}) # x:201 y:256 OperatableStateMachine.add( 'Listen2', RegexTester(regex=".*((car)|(here it is)|(now)).*"), transitions={ 'true': 'done', 'false': 'Listen2' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'text': 'words', 'result': 'result' }) # x:520 y:327 _sm_follow_9 = OperatableStateMachine(outcomes=['failed'], input_keys=['ID']) with _sm_follow_9: # x:128 y:127 OperatableStateMachine.add( 'Action_follow', self.use_behavior(Action_followSM, 'Follow and listen/follow/Action_follow'), transitions={'failed': 'set name'}, autonomy={'failed': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:288 y:346 OperatableStateMachine.add('list people', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'get first ID', 'none_found': 'failed' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'name', 'entity_list': 'entity_list', 'number': 'number' }) # x:109 y:334 OperatableStateMachine.add( 'get first ID', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'Action_follow'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'ID' }) # x:349 y:116 OperatableStateMachine.add('set name', SetKey(Value="person"), transitions={'done': 'list people'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'name'}) # x:30 y:365, x:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365 _sm_recevoir_sac_10 = ConcurrencyContainer( outcomes=['failed', 'done'], input_keys=['Closed_Gripper_Width', 'Open_Gripper_Width'], conditions=[('failed', [('Recevoir sac', 'failed')]), ('done', [('Recevoir sac', 'done')]), ('done', [('LookAtClosest', 'failed')])]) with _sm_recevoir_sac_10: # x:30 y:40 OperatableStateMachine.add('Recevoir sac', _sm_recevoir_sac_7, transitions={ 'failed': 'failed', 'done': 'done' }, autonomy={ 'failed': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'Closed_Gripper_Width': 'Closed_Gripper_Width', 'Open_Gripper_Width': 'Open_Gripper_Width' }) # x:234 y:114 OperatableStateMachine.add('LookAtClosest', self.use_behavior( LookAtClosestSM, 'recevoir sac/LookAtClosest'), transitions={'failed': 'done'}, autonomy={'failed': Autonomy.Inherit}) # x:390 y:296, x:111 y:269, x:230 y:365, x:542 y:339, x:465 y:331, x:530 y:380 _sm_follow_and_listen_11 = ConcurrencyContainer( outcomes=['done', 'failed'], input_keys=['ID'], conditions=[('done', [('Listen', 'done')]), ('failed', [('follow', 'failed')]), ('done', [('continue', 'true')]), ('done', [('continue', 'false')])]) with _sm_follow_and_listen_11: # x:132 y:90 OperatableStateMachine.add('follow', _sm_follow_9, transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:356 y:105 OperatableStateMachine.add('Listen', _sm_listen_8, transitions={'done': 'done'}, autonomy={'done': Autonomy.Inherit}) # x:485 y:162 OperatableStateMachine.add('continue', ContinueButton(), transitions={ 'true': 'done', 'false': 'done' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }) # x:76 y:535 _sm_getidope_12 = OperatableStateMachine(outcomes=['done'], output_keys=['ID']) with _sm_getidope_12: # x:55 y:63 OperatableStateMachine.add('Person', SetKey(Value="person"), transitions={'done': 'GEtId'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'name'}) # x:43 y:180 OperatableStateMachine.add('GEtId', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'ID', 'none_found': 'GEtId' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'name', 'entity_list': 'Entities_list', 'number': 'number' }) # x:52 y:290 OperatableStateMachine.add( 'ID', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'setID'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Entities_list', 'output_value': 'ID' }) # x:37 y:399 OperatableStateMachine.add( 'setID', SetRosParam(ParamName="behavior/Operator/Id"), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'ID'}) with _state_machine: # x:38 y:33 OperatableStateMachine.add('continue', ContinueButton(), transitions={ 'true': 'GEtPose', 'false': 'GEtPose' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }) # x:35 y:351 OperatableStateMachine.add( 'ImHere', SaraSay(sentence="I am ready to carry your luggage!", input_keys=[], emotion=1, block=True), transitions={'done': 'recevoir sac'}, autonomy={'done': Autonomy.Off}) # x:28 y:539 OperatableStateMachine.add( 'GetIDOpe', _sm_getidope_12, transitions={'done': 'Follow and listen'}, autonomy={'done': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:220 y:536 OperatableStateMachine.add('Follow and listen', _sm_follow_and_listen_11, transitions={ 'done': 'Action_Give_Back_Bag', 'failed': 'failed' }, autonomy={ 'done': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'ID': 'ID'}) # x:411 y:525 OperatableStateMachine.add('Action_Give_Back_Bag', self.use_behavior( Action_Give_Back_BagSM, 'Action_Give_Back_Bag'), transitions={ 'finished': 'GoBackHome', 'failed': 'Action_Give_Back_Bag' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:16 y:206 OperatableStateMachine.add('Init_Sequence', self.use_behavior( Init_SequenceSM, 'Init_Sequence'), transitions={ 'finished': 'move head up', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:634 y:522 OperatableStateMachine.add( 'GoBackHome', SaraSay(sentence="I will go back home now. Have a good day!", input_keys=[], emotion=0, block=True), transitions={'done': 'setarm'}, autonomy={'done': Autonomy.Off}) # x:922 y:519 OperatableStateMachine.add('Action_Move', self.use_behavior( Action_MoveSM, 'Action_Move'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'Origin'}) # x:35 y:281 OperatableStateMachine.add('move head up', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'ImHere'}, autonomy={'done': Autonomy.Off}) # x:35 y:427 OperatableStateMachine.add('recevoir sac', _sm_recevoir_sac_10, transitions={ 'failed': 'failed', 'done': 'GetIDOpe' }, autonomy={ 'failed': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'Closed_Gripper_Width': 'Closed_Gripper_Width', 'Open_Gripper_Width': 'Open_Gripper_Width' }) # x:770 y:526 OperatableStateMachine.add('setarm', RunTrajectory(file="sac_transport", duration=0), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:24 y:134 OperatableStateMachine.add('GEtPose', Get_Robot_Pose(), transitions={'done': 'Init_Sequence'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'Origin'}) return _state_machine
def create(self): # x:30 y:365 _state_machine = OperatableStateMachine(outcomes=['failed']) _state_machine.userdata.ObjectName = "person" _state_machine.userdata.ID = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365, x:130 y:365 _sm_keeplookingforatime_0 = ConcurrencyContainer( outcomes=['failed'], input_keys=['ID'], conditions=[('failed', [('keep looking', 'failed'), ('wait 3', 'done')])]) with _sm_keeplookingforatime_0: # x:210 y:103 OperatableStateMachine.add('wait 3', WaitState(wait_time=3), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:37 y:128 OperatableStateMachine.add('keep looking', KeepLookingAt(), transitions={'failed': 'failed'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) with _state_machine: # x:49 y:113 OperatableStateMachine.add('list', list_entities_by_name( frontality_level=0.8, distance_max=10), transitions={ 'found': 'getclosest', 'none_found': 'list' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'ObjectName', 'entity_list': 'entity_list', 'number': 'number' }) # x:296 y:131 OperatableStateMachine.add( 'getclosest', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'KeepLookingForATime'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'ID' }) # x:220 y:258 OperatableStateMachine.add('KeepLookingForATime', _sm_keeplookingforatime_0, transitions={'failed': 'list'}, autonomy={'failed': Autonomy.Inherit}, remapping={'ID': 'ID'}) 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): 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: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): # x:1474 y:331, x:56 y:575 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.half_turn = 3.1416 _state_machine.userdata.person = "person" _state_machine.userdata.operator_param = "behavior/Operaror/Id" _state_machine.userdata.join = ["Move", "spr/initialpose"] _state_machine.userdata.leave = ["Move", "door2/exit"] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:1182 y:163 _sm_rotate_0 = OperatableStateMachine(outcomes=['finished']) with _sm_rotate_0: # x:103 y:61 OperatableStateMachine.add('Look Left', SaraSetHeadAngle(pitch=-0.35, yaw=0.5), transitions={'done': 'Rotate Left'}, autonomy={'done': Autonomy.Off}) # x:794 y:54 OperatableStateMachine.add('Look Right', SaraSetHeadAngle(pitch=-0.35, yaw=-0.5), transitions={'done': 'Rotate Right'}, autonomy={'done': Autonomy.Off}) # x:325 y:61 OperatableStateMachine.add('Rotate Left', WaitState(wait_time=8), transitions={'done': 'Look Center'}, autonomy={'done': Autonomy.Off}) # x:961 y:65 OperatableStateMachine.add('Rotate Right', WaitState(wait_time=4), transitions={'done': 'Look Center 2'}, autonomy={'done': Autonomy.Off}) # x:1115 y:62 OperatableStateMachine.add('Look Center 2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:484 y:54 OperatableStateMachine.add('Look Center', SaraSetHeadAngle(pitch=-0.35, yaw=0), transitions={'done': 'Rotate Center'}, autonomy={'done': Autonomy.Off}) # x:657 y:49 OperatableStateMachine.add('Rotate Center', WaitState(wait_time=4), transitions={'done': 'Look Right'}, autonomy={'done': Autonomy.Off}) # x:30 y:458 _sm_follow_head_1 = OperatableStateMachine(outcomes=['end'], input_keys=['person']) with _sm_follow_head_1: # x:214 y:48 OperatableStateMachine.add('list all entities', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'Get Nearest', 'none_found': 'list all entities' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'person', 'entity_list': 'entity_list', 'number': 'number' }) # x:456 y:51 OperatableStateMachine.add( 'Get Nearest', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'look'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'ID' }) # x:291 y:177 OperatableStateMachine.add( 'look', KeepLookingAt(), transitions={'failed': 'list all entities'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:12 y:125, x:1130 y:515 _sm_nlu_2 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_nlu_2: # x:156 y:37 OperatableStateMachine.add( 'say ask', SaraSay(sentence="You can ask me your questions.", input_keys=[], emotion=1, block=True), transitions={'done': 'Loop Questions'}, autonomy={'done': Autonomy.Off}) # x:1091 y:84 OperatableStateMachine.add('Listen', GetSpeech(watchdog=10), transitions={ 'done': 'Engine', 'nothing': 'Listen', 'fail': 'Listen' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'sentence'}) # x:1324 y:110 OperatableStateMachine.add('Engine', SaraNLUspr(), transitions={ 'understood': 'Say_Answer', 'not_understood': 'Listen', 'fail': 'Listen' }, autonomy={ 'understood': Autonomy.Off, 'not_understood': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'sentence': 'sentence', 'answer': 'answer' }) # x:632 y:77 OperatableStateMachine.add( 'Select Story', CalculationState(calculation=lambda x: x + 6), transitions={'done': 'Set_a_step'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'index', 'output_value': 'story' }) # x:874 y:78 OperatableStateMachine.add('Set_a_step', Set_a_step(step=1), transitions={'done': 'Listen'}, autonomy={'done': Autonomy.Off}) # x:398 y:81 OperatableStateMachine.add('Loop Questions', ForLoop(repeat=5), transitions={ 'do': 'Select Story', 'end': 'loop step' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:439 y:252 OperatableStateMachine.add( 'Say Blind Game', SaraSay(sentence="Let's play the blind game !", input_keys=[], emotion=1, block=True), transitions={'done': 'Loop Questions'}, autonomy={'done': Autonomy.Off}) # x:189 y:193 OperatableStateMachine.add('loop step', ForLoop(repeat=1), transitions={ 'do': 'Say Blind Game', 'end': 'finished' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:721 y:205 OperatableStateMachine.add('Say_Answer', SaraSay(sentence=lambda x: str(x), input_keys=[], emotion=0, block=True), transitions={'done': 'Loop Questions'}, autonomy={'done': Autonomy.Off}) # x:817 y:123, x:130 y:458 _sm_analyse_crowd_3 = OperatableStateMachine( outcomes=['finished', 'error']) with _sm_analyse_crowd_3: # x:87 y:97 OperatableStateMachine.add('clear database', WonderlandClearPeoples(), transitions={ 'done': 'Rotate', 'error': 'error' }, autonomy={ 'done': Autonomy.Off, 'error': Autonomy.Off }) # x:531 y:112 OperatableStateMachine.add('Add Update Persons', WonderlandAddUpdatePeople(), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:311 y:97 OperatableStateMachine.add( 'Rotate', _sm_rotate_0, transitions={'finished': 'Add Update Persons'}, autonomy={'finished': Autonomy.Inherit}) # x:1203 y:11, x:1006 y:366 _sm_init_scenario_4 = OperatableStateMachine( outcomes=['done', 'error']) with _sm_init_scenario_4: # x:30 y:42 OperatableStateMachine.add( 'Generate Vizbox Story', Set_Story(titre="Speech and Person Recognition", storyline=[ "Waiting Begining", "Join Game Room", "Waiting Crowd Placement", "Analysing Crowd", "Begin Game", "Find Operator", "Question 1", "Question 2", "Question 3", "Question 4", "Question 5", "Leave Arena" ]), transitions={'done': 'Set Story Step'}, autonomy={'done': Autonomy.Off}) # x:559 y:44 OperatableStateMachine.add('WaitForBegining', ContinueButton(), transitions={ 'true': 'Reset Persons', 'false': 'Reset Persons' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }) # x:807 y:61 OperatableStateMachine.add('Reset Persons', WonderlandClearPeoples(), transitions={ 'done': 'done', 'error': 'error' }, autonomy={ 'done': Autonomy.Off, 'error': Autonomy.Off }) # x:247 y:49 OperatableStateMachine.add('Set Story Step', Set_a_step(step=0), transitions={'done': 'setIDLE'}, autonomy={'done': Autonomy.Off}) # x:388 y:208 OperatableStateMachine.add('Reset Arm', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'Reset Persons', 'failed': 'error' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'target'}) # x:427 y:40 OperatableStateMachine.add('setIDLE', SetKey(Value="IdlePose"), transitions={'done': 'Reset Arm'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:30 y:458, x:230 y:458 _sm_join_area_5 = OperatableStateMachine( outcomes=['failed', 'finished'], input_keys=['join']) with _sm_join_area_5: # x:95 y:40 OperatableStateMachine.add( 'Say Join Area', SaraSay(sentence="I will join the playing room !", input_keys=[], emotion=1, block=True), transitions={'done': 'Join Arena'}, autonomy={'done': Autonomy.Off}) # x:92 y:134 OperatableStateMachine.add( 'Join Arena', self.use_behavior(sara_flexbe_behaviors__ActionWrapper_MoveSM, 'Join Area/Join Arena'), transitions={ 'finished': 'finished', 'failed': 'failed', 'critical_fail': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit }, remapping={'Action': 'join'}) # x:489 y:56, x:604 y:278 _sm_waiting_and_turn_6 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['half_turn']) with _sm_waiting_and_turn_6: # x:50 y:51 OperatableStateMachine.add( 'Want Play', SaraSay(sentence="Hum, I want to play riddles !", input_keys=[], emotion=1, block=True), transitions={'done': 'Wait 10s'}, autonomy={'done': Autonomy.Off}) # x:272 y:121 OperatableStateMachine.add( 'action_turn', self.use_behavior(sara_flexbe_behaviors__action_turnSM, 'Waiting And Turn/action_turn'), transitions={ 'finished': 'finished', 'failed': 'Cant turn' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'half_turn'}) # x:437 y:240 OperatableStateMachine.add('Cant turn', SaraSay(sentence="I can't turn !", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:63 y:178 OperatableStateMachine.add( 'Wait 10s', WaitState(wait_time=10), transitions={'done': 'Look In Front Of'}, autonomy={'done': Autonomy.Off}) # x:61 y:260 OperatableStateMachine.add('Look In Front Of', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}) # x:472 y:69, x:476 y:113, x:470 y:196, x:330 y:458, x:430 y:458 _sm_questions_7 = ConcurrencyContainer( outcomes=['finished', 'failed'], input_keys=['person'], conditions=[('finished', [('NLU', 'finished')]), ('failed', [('NLU', 'failed')]), ('finished', [('Follow Head', 'end')])]) with _sm_questions_7: # x:85 y:58 OperatableStateMachine.add('NLU', _sm_nlu_2, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:84 y:185 OperatableStateMachine.add('Follow Head', _sm_follow_head_1, transitions={'end': 'finished'}, autonomy={'end': Autonomy.Inherit}, remapping={'person': 'person'}) # x:283 y:294, x:60 y:571 _sm_find_operator_8 = OperatableStateMachine( outcomes=['not_found', 'done'], input_keys=['person', 'operator_param']) with _sm_find_operator_8: # x:51 y:40 OperatableStateMachine.add( 'Ask Player', SaraSay(sentence="Who wan't to play with me ?", input_keys=[], emotion=1, block=True), transitions={'done': 'Wait Operator'}, autonomy={'done': Autonomy.Off}) # x:39 y:297 OperatableStateMachine.add( 'Get operator id', CalculationState(calculation=lambda x: x.entities[0].face.id), transitions={'done': 'Set Operator Id'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'operator' }) # x:33 y:384 OperatableStateMachine.add('Set Operator Id', SetRosParamKey(), transitions={'done': 'Operator Id'}, autonomy={'done': Autonomy.Off}, remapping={ 'Value': 'operator_param', 'ParamName': 'operator' }) # x:46 y:117 OperatableStateMachine.add('Wait Operator', WaitState(wait_time=6), transitions={'done': 'Find Operator'}, autonomy={'done': Autonomy.Off}) # x:35 y:495 OperatableStateMachine.add('Operator Id', LogKeyState( text="Operator find. Id: {}", severity=Logger.REPORT_HINT), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'operator'}) # x:30 y:205 OperatableStateMachine.add('Find Operator', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'Get operator id', 'none_found': 'Find Operator' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'person', 'entity_list': 'entity_list', 'number': 'number' }) # x:703 y:198, x:88 y:199 _sm_tell_basic_stats_9 = OperatableStateMachine( outcomes=['finished', 'failed']) with _sm_tell_basic_stats_9: # x:50 y:40 OperatableStateMachine.add('wait', WaitState(wait_time=0), transitions={'done': 'GetPeopleStats'}, autonomy={'done': Autonomy.Off}) # x:218 y:43 OperatableStateMachine.add('GetPeopleStats', WonderlandGetPersonStat(), transitions={ 'done': 'GenerateSentence', 'none': 'Nobody', 'error': 'failed' }, autonomy={ 'done': Autonomy.Off, 'none': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'women': 'women', 'men': 'men', 'others': 'others' }) # x:466 y:46 OperatableStateMachine.add('Nobody', SaraSay( sentence="There is nobody here !", input_keys=[], emotion=1, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:162 y:243 OperatableStateMachine.add( 'GenerateSentence', FlexibleCalculationState(calculation=lambda x: "There is " + str(x[0] + x[1] + x[2]) + " persons.", input_keys=['men', 'women', 'others']), transitions={'done': 'Tell_Stats'}, autonomy={'done': Autonomy.Off}, remapping={ 'men': 'men', 'women': 'women', 'others': 'others', 'output_value': 'sentence' }) # x:151 y:345 OperatableStateMachine.add( 'Generate Sentence 2', FlexibleCalculationState( calculation=lambda x: "I recognize " + str(x[ 1]) + " women and " + str(x[0]) + " men.", input_keys=['men', 'women', 'others']), transitions={'done': 'Tell_Stats 2'}, autonomy={'done': Autonomy.Off}, remapping={ 'men': 'men', 'women': 'women', 'others': 'others', 'output_value': 'sentence' }) # x:380 y:172 OperatableStateMachine.add( 'Tell_Stats', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'Generate Sentence 2'}, autonomy={'done': Autonomy.Off}) # x:409 y:276 OperatableStateMachine.add('Tell_Stats 2', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:30 y:40 OperatableStateMachine.add('continue', ContinueButton(), transitions={ 'true': 'Init Scenario', 'false': 'Init Scenario' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }) # x:715 y:127 OperatableStateMachine.add('Tell basic stats', _sm_tell_basic_stats_9, transitions={ 'finished': 'Set Find Operator', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:899 y:126 OperatableStateMachine.add('Find Operator', _sm_find_operator_8, transitions={ 'not_found': 'Find Operator', 'done': 'Questions' }, autonomy={ 'not_found': Autonomy.Inherit, 'done': Autonomy.Inherit }, remapping={ 'person': 'person', 'operator_param': 'operator_param' }) # x:1120 y:126 OperatableStateMachine.add('Questions', _sm_questions_7, transitions={ 'finished': 'set head', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'person': 'person'}) # x:343 y:49 OperatableStateMachine.add( 'Set Story Waiting', Set_a_step(step=2), transitions={'done': 'Waiting And Turn'}, autonomy={'done': Autonomy.Off}) # x:532 y:48 OperatableStateMachine.add('Set Analyse', Set_a_step(step=3), transitions={'done': 'Analyse Crowd'}, autonomy={'done': Autonomy.Off}) # x:696 y:47 OperatableStateMachine.add( 'Set Begin Game', Set_a_step(step=4), transitions={'done': 'Tell basic stats'}, autonomy={'done': Autonomy.Off}) # x:899 y:49 OperatableStateMachine.add('Set Find Operator', Set_a_step(step=5), transitions={'done': 'Find Operator'}, autonomy={'done': Autonomy.Off}) # x:332 y:127 OperatableStateMachine.add('Waiting And Turn', _sm_waiting_and_turn_6, transitions={ 'finished': 'Set Analyse', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'half_turn': 'half_turn'}) # x:1333 y:232 OperatableStateMachine.add('Set Go Out', Set_a_step(step=11), transitions={'done': 'Say And Of Game'}, autonomy={'done': Autonomy.Off}) # x:1517 y:148 OperatableStateMachine.add( 'Leave Arena', self.use_behavior(sara_flexbe_behaviors__ActionWrapper_MoveSM, 'Leave Arena'), transitions={ 'finished': 'finished', 'failed': 'finished', 'critical_fail': 'finished' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit }, remapping={'Action': 'leave'}) # x:176 y:42 OperatableStateMachine.add( 'Set Join', Set_a_step(step=1), transitions={'done': 'Set Story Waiting'}, autonomy={'done': Autonomy.Off}) # x:177 y:120 OperatableStateMachine.add('Join Area', _sm_join_area_5, transitions={ 'failed': 'failed', 'finished': 'Set Story Waiting' }, autonomy={ 'failed': Autonomy.Inherit, 'finished': Autonomy.Inherit }, remapping={'join': 'join'}) # x:1302 y:140 OperatableStateMachine.add( 'Say And Of Game', SaraSay( sentence= "The game is finished. I will leave the arena. Thank you for playing with me.", input_keys=[], emotion=1, block=True), transitions={'done': 'Leave Arena'}, autonomy={'done': Autonomy.Off}) # x:1120 y:237 OperatableStateMachine.add('set head', SaraSetHeadAngle(pitch=-0.2, yaw=0), transitions={'done': 'Set Go Out'}, autonomy={'done': Autonomy.Off}) # x:34 y:130 OperatableStateMachine.add('Init Scenario', _sm_init_scenario_4, transitions={ 'done': 'Set Join', 'error': 'failed' }, autonomy={ 'done': Autonomy.Inherit, 'error': Autonomy.Inherit }) # x:517 y:124 OperatableStateMachine.add('Analyse Crowd', _sm_analyse_crowd_3, transitions={ 'finished': 'Set Begin Game', 'error': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'error': Autonomy.Inherit }) return _state_machine
def create(self): session_id = None wait_for_question = 25 wait_for_utter = 15 intents = ['YesNo', 'Complete'] answer_key = 'confirm' detail_levels = 'low' # x:73 y:608, x:539 y:365, x:575 y:287 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'max_retry'], output_keys=['answer']) _state_machine.userdata.question = 'Please, is the gripper complete?' _state_machine.userdata.retry = 2 _state_machine.userdata.answer = '' # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:373, x:385 y:382 _sm_continueretry_0 = OperatableStateMachine(outcomes=['true', 'false'], input_keys=['retry'], output_keys=['retry']) with _sm_continueretry_0: # x:158 y:82 OperatableStateMachine.add('decrease_retry', CalculationState(calculation=lambda x: x-1), transitions={'done': 'continue_asking'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'retry', 'output_value': 'retry'}) # x:156 y:221 OperatableStateMachine.add('continue_asking', CheckConditionState(predicate=lambda x: x > 0), transitions={'true': 'true', 'false': 'false'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'retry'}) with _state_machine: # x:416 y:26 OperatableStateMachine.add('AskForComplete', LisaUtterAndWaitForIntentState(context_id=session_id, intents=intents, wait_time=wait_for_question), transitions={'intent_recognized': 'Recognized', 'intent_not_recognized': 'NotRecognized', 'preempt': 'failed', 'timeouted': 'failed', 'error': 'failed'}, autonomy={'intent_recognized': Autonomy.Off, 'intent_not_recognized': Autonomy.Off, 'preempt': Autonomy.Off, 'timeouted': Autonomy.Off, 'error': Autonomy.Off}, remapping={'text_to_utter': 'question', 'payload': 'payload', 'original_sentence': 'original_sentence', 'error_reason': 'error_reason', 'intent_recognized': 'intent_recognized'}) # x:690 y:190 OperatableStateMachine.add('ContinueRetry', _sm_continueretry_0, transitions={'true': 'log_retry_value', 'false': 'max_retry'}, autonomy={'true': Autonomy.Inherit, 'false': Autonomy.Inherit}, remapping={'retry': 'retry'}) # x:52 y:390 OperatableStateMachine.add('GetAnswerKey', LisaGetPayloadKeyState(payload_key=answer_key), transitions={'done': 'log_output', 'error': 'failed'}, autonomy={'done': Autonomy.Off, 'error': Autonomy.Off}, remapping={'payload': 'payload', 'payload_value': 'answer'}) # x:783 y:27 OperatableStateMachine.add('NotRecognized', LisaRecognitionResultToStringState(context_id=session_id, detail_levels=detail_levels), transitions={'done': 'UtterNotRecognizedText'}, autonomy={'done': Autonomy.Off}, remapping={'payload': 'payload', 'original_sentence': 'original_sentence', 'error_reason': 'error_reason', 'intent_recognized': 'intent_recognized', 'text_to_utter': 'text_to_utter'}) # x:42 y:122 OperatableStateMachine.add('Recognized', LisaRecognitionResultToStringState(context_id=session_id, detail_levels=detail_levels), transitions={'done': 'GetAnswerKey'}, autonomy={'done': Autonomy.Off}, remapping={'payload': 'payload', 'original_sentence': 'original_sentence', 'error_reason': 'error_reason', 'intent_recognized': 'intent_recognized', 'text_to_utter': 'text_to_utter'}) # x:878 y:289 OperatableStateMachine.add('UtterNotRecognizedText', LisaUtterState(context_id=session_id, wait_time=wait_for_utter, suspend_time=1), transitions={'done': 'ContinueRetry', 'preempt': 'failed', 'timeouted': 'failed', 'error': 'failed'}, autonomy={'done': Autonomy.Off, 'preempt': Autonomy.Off, 'timeouted': Autonomy.Off, 'error': Autonomy.Off}, remapping={'text_to_utter': 'text_to_utter', 'error_reason': 'error_reason'}) # x:278 y:216 OperatableStateMachine.add('UtterRecognized', LisaUtterState(context_id=session_id, wait_time=wait_for_utter, suspend_time=1), transitions={'done': 'GetAnswerKey', 'preempt': 'failed', 'timeouted': 'failed', 'error': 'failed'}, autonomy={'done': Autonomy.Off, 'preempt': Autonomy.Off, 'timeouted': Autonomy.Off, 'error': Autonomy.Off}, remapping={'text_to_utter': 'text_to_utter', 'error_reason': 'error_reason'}) # x:74 y:506 OperatableStateMachine.add('log_output', LogKeyState(text="GC: exit with answer: {}", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'answer'}) # x:646 y:73 OperatableStateMachine.add('log_retry_value', LogKeyState(text="GC: retry level is {}", severity=Logger.REPORT_HINT), transitions={'done': 'AskForComplete'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'retry'}) return _state_machine