def create(self): # x:609 y:365, x:602 y:89 _state_machine = OperatableStateMachine(outcomes=['ok', 'too_far'], input_keys=['pose']) _state_machine.userdata.pose = 0 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:42 y:56 OperatableStateMachine.add('gen', GenGripperPose(l=0, z=0, planar=false), transitions={'done': 'kinematic test', 'fail': 'too_far'}, autonomy={'done': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'pose_in': 'pose', 'pose_out': 'pose_out'}) # x:195 y:347 OperatableStateMachine.add('third check', CheckConditionState(predicate=lambda x: (x.position.x**2+x.position.y**2+(x.position.z-1))**0.5 < 1.5), transitions={'true': 'kinematic test', 'false': 'too_far'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'pose_out'}) # x:190 y:147 OperatableStateMachine.add('first check', CheckConditionState(predicate=lambda x: x.position.x<0.8), transitions={'true': 'second check', 'false': 'too_far'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'pose_out'}) # x:196 y:253 OperatableStateMachine.add('second check', CheckConditionState(predicate=lambda x: x.position.z>0.5), transitions={'true': 'third check', 'false': 'too_far'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'pose_out'}) # x:99 y:520 OperatableStateMachine.add('kinematic test', MoveitMove(move=False, waitForExecution=True, group="RightArm"), transitions={'done': 'ok', 'failed': 'too_far'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'target': 'pose_out'}) return _state_machine
def create(self): # x:30 y:365, x:150 y:452 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:283 y:40 OperatableStateMachine.add('say', Talk_Last_Response(target_time=1), transitions={'continue': 'sub', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:457 y:297 OperatableStateMachine.add('logfalse', LogState(text="false", severity=Logger.REPORT_HINT), transitions={'done': 'sub'}, autonomy={'done': Autonomy.Off}) # x:391 y:168 OperatableStateMachine.add('unavailable_topic_string_log', LogState(text="the topic is not available so finishing", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:973 y:355 OperatableStateMachine.add('takephoto', TakePhotoState(), transitions={'continue': 'move_lips', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:975 y:444 OperatableStateMachine.add('move_lips', MoveRobotLipsState(target_time=0), transitions={'continue': 'finished', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:776 y:71 OperatableStateMachine.add('sub', SubscriberState(topic='/stt_topic', blocking=True, clear=False), transitions={'received': 'chyeck', 'unavailable': 'unavailable_topic_string_log'}, autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off}, remapping={'message': 'message'}) # x:778 y:303 OperatableStateMachine.add('chyeck', CheckConditionState(predicate=lambda message: message.query == "take photo"), transitions={'true': 'takephoto', 'false': 'logfalse'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'message'}) return _state_machine
def create(self): # 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:214 y:238, x:1435 y:649, x:153 y:589, x:383 y:502, x:1263 y:790 _state_machine = OperatableStateMachine(outcomes=[ 'no_detection', 'water_depleted', 'mission_part_failure', 'preempted', 'target_lost' ]) _state_machine.userdata.flying_altitude = 5 _state_machine.userdata.orbit_goal = lidar_flier.msg.lfGoal() _state_machine.userdata.fire_position = fire_detect.msg.firemanGoal() _state_machine.userdata.fire_extinguish_position = lidar_flier.msg.lfGoal( ) _state_machine.userdata.is_preempted = False _state_machine.userdata.is_water_depleted = False _state_machine.userdata.is_target_lost = False _state_machine.userdata.lidar_flier_speed_goto = 0.5 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] param_prefix_namespace = "flexbe_behavior_launcher/" flying_altitude_list = rospy.get_param(param_prefix_namespace + 'main/flying_altitudes') uav_name_list = rospy.get_param(param_prefix_namespace + 'main/robot_name_list') robot_name = rospy.get_param(param_prefix_namespace + 'robot_name') lidar_flier_speed_goto = rospy.get_param( param_prefix_namespace + 'lidar_flier/lidar_flier_speed_goto') lidar_flier_speed_orbit = rospy.get_param( param_prefix_namespace + 'lidar_flier/lidar_flier_speed_orbit') lidar_flier_clockwise = rospy.get_param( param_prefix_namespace + 'lidar_flier/lidar_flier_clockwise') lidar_flier_stick_distance = rospy.get_param( param_prefix_namespace + 'lidar_flier/lidar_flier_stick_distance') fire_detection_topic = rospy.get_param(param_prefix_namespace + 'main/fire_detection_topic') fire_detection_topic = "/" + robot_name + "/" + fire_detection_topic Logger.loginfo("fire_detection_topic: %s" % fire_detection_topic) lidar_flier_action_server_name = rospy.get_param( param_prefix_namespace + 'main/lidar_flier_action_server_name') lidar_flier_action_server_name = "/" + robot_name + "/" + lidar_flier_action_server_name Logger.loginfo("lidar_flier_action_server_name: %s" % lidar_flier_action_server_name) fire_extinguish_action_server_name = rospy.get_param( param_prefix_namespace + 'main/fire_extinguish_action_server_name') fire_extinguish_action_server_name = "/" + robot_name + "/" + fire_extinguish_action_server_name Logger.loginfo("fire_extinguish_action_server_name: %s" % fire_extinguish_action_server_name) control_manager_diagnostics_topic = rospy.get_param( param_prefix_namespace + 'main/control_manager_diagnostics_topic') control_manager_diagnostics_topic = "/" + robot_name + "/" + control_manager_diagnostics_topic Logger.loginfo("control_manager_diagnostics_topic: %s" % control_manager_diagnostics_topic) control_manager_cmd_topic = rospy.get_param( param_prefix_namespace + 'main/control_manager_cmd_topic') control_manager_cmd_topic = "/" + robot_name + "/" + control_manager_cmd_topic Logger.loginfo("control_manager_cmd_topic: %s" % control_manager_cmd_topic) constraint_switch_service_topic = rospy.get_param( param_prefix_namespace + 'main/constraint_switch_service_topic') constraint_switch_service_topic = "/" + robot_name + "/" + constraint_switch_service_topic Logger.loginfo("constraint_switch_service_topic: %s" % constraint_switch_service_topic) control_manager_switch_controller_service_topic = rospy.get_param( param_prefix_namespace + 'main/control_manager_switch_controller_service_topic') control_manager_switch_controller_service_topic = "/" + robot_name + "/" + control_manager_switch_controller_service_topic Logger.loginfo("control_manager_switch_controller_service_topic: %s" % control_manager_switch_controller_service_topic) outside_flying_controller = rospy.get_param( param_prefix_namespace + 'main/outside_flying_controller') outside_flying_controller = outside_flying_controller Logger.loginfo("outside_flying_controller: %s" % outside_flying_controller) extinguishing_controller = rospy.get_param( param_prefix_namespace + 'main/extinguishing_controller') extinguishing_controller = extinguishing_controller Logger.loginfo("extinguishing_controller: %s" % extinguishing_controller) outside_flying_constraints = rospy.get_param( param_prefix_namespace + 'main/outside_flying_constraints') outside_flying_constraints = outside_flying_constraints Logger.loginfo("outside_flying_constraints: %s" % outside_flying_constraints) extinguishing_constraints = rospy.get_param( param_prefix_namespace + 'main/extinguishing_constraints') extinguishing_constraints = extinguishing_constraints Logger.loginfo("extinguishing_constraints: %s" % extinguishing_constraints) flying_altitude = 3 for i in range(len(uav_name_list)): if uav_name_list[i] == robot_name: flying_altitude = flying_altitude_list[i] break orbit_goal = lidar_flier.msg.lfGoal() orbit_goal.altitude = flying_altitude orbit_goal.orbit = True orbit_goal.frame_id = str(robot_name + "/gps_origin") orbit_goal.speed = lidar_flier_speed_orbit orbit_goal.stick_distance = lidar_flier_stick_distance orbit_goal.clockwise = lidar_flier_clockwise _state_machine.userdata.flying_altitude = flying_altitude _state_machine.userdata.orbit_goal = orbit_goal _state_machine.userdata.lidar_flier_speed_goto = lidar_flier_speed_goto Logger.loginfo("fire_ch_sm.flying_altitude: %s" % _state_machine.userdata.flying_altitude) Logger.loginfo("fire_ch_sm.orbit_goal: %s" % _state_machine.userdata.orbit_goal) Logger.loginfo("fire_ch_sm.lidar_flier_speed_goto: %s" % _state_machine.userdata.lidar_flier_speed_goto) # [/MANUAL_CREATE] # x:30 y:490, x:394 y:528, x:276 y:624, x:711 y:486, x:30 y:576, x:530 y:490, x:630 y:490, x:894 y:493, x:614 y:408, x:778 y:489, x:591 y:614, x:687 y:612, x:1230 y:490 _sm_lookforfire_0 = ConcurrencyContainer( outcomes=['error', 'preempted', 'no_detection', 'fire_detected'], input_keys=[ 'orbit_goal', 'fire_position', 'lidar_flier_speed_goto', 'fire_extinguish_position' ], output_keys=['fire_position', 'fire_extinguish_position'], conditions=[ ('error', [('LidarFlierOrbit', 'error')]), ('preempted', [('LidarFlierOrbit', 'preempted')]), ('error', [('LidarFlierOrbit', 'server_not_initialized')]), ('error', [('LidarFlierOrbit', 'stop_requested')]), ('error', [('LidarFlierOrbit', 'no_valid_points_in_scan')]), ('no_detection', [('LidarFlierOrbit', 'successed')]), ('fire_detected', [('WaitForDetection', 'successed')]), ('error', [('WaitForDetection', 'failed')]), ('error', [('InfiniteWait', 'done')]) ]) with _sm_lookforfire_0: # x:537 y:175 OperatableStateMachine.add('InfiniteWait', WaitState(wait_time=1000000), transitions={'done': 'error'}, autonomy={'done': Autonomy.Off}) # x:118 y:146 OperatableStateMachine.add( 'LidarFlierOrbit', LidarFlierActionState( action_server_name=lidar_flier_action_server_name), transitions={ 'successed': 'no_detection', 'no_valid_points_in_scan': 'error', 'stop_requested': 'error', 'server_not_initialized': 'error', 'error': 'error', 'preempted': 'preempted' }, autonomy={ 'successed': Autonomy.Off, 'no_valid_points_in_scan': Autonomy.Off, 'stop_requested': Autonomy.Off, 'server_not_initialized': Autonomy.Off, 'error': Autonomy.Off, 'preempted': Autonomy.Off }, remapping={'goal': 'orbit_goal'}) # x:828 y:141 OperatableStateMachine.add( 'WaitForDetection', WaitForMsgState( topic=fire_detection_topic, wait_time=-1, function=self.callback_true, input_keys=['lidar_flier_speed_goto'], output_keys=['fire_extinguish_position', 'fire_position'], output_function=self.fire_detect_output_function), transitions={ 'successed': 'fire_detected', 'failed': 'error' }, autonomy={ 'successed': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'lidar_flier_speed_goto': 'lidar_flier_speed_goto', 'fire_extinguish_position': 'fire_extinguish_position', 'fire_position': 'fire_position' }) with _state_machine: # x:322 y:26 OperatableStateMachine.add('LookForFire', _sm_lookforfire_0, transitions={ 'error': 'mission_part_failure', 'preempted': 'preempted', 'no_detection': 'no_detection', 'fire_detected': 'LidarFlierGotoExtinguishPosition' }, autonomy={ 'error': Autonomy.Inherit, 'preempted': Autonomy.Inherit, 'no_detection': Autonomy.Inherit, 'fire_detected': Autonomy.Inherit }, remapping={ 'orbit_goal': 'orbit_goal', 'fire_position': 'fire_position', 'lidar_flier_speed_goto': 'lidar_flier_speed_goto', 'fire_extinguish_position': 'fire_extinguish_position' }) # x:856 y:209 OperatableStateMachine.add( 'FireExtinguish', FireExtinguishActionState( action_server_name=fire_extinguish_action_server_name), transitions={ 'successed': 'SetExtinguishingAsSuccessful', 'lost_target': 'SetTargetAsLost', 'error': 'SwitchControlletForFlyingOutside', 'preempted': 'SetPreempted' }, autonomy={ 'successed': Autonomy.Off, 'lost_target': Autonomy.Off, 'error': Autonomy.Off, 'preempted': Autonomy.Off }, remapping={ 'goal': 'fire_position', 'is_tank_depleted': 'is_tank_depleted' }) # x:543 y:27 OperatableStateMachine.add( 'LidarFlierGotoExtinguishPosition', LidarFlierActionState( action_server_name=lidar_flier_action_server_name), transitions={ 'successed': 'SwitchControllerForExtinguishing', 'no_valid_points_in_scan': 'mission_part_failure', 'stop_requested': 'mission_part_failure', 'server_not_initialized': 'mission_part_failure', 'error': 'mission_part_failure', 'preempted': 'preempted' }, autonomy={ 'successed': Autonomy.Off, 'no_valid_points_in_scan': Autonomy.Off, 'stop_requested': Autonomy.Off, 'server_not_initialized': Autonomy.Off, 'error': Autonomy.Off, 'preempted': Autonomy.Off }, remapping={'goal': 'fire_extinguish_position'}) # x:848 y:29 OperatableStateMachine.add( 'SwitchControllerForExtinguishing', ServiceStringState( service_topic= control_manager_switch_controller_service_topic, msg_text=extinguishing_controller, state_name="ControlManager"), transitions={ 'finished': 'SwitchConstraintsForExtinguishing', 'failed': 'SwitchControllerForExtinguishing' }, autonomy={ 'finished': Autonomy.Off, 'failed': Autonomy.Off }) # x:838 y:416 OperatableStateMachine.add( 'SwitchControlletForFlyingOutside', ServiceStringState( service_topic= control_manager_switch_controller_service_topic, msg_text=outside_flying_controller, state_name="ControlManager"), transitions={ 'finished': 'SwitchConstraintsForOutsideFlying', 'failed': 'SwitchControlletForFlyingOutside' }, autonomy={ 'finished': Autonomy.Off, 'failed': Autonomy.Off }) # x:837 y:128 OperatableStateMachine.add( 'SwitchConstraintsForExtinguishing', ServiceStringState( service_topic=constraint_switch_service_topic, msg_text=extinguishing_constraints, state_name="ControlManager"), transitions={ 'finished': 'FireExtinguish', 'failed': 'SwitchConstraintsForExtinguishing' }, autonomy={ 'finished': Autonomy.Off, 'failed': Autonomy.Off }) # x:832 y:520 OperatableStateMachine.add( 'SwitchConstraintsForOutsideFlying', ServiceStringState( service_topic=constraint_switch_service_topic, msg_text=outside_flying_constraints, state_name="ControlManager"), transitions={ 'finished': 'WasExtinguishingSuccessful', 'failed': 'SwitchConstraintsForOutsideFlying' }, autonomy={ 'finished': Autonomy.Off, 'failed': Autonomy.Off }) # x:873 y:717 OperatableStateMachine.add( 'WasPreempted', CheckConditionState(predicate=lambda x: x == True), transitions={ 'true': 'preempted', 'false': 'WasTargetLost' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'is_preempted'}) # x:854 y:622 OperatableStateMachine.add( 'WasExtinguishingSuccessful', CheckConditionState(predicate=lambda x: x == True), transitions={ 'true': 'water_depleted', 'false': 'WasPreempted' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'is_water_depleted'}) # x:873 y:802 OperatableStateMachine.add( 'WasTargetLost', CheckConditionState(predicate=lambda x: x == True), transitions={ 'true': 'target_lost', 'false': 'mission_part_failure' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'is_target_lost'}) # x:1253 y:309 OperatableStateMachine.add( 'SetPreempted', SetVariableToTrueState(key=['is_preempted'], function=self.set_preempted_variable), transitions={'done': 'SwitchControlletForFlyingOutside'}, autonomy={'done': Autonomy.Off}, remapping={'is_preempted': 'is_preempted'}) # x:675 y:310 OperatableStateMachine.add( 'SetExtinguishingAsSuccessful', SetVariableToTrueState( key=['is_water_depleted'], function=self.set_water_depleted_variable), transitions={'done': 'SwitchControlletForFlyingOutside'}, autonomy={'done': Autonomy.Off}, remapping={'is_water_depleted': 'is_water_depleted'}) # x:979 y:310 OperatableStateMachine.add( 'SetTargetAsLost', SetVariableToTrueState(key=['is_target_lost'], function=self.set_target_lost_variable), transitions={'done': 'SwitchControlletForFlyingOutside'}, autonomy={'done': Autonomy.Off}, remapping={'is_target_lost': 'is_target_lost'}) 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: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): 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
def create(self): # x:456 y:478, x:347 y:237 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:365, x:487 y:245 _sm_guess_age_interaction_0 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_guess_age_interaction_0: # x:153 y:68 OperatableStateMachine.add('smile', FacialExpressionState(expression_num=1), transitions={'continue': 'wait 1', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:378 y:48 OperatableStateMachine.add('wait 1', WaitState(wait_time=1), transitions={'done': 'blink'}, autonomy={'done': Autonomy.Off}) # x:605 y:69 OperatableStateMachine.add('blink', FacialExpressionState(expression_num=2), transitions={'continue': 'introduction', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:817 y:120 OperatableStateMachine.add('introduction', TalkState(sentence_number=1), transitions={'continue': 'listen for name', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:919 y:202 OperatableStateMachine.add('listen for name', ListeningState(), transitions={'continue': 'welcome to', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1004 y:291 OperatableStateMachine.add('welcome to', TalkState(sentence_number=2), transitions={'continue': 'sub please take my photo', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1027 y:401 OperatableStateMachine.add('sub please take my photo', SubscriberState(topic='/stt_topic', blocking=True, clear=False), transitions={'received': 'cond please take my photo', 'unavailable': 'failed'}, autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off}, remapping={'message': 'message'}) # x:1025 y:515 OperatableStateMachine.add('cond please take my photo', CheckConditionState(predicate=lambda message:message.data =="please take my photo"), transitions={'true': 'cool go ahead', 'false': 'not heard take photo'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'message'}) # x:823 y:458 OperatableStateMachine.add('not heard take photo', LogState(text=not yet heard "please take my photo", severity=Logger.REPORT_HINT), transitions={'done': 'sub please take my photo'}, autonomy={'done': Autonomy.Off}) # x:938 y:641 OperatableStateMachine.add('cool go ahead', TalkState(sentence_number=3), transitions={'continue': 'finished', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:161 y:371, x:268 y:170 _sm_start_face_det_and_trk_1 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_start_face_det_and_trk_1: # x:149 y:36 OperatableStateMachine.add('say face found', TalkState(sentence_number=100), transitions={'continue': 'face det trk', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:519 y:73 OperatableStateMachine.add('face det trk', FaceDetTrack(), transitions={'continue': 'say start face det trk', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:563 y:328 OperatableStateMachine.add('say start face det trk', TalkState(sentence_number=101), transitions={'continue': 'finished', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:128 y:223, x:130 y:365 _sm_stop_video_detects_and_tracks_2 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_stop_video_detects_and_tracks_2: # x:135 y:64 OperatableStateMachine.add('stop obj det trk', StopFaceDetectAndTrack(), transitions={'continue': 'stop face det trk', 'failed': 'stop face det trk'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:410 y:78 OperatableStateMachine.add('stop face det trk', StopFaceDetectAndTrack(), transitions={'continue': 'stop video stream', 'failed': 'stop video stream'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:466 y:250 OperatableStateMachine.add('stop video stream', StopCameraStream(), transitions={'continue': 'finished', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:438 y:355, x:121 y:296 _sm_check_face_nearby_3 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_check_face_nearby_3: # x:237 y:77 OperatableStateMachine.add('subscr face found topic', SubscriberState(topic='/is_person_nearby', blocking=True, clear=False), transitions={'received': 'face condition', 'unavailable': 'failed'}, autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off}, remapping={'message': 'message'}) # x:521 y:63 OperatableStateMachine.add('face condition', CheckConditionState(predicate=lambda message:message.data == "yes"), transitions={'true': 'finished', 'false': 'no face found yet'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'message'}) # x:297 y:218 OperatableStateMachine.add('no face found yet', LogState(text="no face found yet", severity=Logger.REPORT_HINT), transitions={'done': 'subscr face found topic'}, autonomy={'done': Autonomy.Off}) # x:30 y:365, x:132 y:205 _sm_launch_stream_and_object_detector_4 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_launch_stream_and_object_detector_4: # x:126 y:42 OperatableStateMachine.add('launch video stream', LaunchVideoStream(), transitions={'continue': 'say launching video', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:389 y:56 OperatableStateMachine.add('say launching video', TalkState(sentence_number=99), transitions={'continue': 'launch obj det trk', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:483 y:152 OperatableStateMachine.add('launch obj det trk', LaunchObjDetectAndTrack(), transitions={'continue': 'finished', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) with _state_machine: # x:135 y:38 OperatableStateMachine.add('launch stream and object detector', _sm_launch_stream_and_object_detector_4, transitions={'finished': 'check face nearby', 'failed': 'stop video detects and tracks'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:469 y:33 OperatableStateMachine.add('check face nearby', _sm_check_face_nearby_3, transitions={'finished': 'stop obj det trk', 'failed': 'stop video detects and tracks'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:546 y:220 OperatableStateMachine.add('stop video detects and tracks', _sm_stop_video_detects_and_tracks_2, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:701 y:69 OperatableStateMachine.add('stop obj det trk', StopObjectDetectAndTrack(), transitions={'continue': 'start face det and trk', 'failed': 'stop video detects and tracks'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:892 y:135 OperatableStateMachine.add('start face det and trk', _sm_start_face_det_and_trk_1, transitions={'finished': 'guess age interaction', 'failed': 'stop video detects and tracks'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:887 y:356 OperatableStateMachine.add('guess age interaction', _sm_guess_age_interaction_0, transitions={'finished': 'finished', 'failed': 'stop video detects and tracks'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) return _state_machine
def create(self): # x:283 y:267 _state_machine = OperatableStateMachine(outcomes=['Shutdown']) _state_machine.userdata.Command = "no nothing" _state_machine.userdata.End = False # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:887 y:420 _sm_sara_brain_0 = OperatableStateMachine( outcomes=['error'], input_keys=['HighFIFO', 'LowFIFO', 'MedFIFO', 'DoNow', 'End']) with _sm_sara_brain_0: # x:270 y:346 OperatableStateMachine.add( 'sara_command_manager', self.use_behavior( sara_command_managerSM, 'Sara parallel Runtime/Sara brain/sara_command_manager'), transitions={ 'finished': 'if stop', 'failed': 'set end' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'DoNow': 'DoNow' }) # x:725 y:388 OperatableStateMachine.add( 'set end', CalculationState(calculation=lambda x: True), transitions={'done': 'error'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'End', 'output_value': 'End' }) # x:516 y:448 OperatableStateMachine.add( 'if stop', CheckConditionState(predicate=lambda x: x[0][0] == "Stop"), transitions={ 'true': 'set end', 'false': 'sara_command_manager' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'DoNow'}) # x:841 y:231 _sm_sara_action_executor_1 = OperatableStateMachine( outcomes=['shutdown'], input_keys=['HighFIFO', 'MedFIFO', 'LowFIFO', 'DoNow', 'End']) with _sm_sara_action_executor_1: # x:128 y:134 OperatableStateMachine.add( 'log', LogState(text="Start action executor", severity=Logger.REPORT_HINT), transitions={'done': 'Sara action executor'}, autonomy={'done': Autonomy.Off}) # x:636 y:111 OperatableStateMachine.add( 'Critical failure', LogState(text="Critical fail in action executer!", severity=Logger.REPORT_HINT), transitions={'done': 'shutdown'}, autonomy={'done': Autonomy.Off}) # x:366 y:149 OperatableStateMachine.add( 'Sara action executor', self.use_behavior( SaraactionexecutorSM, 'Sara parallel Runtime/Sara action executor/Sara action executor' ), transitions={ 'CriticalFail': 'Critical failure', 'Shutdown': 'shutdown' }, autonomy={ 'CriticalFail': Autonomy.Inherit, 'Shutdown': Autonomy.Inherit }, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'End': 'End' }) # x:30 y:365 _sm_sara_init_2 = OperatableStateMachine(outcomes=['done']) with _sm_sara_init_2: # x:30 y:40 OperatableStateMachine.add('set head', SaraSetHeadAngle(angle=0.1), transitions={'done': 'set face'}, autonomy={'done': Autonomy.Off}) # x:47 y:119 OperatableStateMachine.add('set arm', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={ 'done': 'hello', 'failed': 'hello' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'target'}) # x:242 y:288 OperatableStateMachine.add( 'hello', SaraSay( sentence= "Good morning. I am Sara the robot. Please give me orders.", emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:239 y:21 OperatableStateMachine.add('set face', SetExpression(emotion=0, brightness=200), transitions={'done': 'www'}, autonomy={'done': Autonomy.Off}) # x:339 y:116 OperatableStateMachine.add('on face', SetExpression(emotion=1, brightness=-1), transitions={'done': 'setTarget'}, autonomy={'done': Autonomy.Off}) # x:484 y:41 OperatableStateMachine.add('www', WaitState(wait_time=2), transitions={'done': 'on face'}, autonomy={'done': Autonomy.Off}) # x:197 y:113 OperatableStateMachine.add('setTarget', SetKey(Value="IdlePose"), transitions={'done': 'set arm'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:55 y:366 _sm_create_fifos_3 = OperatableStateMachine( outcomes=['done'], output_keys=['HighFIFO', 'MedFIFO', 'LowFIFO', 'DoNow']) with _sm_create_fifos_3: # x:33 y:40 OperatableStateMachine.add('Create HighFIFO', FIFO_New(), transitions={'done': 'Create MedFIFO'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'HighFIFO'}) # x:30 y:114 OperatableStateMachine.add('Create MedFIFO', FIFO_New(), transitions={'done': 'Create LowFIFO'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'MedFIFO'}) # x:30 y:189 OperatableStateMachine.add('Create LowFIFO', FIFO_New(), transitions={'done': 'Create DoNow'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'LowFIFO'}) # x:34 y:260 OperatableStateMachine.add('Create DoNow', FIFO_New(), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'FIFO': 'DoNow'}) # x:30 y:322 _sm_sara_shutdown_4 = OperatableStateMachine(outcomes=['finished']) with _sm_sara_shutdown_4: # x:57 y:86 OperatableStateMachine.add('log', LogState(text="shutdown", severity=Logger.REPORT_HINT), transitions={'done': 'say'}, autonomy={'done': Autonomy.Off}) # x:122 y:228 OperatableStateMachine.add( 'say', SaraSay(sentence="I'm goint to shutdown for safety reasons", emotion=1, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:336 y:314, x:315 y:433, x:276 y:271 _sm_sara_parallel_runtime_5 = ConcurrencyContainer( outcomes=['Shutdown'], input_keys=['HighFIFO', 'MedFIFO', 'LowFIFO', 'DoNow', 'End'], conditions=[('Shutdown', [('Sara brain', 'error')]), ('Shutdown', [('Sara action executor', 'shutdown')])]) with _sm_sara_parallel_runtime_5: # x:52 y:416 OperatableStateMachine.add('Sara action executor', _sm_sara_action_executor_1, transitions={'shutdown': 'Shutdown'}, autonomy={'shutdown': Autonomy.Inherit}, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'DoNow': 'DoNow', 'End': 'End' }) # x:57 y:333 OperatableStateMachine.add('Sara brain', _sm_sara_brain_0, transitions={'error': 'Shutdown'}, autonomy={'error': Autonomy.Inherit}, remapping={ 'HighFIFO': 'HighFIFO', 'LowFIFO': 'LowFIFO', 'MedFIFO': 'MedFIFO', 'DoNow': 'DoNow', 'End': 'End' }) with _state_machine: # x:43 y:60 OperatableStateMachine.add('log', LogState(text="Start Sara", severity=Logger.REPORT_HINT), transitions={'done': 'low head'}, autonomy={'done': Autonomy.Off}) # x:306 y:365 OperatableStateMachine.add( 'Sara parallel Runtime', _sm_sara_parallel_runtime_5, transitions={'Shutdown': 'Sara shutdown'}, autonomy={'Shutdown': Autonomy.Inherit}, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'DoNow': 'DoNow', 'End': 'End' }) # x:315 y:462 OperatableStateMachine.add('Sara shutdown', _sm_sara_shutdown_4, transitions={'finished': 'low head'}, autonomy={'finished': Autonomy.Inherit}) # x:328 y:284 OperatableStateMachine.add( 'Create_FIFOs', _sm_create_fifos_3, transitions={'done': 'Sara parallel Runtime'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'HighFIFO': 'HighFIFO', 'MedFIFO': 'MedFIFO', 'LowFIFO': 'LowFIFO', 'DoNow': 'DoNow' }) # x:150 y:207 OperatableStateMachine.add('listen', GetSpeech(watchdog=10), transitions={ 'done': 'check hello', 'nothing': 'listen', 'fail': 'Shutdown' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'words'}) # x:345 y:195 OperatableStateMachine.add('Sara init', _sm_sara_init_2, transitions={'done': 'Create_FIFOs'}, autonomy={'done': Autonomy.Inherit}) # x:355 y:96 OperatableStateMachine.add( 'check hello', RegexTester( regex= ".*((wake up)|(sarah?)|(shut up)|(hello)(robot)|(hi)|(morning)|(greet)).*" ), transitions={ 'true': 'Sara init', 'false': 'listen' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'text': 'words', 'result': 'result' }) # x:15 y:220 OperatableStateMachine.add('close face', SetExpression(emotion=0, brightness=20), transitions={'done': 'listen'}, autonomy={'done': Autonomy.Off}) # x:102 y:115 OperatableStateMachine.add('low head', SaraSetHeadAngle(angle=0.8), transitions={'done': 'close face'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:284 y:484, x:47 y:223 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:454, x:130 y:454 _sm_take_photo_container_(guess_age)_0 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_take_photo_container_(guess_age)_0: # x:124 y:43 OperatableStateMachine.add('ok if you are ready show me a big smile', TalkState(sentence_number=3), transitions={'continue': 'smile', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:699 y:57 OperatableStateMachine.add('wait as if take photo', WaitState(wait_time=3), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:481 y:41 OperatableStateMachine.add('smile', FacialExpressionState(expression_num=1), transitions={'continue': 'wait as if take photo', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) with _state_machine: # x:109 y:52 OperatableStateMachine.add('launch video', LaunchVideoStream(), transitions={'continue': 'starting object track talk', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:514 y:19 OperatableStateMachine.add('detect and track', LaunchObjDetectAndTrack(), transitions={'continue': 'subscriber_is_nearby_face_State', 'failed': 'stop face detect'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:734 y:20 OperatableStateMachine.add('subscriber_is_nearby_face_State', SubscriberState(topic='/is_person_nearby', blocking=True, clear=False), transitions={'received': 'check_if_nearby', 'unavailable': 'stop obj detect state'}, autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off}, remapping={'message': 'message'}) # x:732 y:157 OperatableStateMachine.add('check_if_nearby', CheckConditionState(predicate=lambda message:message.data == "yes"), transitions={'true': 'stop obj detect state', 'false': 'logger'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'message'}) # x:588 y:115 OperatableStateMachine.add('logger', LogState(text="false", severity=Logger.REPORT_HINT), transitions={'done': 'subscriber_is_nearby_face_State'}, autonomy={'done': Autonomy.Off}) # x:935 y:201 OperatableStateMachine.add('stop obj detect state', StopObjectDetectAndTrack(), transitions={'continue': 'face identified', 'failed': 'stop face detect'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:960 y:281 OperatableStateMachine.add('launch face detect and track', FaceDetTrack(), transitions={'continue': 'wait to show face trk', 'failed': 'stop face detect'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1178 y:285 OperatableStateMachine.add('wait to show face trk', WaitState(wait_time=5), transitions={'done': 'talk hello'}, autonomy={'done': Autonomy.Off}) # x:1165 y:361 OperatableStateMachine.add('talk hello', TalkState(sentence_number=2), transitions={'continue': 'listen_for_input_state', 'failed': 'stop face detect'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:342 y:308 OperatableStateMachine.add('stop face detect', StopFaceDetectAndTrack(), transitions={'continue': 'finished', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:967 y:38 OperatableStateMachine.add('face identified', TalkState(sentence_number=100), transitions={'continue': 'start face track', 'failed': 'stop obj detect state'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1171 y:33 OperatableStateMachine.add('start face track', TalkState(sentence_number=101), transitions={'continue': 'launch face detect and track', 'failed': 'stop obj detect state'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:273 y:126 OperatableStateMachine.add('starting object track talk', TalkState(sentence_number=99), transitions={'continue': 'detect and track', 'failed': 'failed'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1164 y:441 OperatableStateMachine.add('listen_for_input_state', ListeningState(), transitions={'continue': 'subscribe to user input1', 'failed': 'stop face detect'}, autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off}) # x:1160 y:516 OperatableStateMachine.add('subscribe to user input1', SubscriberState(topic='/stt_topic', blocking=True, clear=False), transitions={'received': 'check user input1', 'unavailable': 'stop face detect'}, autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off}, remapping={'message': 'message_stt'}) # x:1180 y:590 OperatableStateMachine.add('check user input1', CheckConditionState(predicate=lambda message: message.query =="please take my photo"), transitions={'true': 'yes take photo', 'false': 'no take photo not received'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'message_stt'}) # x:1005 y:585 OperatableStateMachine.add('yes take photo', LogState(text="yes take photo recieved", severity=Logger.REPORT_HINT), transitions={'done': 'take photo container (guess age)'}, autonomy={'done': Autonomy.Off}) # x:1367 y:517 OperatableStateMachine.add('no take photo not received', LogState(text="no not recieved take photo", severity=Logger.REPORT_HINT), transitions={'done': 'listen_for_input_state'}, autonomy={'done': Autonomy.Off}) # x:727 y:532 OperatableStateMachine.add('take photo container (guess age)', _sm_take_photo_container_(guess_age)_0, transitions={'finished': 'left container', 'failed': 'stop face detect'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:585 y:606 OperatableStateMachine.add('left container', LogState(text="left container - now back on main", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:814 y:45, x:514 y:274 _state_machine = OperatableStateMachine(outcomes=['Found', 'NotFound'], output_keys=['Operator']) _state_machine.userdata.Operator = None _state_machine.userdata.Name = "person" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:506 y:393, x:515 y:462 _sm_move_to_person_0 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['Operator']) with _sm_move_to_person_0: # x:30 y:83 OperatableStateMachine.add( 'Getpos', CalculationState(calculation=lambda x: x.position), transitions={'done': 'setDistance'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Operator', 'output_value': 'pose_in' }) # x:35 y:450 OperatableStateMachine.add('Action_Move', self.use_behavior( Action_MoveSM, 'Move to person/Action_Move'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'Pose'}) # x:47 y:368 OperatableStateMachine.add('set not rel', SetKey(Value=False), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'relative'}) # x:41 y:179 OperatableStateMachine.add('setDistance', SetKey(Value=1.5), transitions={'done': 'Close position'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distance'}) # x:27 y:280 OperatableStateMachine.add('Close position', Get_Reacheable_Waypoint(), transitions={'done': 'set not rel'}, autonomy={'done': Autonomy.Off}, remapping={ 'pose_in': 'pose_in', 'distance': 'distance', 'pose_out': 'Pose' }) with _state_machine: # x:64 y:35 OperatableStateMachine.add( 'Get previous ID', GetRosParam(ParamName="behavior/Operator/Id"), transitions={ 'done': 'Get Operator', 'failed': 'for 3' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'ID'}) # x:271 y:37 OperatableStateMachine.add('Get Operator', GetEntityByID(), transitions={ 'found': 'Found', 'not_found': 'Say lost operator' }, autonomy={ 'found': Autonomy.Off, 'not_found': Autonomy.Off }, remapping={ 'ID': 'ID', 'Entity': 'Operator' }) # x:263 y:155 OperatableStateMachine.add('Say lost operator', SaraSay(sentence="I lost my operator", input_keys=[], emotion=1, block=True), transitions={'done': 'for 3'}, autonomy={'done': Autonomy.Off}) # x:780 y:517 OperatableStateMachine.add('ask if operator', SaraSay(sentence="Are you my operator?", input_keys=[], emotion=1, block=True), transitions={'done': 'get speech'}, autonomy={'done': Autonomy.Off}) # x:70 y:273 OperatableStateMachine.add('for 3', ForLoop(repeat=3), transitions={ 'do': 'for 3_2', 'end': 'set None' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:249 y:357 OperatableStateMachine.add('say where are you', SaraSay( sentence="Operator. Where are you?", input_keys=[], emotion=1, block=True), transitions={'done': 'for 3'}, autonomy={'done': Autonomy.Off}) # x:281 y:265 OperatableStateMachine.add('set None', SetKey(Value=None), transitions={'done': 'NotFound'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Operator'}) # x:49 y:511 OperatableStateMachine.add('Get persons', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'get next closest', 'none_found': 'say where are you' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'Name', 'entity_list': 'entity_list', 'number': 'number' }) # x:461 y:475 OperatableStateMachine.add('Move to person', _sm_move_to_person_0, transitions={ 'finished': 'ask if operator', 'failed': 'NotFound' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'Operator': 'Operator'}) # x:783 y:161 OperatableStateMachine.add( 'set new ID', SetRosParam(ParamName="behavior/Operator/Id"), transitions={'done': 'Found'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'ID'}) # x:775 y:269 OperatableStateMachine.add( 'get ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'set new ID'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Operator', 'output_value': 'ID' }) # x:784 y:433 OperatableStateMachine.add('get speech', GetSpeech(watchdog=5), transitions={ 'done': 'Yes ?', 'nothing': 'for 3_2', 'fail': 'NotFound' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'words'}) # x:69 y:402 OperatableStateMachine.add('for 3_2', ForLoop(repeat=3), transitions={ 'do': 'Get persons', 'end': 'set None' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index2'}) # x:744 y:332 OperatableStateMachine.add( 'Yes ?', CheckConditionState(predicate=lambda x: "yes" in x), transitions={ 'true': 'get ID', 'false': 'for 3_2' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'words'}) # x:263 y:535 OperatableStateMachine.add( 'get next closest', FlexibleCalculationState(calculation=lambda x: x[0][x[1]], input_keys=["entity_list", "index"]), transitions={'done': 'ask if operator'}, autonomy={'done': Autonomy.Off}, remapping={ 'entity_list': 'entity_list', 'index': 'index', 'output_value': 'Operator' }) return _state_machine
def create(self): exec_speed = 0.80 #was scary at 0.35 exec_acceleration = 1.00 # was scary at 0.40 coll_threshold = 10 # was 14 with CVL at first, then 12 before switch to D435 coll_repeats = 0 utter_incomplete = 'The gripper is not finished' utter_complete = 'The gripper is finished, remove from the workspace' utter_next_screw = 'move to next screw in the same gripper' utter_unknown = "Gripper is in an unknwon state" # x:1105 y:39, x:24 y:178 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.eSELECT_AVAILABLE_TRAJ = 12 _state_machine.userdata.eEXECUTE_FW = 1 _state_machine.userdata.eREAD_FROM_DISK = 6 _state_machine.userdata.eEXECUTE_BW = 2 _state_machine.userdata.ZERO = 0 _state_machine.userdata.endeffector_frame = 'tool_screwdriver_tip_link' _state_machine.userdata.reference_frame = 'base_link' _state_machine.userdata.tf_frame = 'screw_' _state_machine.userdata.velocity = 1.0 _state_machine.userdata.target_offset = [] _state_machine.userdata.offset_position_to_screw = [0.0, 0.0, 0.003] _state_machine.userdata.offset_position_from_screw = [0.0, 0.0, -0.022] _state_machine.userdata.offset_orientation_jiggle = [ 0.0, 0.0, 0.2, 1.0 ] _state_machine.userdata.no_offset_position = [0.0, 0.0, 0, 0] _state_machine.userdata.no_offset_orientation = [0.0, 0.0, 0.0, 1.0] _state_machine.userdata.eDRIVE_TO_START = 11 _state_machine.userdata.start_step = 0 _state_machine.userdata.eSELECT_AVAILABLE_REPLAN_TRAJ = 14 _state_machine.userdata.eEXECUTE_REPLAN_TRAJ = 15 _state_machine.userdata.eEXECUTE_BW_FROM_STEP = 17 _state_machine.userdata.eEXECUTE_FW_FROM_STEP = 16 _state_machine.userdata.eCREATE_NEW_TRAJ = 19 _state_machine.userdata.eEXECUTE_NEW_TRAJ = 20 _state_machine.userdata.next_trajectory = 0 _state_machine.userdata.list_available_screw_poses = [] _state_machine.userdata.use_same_gripper = False _state_machine.userdata.same_gripper_retry = 3 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:727 y:338, x:493 y:611 _sm_search_and_move_to_next_screw_0 = OperatableStateMachine( outcomes=['error', 'moved_to_next_screw'], input_keys=[ 'eEXECUTE_NEW_TRAJ', 'eCREATE_NEW_TRAJ', 'next_trajectory', 'list_available_screw_poses', 'use_same_gripper', 'same_gripper_retry' ], output_keys=['next_trajectory']) with _sm_search_and_move_to_next_screw_0: # x:92 y:22 OperatableStateMachine.add( 'use_same_gripper', CheckConditionState(predicate=lambda x: x == True), transitions={ 'true': 'log_use_same_grip', 'false': 'log_use_another_grip' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'use_same_gripper'}) # x:10 y:244 OperatableStateMachine.add( 'Available Screws Same Gripper', PartAvailablePoseService(check_only_current_gripper=True), transitions={ 'succeeded': 'Find Collision Free Trajectory', 'aborted': 'decrese_retry_same_gripper' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'last_trajectory_id': 'next_trajectory', 'list_available_parts': 'list_available_screw_poses' }) # x:51 y:484 OperatableStateMachine.add( 'Check_retry', CheckConditionState(predicate=lambda x: x > 0), transitions={ 'true': 'WaitForScrews', 'false': 'set_another_gripper' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'same_gripper_retry'}) # x:1035 y:602 OperatableStateMachine.add('Execute New Trajectory', SharedWsActionState( exec_speed=exec_speed, exec_acceleration=exec_acceleration, coll_threshold=coll_threshold, coll_repeats=coll_repeats), transitions={ 'succeeded': 'set_retry', 'preempted': 'error', 'aborted': 'utter new traj aborted' }, autonomy={ 'succeeded': Autonomy.Off, 'preempted': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'action_id': 'eEXECUTE_NEW_TRAJ', 'trajectory_id': 'next_trajectory', 'results': 'results' }) # x:597 y:178 OperatableStateMachine.add( 'Find Collision Free Trajectory', SharedWsNewTrajActionState(exec_speed=exec_speed, exec_acceleration=exec_acceleration, coll_threshold=coll_threshold, coll_repeats=coll_repeats), transitions={ 'succeeded': 'Log_next_traj_id', 'preempted': 'error', 'aborted': 'utter obstacle' }, autonomy={ 'succeeded': Autonomy.Off, 'preempted': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'action_id': 'eCREATE_NEW_TRAJ', 'trajectory_id': 'next_trajectory', 'list_available_screw_poses': 'list_available_screw_poses', 'results': 'results', 'next_trajectory': 'next_trajectory' }) # x:1081 y:148 OperatableStateMachine.add( 'Log_next_traj_id', LogKeyState(text="MAIN: Next Free Tjact. ID is for screw : {}", severity=Logger.REPORT_HINT), transitions={'done': 'Execute New Trajectory'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'next_trajectory'}) # x:1268 y:2 OperatableStateMachine.add( 'Sleep Until Retry', WaitState(wait_time=2), transitions={'done': 'use_same_gripper'}, autonomy={'done': Autonomy.Off}) # x:316 y:138 OperatableStateMachine.add( 'WaitForScrews', WaitState(wait_time=1), transitions={'done': 'use_same_gripper'}, autonomy={'done': Autonomy.Off}) # x:11 y:385 OperatableStateMachine.add( 'decrese_retry_same_gripper', CalculationState(calculation=lambda x: x - 1), transitions={'done': 'Check_retry'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'same_gripper_retry', 'output_value': 'same_gripper_retry' }) # x:339 y:50 OperatableStateMachine.add( 'log_use_another_grip', LogState(text="SMNS: use another gripper", severity=Logger.REPORT_HINT), transitions={'done': 'Available Screws Request'}, autonomy={'done': Autonomy.Off}) # x:54 y:117 OperatableStateMachine.add( 'log_use_same_grip', LogState(text="SMNS: use same gripper", severity=Logger.REPORT_HINT), transitions={'done': 'Available Screws Same Gripper'}, autonomy={'done': Autonomy.Off}) # x:98 y:633 OperatableStateMachine.add( 'set_another_gripper', CalculationState(calculation=lambda x: False), transitions={'done': 'utter_change gripper'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'use_same_gripper', 'output_value': 'use_same_gripper' }) # x:756 y:596 OperatableStateMachine.add( 'set_retry', CalculationState(calculation=lambda x: 3), transitions={'done': 'moved_to_next_screw'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'same_gripper_retry', 'output_value': 'same_gripper_retry' }) # x:1240 y:366 OperatableStateMachine.add( 'utter new traj aborted', LisaUtterActionState( text_to_utter="Trajectory execution was aborted", wait_time=0), transitions={ 'uttered_all': 'Sleep Until Retry', 'timeout': 'Sleep Until Retry', 'command_error': 'error' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:626 y:524 OperatableStateMachine.add( 'utter obstacle', LisaUtterActionState( text_to_utter= "Part available but no free path to move on it", wait_time=0), transitions={ 'uttered_all': 'WaitForScrews', 'timeout': 'WaitForScrews', 'command_error': 'error' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:288 y:377 OperatableStateMachine.add( 'utter_change gripper', LisaUtterActionState(text_to_utter="Change to another gripper", wait_time=0), transitions={ 'uttered_all': 'WaitForScrews', 'timeout': 'WaitForScrews', 'command_error': 'error' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:628 y:51 OperatableStateMachine.add( 'Available Screws Request', PartAvailablePoseService(check_only_current_gripper=False), transitions={ 'succeeded': 'Find Collision Free Trajectory', 'aborted': 'WaitForScrews' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'last_trajectory_id': 'next_trajectory', 'list_available_parts': 'list_available_screw_poses' }) # x:30 y:538, x:686 y:404, x:230 y:538 _sm_utter_wrong_insertion_1 = OperatableStateMachine( outcomes=['error', 'retry', 'skip'], input_keys=['next_trajectory']) with _sm_utter_wrong_insertion_1: # x:30 y:113 OperatableStateMachine.add( 'utter_not_inserted_text', LisaMotekConcatenateScrewPartString( text_format= "{}, was not inserted, would you like to: retry or, skip?", number_part_screws=4), transitions={'done': 'ask_retry'}, autonomy={'done': Autonomy.Off}, remapping={ 'screw_id': 'next_trajectory', 'text_to_utter': 'text_to_utter' }) # x:619 y:277 OperatableStateMachine.add( 'check_retry', CheckConditionState(predicate=lambda x: x == 'retry'), transitions={ 'true': 'retry', 'false': 'skip' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'intent_recognized'}) # x:392 y:173 OperatableStateMachine.add( 'ask_retry', LisaUtterAndWaitForIntentState(context_id=None, intents=['retry', 'skip'], wait_time=0), transitions={ 'intent_recognized': 'check_retry', 'intent_not_recognized': 'skip', 'preempt': 'skip', 'timeouted': 'skip', 'error': 'error' }, autonomy={ 'intent_recognized': Autonomy.Off, 'intent_not_recognized': Autonomy.Off, 'preempt': Autonomy.Off, 'timeouted': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'text_to_utter': 'text_to_utter', 'payload': 'payload', 'original_sentence': 'original_sentence', 'error_reason': 'error_reason', 'intent_recognized': 'intent_recognized' }) # x:611 y:158 _sm_log_screws_2 = OperatableStateMachine(outcomes=['done'], input_keys=[ 'screw_found', 'insertion_successful', 'next_trajectory' ]) with _sm_log_screws_2: # x:79 y:175 OperatableStateMachine.add( 'log_screw_id', LogKeyState(text="GCE-ENTER: insterd screw id {}", severity=Logger.REPORT_HINT), transitions={'done': 'log_last_insertion'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'next_trajectory'}) # x:423 y:441 OperatableStateMachine.add( 'log_screw_found', LogKeyState(text="GCE-ENTER: screw was found ? {}", severity=Logger.REPORT_HINT), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'screw_found'}) # x:217 y:276 OperatableStateMachine.add( 'log_last_insertion', LogKeyState(text="GCE-ENTER: last insertion was succesfu? {}", severity=Logger.REPORT_HINT), transitions={'done': 'log_screw_found'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'insertion_successful'}) # x:422 y:304, x:170 y:189 _sm_fix_for_screwed_but_not_found_3 = OperatableStateMachine( outcomes=['found_and_inserted', 'not_inserted'], input_keys=['screw_found', 'screw_inserted'], output_keys=['screw_found']) with _sm_fix_for_screwed_but_not_found_3: # x:156 y:37 OperatableStateMachine.add( 'Check is Inserted', CheckConditionState(predicate=lambda x: x == True), transitions={ 'true': 'check is found', 'false': 'not_inserted' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'screw_inserted'}) # x:287 y:127 OperatableStateMachine.add( 'check is found', CheckConditionState(predicate=lambda x: x == True), transitions={ 'true': 'found_and_inserted', 'false': 'fix for inserted not found' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'screw_found'}) # x:511 y:219 OperatableStateMachine.add( 'fix for inserted not found', CalculationState(calculation=lambda x: True), transitions={'done': 'found_and_inserted'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'screw_found', 'output_value': 'screw_found' }) # x:959 y:321, x:1285 y:172, x:1810 y:717, x:716 y:636, x:900 y:779 _sm_gripper_ended_check_4 = OperatableStateMachine( outcomes=[ 'fail', 'completed', 'screw_available', 'incomplete', 'unknown' ], input_keys=[ 'next_trajectory', 'insertion_successful', 'screw_found', 'use_same_gripper' ], output_keys=['list_available_screw_poses', 'use_same_gripper']) with _sm_gripper_ended_check_4: # x:58 y:31 OperatableStateMachine.add('Fix for screwed but not found', _sm_fix_for_screwed_but_not_found_3, transitions={ 'found_and_inserted': 'Update Screw State', 'not_inserted': 'Utter Wrong Insertion' }, autonomy={ 'found_and_inserted': Autonomy.Inherit, 'not_inserted': Autonomy.Inherit }, remapping={ 'screw_found': 'screw_found', 'screw_inserted': 'insertion_successful' }) # x:1696 y:24 OperatableStateMachine.add( 'Available Screw In current Gripper', PartAvailablePoseService(check_only_current_gripper=True), transitions={ 'succeeded': 'utter_next_traj', 'aborted': 'Ask for Gripper Complete' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'last_trajectory_id': 'next_trajectory', 'list_available_parts': 'list_available_screw_poses' }) # x:92 y:255 OperatableStateMachine.add( 'Log screws', _sm_log_screws_2, transitions={'done': 'Fix for screwed but not found'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'screw_found': 'screw_found', 'insertion_successful': 'insertion_successful', 'next_trajectory': 'next_trajectory' }) # x:318 y:69 OperatableStateMachine.add('Update Screw State', SetScrewServiceState(), transitions={ 'succeeded': 'wait_screw_update', 'aborted': 'log_exit_1' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'screw_id': 'next_trajectory', 'finished_successful': 'insertion_successful', 'screw_found': 'screw_found', 'result': 'result' }) # x:300 y:222 OperatableStateMachine.add( 'Utter Wrong Insertion', _sm_utter_wrong_insertion_1, transitions={ 'error': 'fail', 'retry': 'utter_retry', 'skip': 'Update Screw State' }, autonomy={ 'error': Autonomy.Inherit, 'retry': Autonomy.Inherit, 'skip': Autonomy.Inherit }, remapping={'next_trajectory': 'next_trajectory'}) # x:440 y:334 OperatableStateMachine.add( 'aaaa', WaitState(wait_time=3), transitions={'done': 'is_gripper_terminated'}, autonomy={'done': Autonomy.Off}) # x:1536 y:503 OperatableStateMachine.add( 'check answer', CheckConditionState( predicate=lambda x: x == "yes" or x == "completed"), transitions={ 'true': 'utter_completed', 'false': 'reset_gripper' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'answer'}) # x:839 y:534 OperatableStateMachine.add( 'exit_reset', LogState(text="DBG-EXIT: incomplete, gripper resetted", severity=Logger.REPORT_HINT), transitions={'done': 'utter_exit_not_complete'}, autonomy={'done': Autonomy.Off}) # x:1234 y:742 OperatableStateMachine.add( 'exit_unknwon', LogState(text="DBG-EXIT: unknwon, exit NOT resetted", severity=Logger.REPORT_HINT), transitions={'done': 'utter_unknown'}, autonomy={'done': Autonomy.Off}) # x:691 y:11 OperatableStateMachine.add( 'get_screw_in_gripper', GripperStateListener(input_id_is_screw=True, time_out=1.0, number_part_screws=4), transitions={ 'succeeded': 'aaaa', 'aborted': 'log_err_screws' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'selected_gripper_id': 'next_trajectory', 'gripper_status': 'gripper_status', 'list_gripper_screws_status': 'list_gripper_screws_status', 'gripper_id': 'gripper_id', 'gripper_name': 'gripper_name', 'error_reason': 'error_reason' }) # x:991 y:9 OperatableStateMachine.add( 'is_gripper_terminated', CheckGripperIsFinished(number_part_screws=4), transitions={ 'done': 'set_use_another_gripper', 'screws_available': 'set_use_same_gripper', 'operator_check': 'Ask for Gripper Complete', 'error': 'fail' }, autonomy={ 'done': Autonomy.Off, 'screws_available': Autonomy.Off, 'operator_check': Autonomy.Off, 'error': Autonomy.Off }, remapping={ 'gripper_id': 'gripper_id', 'screw_status_list': 'list_gripper_screws_status' }) # x:1706 y:508 OperatableStateMachine.add('log_answer', LogKeyState( text="GCE: User reply {}", severity=Logger.REPORT_HINT), transitions={'done': 'check answer'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'answer'}) # x:790 y:173 OperatableStateMachine.add( 'log_err_screws', LogKeyState(text="GEC: Fail with available screws: {}", severity=Logger.REPORT_HINT), transitions={'done': 'fail'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'error_reason'}) # x:677 y:174 OperatableStateMachine.add( 'log_exit_1', LogState(text="GEC: Updtae Screw State FAIL", severity=Logger.REPORT_HINT), transitions={'done': 'fail'}, autonomy={'done': Autonomy.Off}) # x:1071 y:276 OperatableStateMachine.add( 'log_exit_failed', LogKeyState(text="GCE: exit failed from Ask for gripper: {}", severity=Logger.REPORT_HINT), transitions={'done': 'fail'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'answer'}) # x:1464 y:257 OperatableStateMachine.add( 'reset_gripper', ResetScrewStatesInGripper(number_part_screws=4), transitions={ 'succeeded': 'exit_reset', 'aborted': 'fail', 'nothing_to_do': 'utter_completed' }, autonomy={ 'succeeded': Autonomy.Off, 'aborted': Autonomy.Off, 'nothing_to_do': Autonomy.Off }, remapping={ 'screw_id': 'next_trajectory', 'screw_status_list': 'list_gripper_screws_status', 'result': 'result', 'reason': 'reason' }) # x:1044 y:92 OperatableStateMachine.add( 'set_use_another_gripper', CalculationState(calculation=lambda x: False), transitions={'done': 'utter_completed'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'use_same_gripper', 'output_value': 'use_same_gripper' }) # x:1419 y:5 OperatableStateMachine.add( 'set_use_same_gripper', CalculationState(calculation=lambda x: True), transitions={'done': 'Available Screw In current Gripper'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'use_same_gripper', 'output_value': 'use_same_gripper' }) # x:1034 y:174 OperatableStateMachine.add( 'utter_completed', LisaUtterActionState(text_to_utter=utter_complete, wait_time=0), transitions={ 'uttered_all': 'completed', 'timeout': 'completed', 'command_error': 'fail' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:397 y:535 OperatableStateMachine.add( 'utter_exit_not_complete', LisaUtterActionState(text_to_utter=utter_incomplete, wait_time=0), transitions={ 'uttered_all': 'incomplete', 'timeout': 'incomplete', 'command_error': 'fail' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:1724 y:586 OperatableStateMachine.add( 'utter_next_traj', LisaUtterActionState(text_to_utter=utter_next_screw, wait_time=0), transitions={ 'uttered_all': 'screw_available', 'timeout': 'screw_available', 'command_error': 'fail' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:1474 y:671 OperatableStateMachine.add( 'utter_retry', LisaUtterActionState(text_to_utter="retry the same screw", wait_time=0), transitions={ 'uttered_all': 'screw_available', 'timeout': 'screw_available', 'command_error': 'fail' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:1020 y:745 OperatableStateMachine.add( 'utter_unknown', LisaUtterActionState(text_to_utter=utter_unknown, wait_time=0), transitions={ 'uttered_all': 'unknown', 'timeout': 'unknown', 'command_error': 'fail' }, autonomy={ 'uttered_all': Autonomy.Off, 'timeout': Autonomy.Off, 'command_error': Autonomy.Off }, remapping={'error_reason': 'error_reason'}) # x:504 y:2 OperatableStateMachine.add( 'wait_screw_update', WaitState(wait_time=2), transitions={'done': 'get_screw_in_gripper'}, autonomy={'done': Autonomy.Off}) # x:1526 y:155 OperatableStateMachine.add( 'Ask for Gripper Complete', self.use_behavior( GripperCompleteSM, 'Gripper Ended Check/Ask for Gripper Complete'), transitions={ 'finished': 'log_answer', 'failed': 'log_exit_failed', 'max_retry': 'exit_unknwon' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'max_retry': Autonomy.Inherit }, remapping={'answer': 'answer'}) with _state_machine: # x:112 y:30 OperatableStateMachine.add('Load Trajectory', SharedWsActionState( exec_speed=exec_speed, exec_acceleration=exec_acceleration, coll_threshold=coll_threshold, coll_repeats=coll_repeats), transitions={ 'succeeded': 'Move To Home', 'preempted': 'failed', 'aborted': 'failed' }, autonomy={ 'succeeded': Autonomy.Off, 'preempted': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'action_id': 'eREAD_FROM_DISK', 'trajectory_id': 'ZERO', 'results': 'results' }) # x:890 y:186 OperatableStateMachine.add( 'MotekInsertScrew', self.use_behavior(MotekInsertScrewSM, 'MotekInsertScrew'), transitions={ 'finished': 'Gripper Ended Check', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'velocity': 'velocity', 'insertion_successful': 'insertion_successful', 'screw_found': 'screw_found' }) # x:334 y:30 OperatableStateMachine.add( 'Move To Home', SharedWsActionState(exec_speed=exec_speed, exec_acceleration=exec_acceleration, coll_threshold=coll_threshold, coll_repeats=coll_repeats), transitions={ 'succeeded': 'search and move to next screw', 'preempted': 'failed', 'aborted': 'failed' }, autonomy={ 'succeeded': Autonomy.Off, 'preempted': Autonomy.Off, 'aborted': Autonomy.Off }, remapping={ 'action_id': 'eDRIVE_TO_START', 'trajectory_id': 'ZERO', 'results': 'results' }) # x:756 y:374 OperatableStateMachine.add( 'log_avail_screws', LogKeyState(text="MAIN: available screws \n{}", severity=Logger.REPORT_HINT), transitions={'done': 'Gripper Ended Check'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'list_available_screw_poses'}) # x:237 y:214 OperatableStateMachine.add( 'log_completed', LogKeyState(text="MAIN: Gripper complete, use_same_gripper={}", severity=Logger.REPORT_HINT), transitions={'done': 'Move To Home'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'use_same_gripper'}) # x:523 y:123 OperatableStateMachine.add( 'log_uncomplete', LogKeyState( text="MAIN: Gripper uncomplete, use_same_gripper={}", severity=Logger.REPORT_HINT), transitions={'done': 'search and move to next screw'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'use_same_gripper'}) # x:385 y:220 OperatableStateMachine.add('log_unknown', LogKeyState( text="gripper is unknown {}", severity=Logger.REPORT_HINT), transitions={'done': 'Move To Home'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'use_same_gripper'}) # x:599 y:216 OperatableStateMachine.add( 'screw_available', LogKeyState( text="Availavke screw same gripper, use_same_gripper={}", severity=Logger.REPORT_HINT), transitions={'done': 'search and move to next screw'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'use_same_gripper'}) # x:716 y:23 OperatableStateMachine.add('search and move to next screw', _sm_search_and_move_to_next_screw_0, transitions={ 'error': 'failed', 'moved_to_next_screw': 'MotekInsertScrew' }, autonomy={ 'error': Autonomy.Inherit, 'moved_to_next_screw': Autonomy.Inherit }, remapping={ 'eEXECUTE_NEW_TRAJ': 'eEXECUTE_NEW_TRAJ', 'eCREATE_NEW_TRAJ': 'eCREATE_NEW_TRAJ', 'next_trajectory': 'next_trajectory', 'list_available_screw_poses': 'list_available_screw_poses', 'use_same_gripper': 'use_same_gripper', 'same_gripper_retry': 'same_gripper_retry' }) # x:362 y:383 OperatableStateMachine.add('Gripper Ended Check', _sm_gripper_ended_check_4, transitions={ 'fail': 'failed', 'completed': 'log_completed', 'screw_available': 'screw_available', 'incomplete': 'log_uncomplete', 'unknown': 'log_unknown' }, autonomy={ 'fail': Autonomy.Inherit, 'completed': Autonomy.Inherit, 'screw_available': Autonomy.Inherit, 'incomplete': Autonomy.Inherit, 'unknown': Autonomy.Inherit }, remapping={ 'next_trajectory': 'next_trajectory', 'insertion_successful': 'insertion_successful', 'screw_found': 'screw_found', 'use_same_gripper': 'use_same_gripper', 'list_available_screw_poses': 'list_available_screw_poses' }) return _state_machine
def create(self): # x:998 y:146, x:723 y:729, x:1065 y:284 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = [] _state_machine.userdata.person_name = "operator" _state_machine.userdata.Empty = None _state_machine.userdata.className = "person" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:585 y:427, x:704 y:287, x:702 y:47 _sm_get_person_0 = OperatableStateMachine( outcomes=['true', 'done', 'pas_done'], input_keys=['Action', 'className'], output_keys=['entity']) with _sm_get_person_0: # x:30 y:40 OperatableStateMachine.add( 'cond', CheckConditionState(predicate=lambda x: x[1] != ''), transitions={ 'true': 'get ', 'false': 'Action_findPerson_2' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'Action'}) # x:9 y:537 OperatableStateMachine.add('name', GetSpeech(watchdog=5), transitions={ 'done': 'confirming_persons_name', 'nothing': 'name', 'fail': 'Action_findPerson' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'person_name'}) # x:295 y:359 OperatableStateMachine.add( 'confirming_persons_name', CheckConditionState(predicate=lambda x: "yes" in x), transitions={ 'true': 'true', 'false': 'Action_findPerson' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'person_name'}) # x:338 y:105 OperatableStateMachine.add( 'Action_findPerson_2', self.use_behavior(sara_flexbe_behaviors__Action_findPersonSM, 'get_person/Action_findPerson_2'), transitions={ 'done': 'done', 'pas_done': 'pas_done' }, autonomy={ 'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit }, remapping={ 'className': 'className', 'entity': 'entity' }) # x:21 y:223 OperatableStateMachine.add( 'Action_findPerson', self.use_behavior(sara_flexbe_behaviors__Action_findPersonSM, 'get_person/Action_findPerson'), transitions={ 'done': 'Is_Person', 'pas_done': 'Action_findPerson_2' }, autonomy={ 'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit }, remapping={ 'className': 'className', 'entity': 'entity' }) # x:47 y:127 OperatableStateMachine.add( 'get ', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Action_findPerson'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Action', 'output_value': 'person_name' }) # x:111 y:343 OperatableStateMachine.add( 'Is_Person', SaraSay(sentence=lambda x: "Hello, are you " + x + "?", input_keys=[], emotion=0, block=True), transitions={'done': 'name'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:43 y:40 OperatableStateMachine.add( 'Object', GetRosParam(ParamName="behavior/GripperContent"), transitions={ 'done': 'get_person', 'failed': 'no_object' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'gripperContent'}) # x:369 y:92 OperatableStateMachine.add( 'Action_Give', self.use_behavior(sara_flexbe_behaviors__Action_GiveSM, 'Action_Give'), transitions={ 'Given': 'empty hand', 'Person_not_found': 'Person_Lost', 'No_object_in_hand': 'cause1', 'fail': 'cause3' }, autonomy={ 'Given': Autonomy.Inherit, 'Person_not_found': Autonomy.Inherit, 'No_object_in_hand': Autonomy.Inherit, 'fail': Autonomy.Inherit }) # x:286 y:217 OperatableStateMachine.add( 'Nobody_here', SaraSay(sentence="I can't find a person. Goodbye.", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:398 y:360 OperatableStateMachine.add( 'getID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'Action_Give'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'ID' }) # x:116 y:178 OperatableStateMachine.add('get_person', _sm_get_person_0, transitions={ 'true': 'confirm giving', 'done': 'confirm giving', 'pas_done': 'Nobody_here' }, autonomy={ 'true': Autonomy.Inherit, 'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit }, remapping={ 'Action': 'Action', 'className': 'className', 'entity': 'entity' }) # x:53 y:480 OperatableStateMachine.add( 'no_object', SaraSay(sentence="There is nothing in my gripper.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}) # x:212 y:355 OperatableStateMachine.add( 'confirm giving', SaraSay(sentence="Let me give you this object.", input_keys=[], emotion=1, block=True), transitions={'done': 'getID'}, autonomy={'done': Autonomy.Off}) # x:806 y:92 OperatableStateMachine.add( 'empty hand', SetRosParam(ParamName="behavior/GripperContent"), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Empty'}) # x:586 y:453 OperatableStateMachine.add( 'cause1', SetKey(Value="There was nothing in my gripper."), transitions={'done': 'setcausefailure'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:789 y:373 OperatableStateMachine.add('cause2', SetKey(Value="I lost the person."), transitions={'done': 'setcausefailure'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:886 y:230 OperatableStateMachine.add( 'cause3', SetKey(Value="I was unable to give the object."), transitions={'done': 'setcausefailure'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:930 y:463 OperatableStateMachine.add( 'setcausefailure', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:680 y:259 OperatableStateMachine.add( 'Person_Lost', SaraSay(sentence=lambda x: "I've lost " + x[1] + "!", input_keys=[], emotion=0, block=True), transitions={'done': 'cause2'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:902 y:757, x:882 y:161 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:1232 y:907, x:1267 y:613, x:1271 y:566 _sm_find_table_and_place_0 = OperatableStateMachine(outcomes=['finished', 'failed', 'no_table']) with _sm_find_table_and_place_0: # x:877 y:673 OperatableStateMachine.add('run traj', RunTrajectory(file="receive_object", duration=0), transitions={'done': 'say release'}, autonomy={'done': Autonomy.Off}) # x:532 y:572 OperatableStateMachine.add('Action_find_2', self.use_behavior(Action_findSM, 'take objects and bring the order to customer/find table and place/Action_find_2'), transitions={'done': 'get table position', 'failed': 'say do not find table'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'className': 'tableKey', 'entity': 'tableEntity'}) # x:576 y:514 OperatableStateMachine.add('set tableKey', SetKey(Value="table"), transitions={'done': 'Action_find_2'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'tableKey'}) # x:538 y:638 OperatableStateMachine.add('get table position', CalculationState(calculation=lambda x: x.position), transitions={'done': 'Action_place'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'tableEntity', 'output_value': 'pose'}) # x:736 y:677 OperatableStateMachine.add('say do not find table', SaraSay(sentence="I can not find the table. Please be ready to grab the object.", input_keys=[], emotion=0, block=True), transitions={'done': 'run traj'}, autonomy={'done': Autonomy.Off}) # x:41 y:231 OperatableStateMachine.add('find a table and a free spot', GetPositionToPlaceOnTable(), transitions={'done': 'Action_place', 'not_found': 'look down_left'}, autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'}) # x:291 y:805 OperatableStateMachine.add('Action_place', self.use_behavior(Action_placeSM, 'take objects and bring the order to customer/find table and place/Action_place'), transitions={'finished': 'finished', 'failed': 'say do not find table'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pos': 'pose'}) # x:50 y:102 OperatableStateMachine.add('look down', SaraSetHeadAngle(pitch=0, yaw=0.7), transitions={'done': 'wait'}, autonomy={'done': Autonomy.Off}) # x:214 y:378 OperatableStateMachine.add('find a table and a free spot_2', GetPositionToPlaceOnTable(), transitions={'done': 'Action_place', 'not_found': 'look down_right'}, autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'}) # x:240 y:232 OperatableStateMachine.add('look down_left', SaraSetHeadAngle(pitch=0.7, yaw=0.6), transitions={'done': 'wait_2'}, autonomy={'done': Autonomy.Off}) # x:387 y:513 OperatableStateMachine.add('find a table and a free spot_2_2', GetPositionToPlaceOnTable(), transitions={'done': 'Action_place', 'not_found': 'set tableKey'}, autonomy={'done': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'distanceFromEdge': 'distanceFromEdge', 'position': 'pose'}) # x:402 y:378 OperatableStateMachine.add('look down_right', SaraSetHeadAngle(pitch=-0.7, yaw=0.6), transitions={'done': 'wait_3'}, autonomy={'done': Autonomy.Off}) # x:66 y:169 OperatableStateMachine.add('wait', WaitState(wait_time=3), transitions={'done': 'find a table and a free spot'}, autonomy={'done': Autonomy.Off}) # x:242 y:294 OperatableStateMachine.add('wait_2', WaitState(wait_time=3), transitions={'done': 'find a table and a free spot_2'}, autonomy={'done': Autonomy.Off}) # x:411 y:444 OperatableStateMachine.add('wait_3', WaitState(wait_time=3), transitions={'done': 'find a table and a free spot_2_2'}, autonomy={'done': Autonomy.Off}) # x:1000 y:670 OperatableStateMachine.add('say release', SaraSay(sentence="Here is your order. I will open my gripper in 3, 2, 1, now.", input_keys=[], emotion=0, block=True), transitions={'done': 'open gripper'}, autonomy={'done': Autonomy.Off}) # x:1122 y:666 OperatableStateMachine.add('open gripper', SetGripperState(width=0.1, effort=1), transitions={'object': 'say good day', 'no_object': 'say good day'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:1170 y:771 OperatableStateMachine.add('say good day', SaraSay(sentence="Thank you. Have a good day.", input_keys=[], emotion=6, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:62 y:29 OperatableStateMachine.add('set distance', SetKey(Value=0.3), transitions={'done': 'look down'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distanceFromEdge'}) # x:30 y:458, x:130 y:458 _sm_keep_looking_1 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['personID']) with _sm_keep_looking_1: # x:30 y:40 OperatableStateMachine.add('keep', KeepLookingAt(), transitions={'failed': 'keep'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'personID'}) # x:30 y:458, x:130 y:458 _sm_ask_2 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['question'], output_keys=['answer']) with _sm_ask_2: # x:30 y:40 OperatableStateMachine.add('Action_Ask', self.use_behavior(Action_AskSM, 'ask and save order/ask AND look person/ask/Action_Ask'), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'question': 'question', 'answer': 'answer'}) # x:30 y:458, x:130 y:458 _sm_confirm_order_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['orderList']) with _sm_confirm_order_3: # x:114 y:67 OperatableStateMachine.add('produce question', CalculationState(calculation=lambda x: "Did you order :" + str(x[0].data)), transitions={'done': 'Action_Ask'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'orderList', 'output_value': 'question'}) # x:423 y:171 OperatableStateMachine.add('Action_Ask', self.use_behavior(Action_AskSM, 'ask and save order/confirm order/Action_Ask'), transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'question': 'question', 'answer': 'answer'}) # x:348 y:66, x:351 y:97, x:339 y:159, x:336 y:195, x:518 y:89, x:515 y:175 _sm_ask_and_look_person_4 = ConcurrencyContainer(outcomes=['finished', 'failed'], input_keys=['personID', 'question'], output_keys=['answer'], conditions=[ ('finished', [('ask', 'finished')]), ('failed', [('ask', 'failed')]), ('finished', [('keep looking', 'finished')]), ('failed', [('keep looking', 'failed')]) ]) with _sm_ask_and_look_person_4: # x:30 y:40 OperatableStateMachine.add('ask', _sm_ask_2, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'question': 'question', 'answer': 'answer'}) # x:30 y:138 OperatableStateMachine.add('keep looking', _sm_keep_looking_1, transitions={'finished': 'finished', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'personID': 'personID'}) # x:30 y:458 _sm_look_around_2_5 = OperatableStateMachine(outcomes=['finished']) with _sm_look_around_2_5: # x:215 y:156 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:394 y:262 OperatableStateMachine.add('right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), 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:38 y:596 OperatableStateMachine.add('w1', WaitState(wait_time=4), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:413 y:159 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'right'}, autonomy={'done': Autonomy.Off}) # x:415 y:370 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'center2'}, autonomy={'done': Autonomy.Off}) # x:39 y:371 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'left'}, autonomy={'done': Autonomy.Off}) # x:25 y:482 OperatableStateMachine.add('left', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:30 y:458 _sm_look_around_6 = OperatableStateMachine(outcomes=['finished']) with _sm_look_around_6: # x:215 y:156 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:394 y:262 OperatableStateMachine.add('right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), 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:38 y:596 OperatableStateMachine.add('w1', WaitState(wait_time=4), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:413 y:159 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'right'}, autonomy={'done': Autonomy.Off}) # x:415 y:370 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'center2'}, autonomy={'done': Autonomy.Off}) # x:39 y:371 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'left'}, autonomy={'done': Autonomy.Off}) # x:25 y:482 OperatableStateMachine.add('left', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:30 y:458, x:130 y:458 _sm_turn_around_7 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_turn_around_7: # x:80 y:26 OperatableStateMachine.add('look around', _sm_look_around_6, transitions={'finished': 'pose with 180 rotation'}, autonomy={'finished': Autonomy.Inherit}) # x:66 y:286 OperatableStateMachine.add('rotation', SaraMoveBase(reference="base_link"), transitions={'arrived': 'look around_2', 'failed': 'failed'}, autonomy={'arrived': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'pose': 'pose'}) # x:56 y:388 OperatableStateMachine.add('look around_2', _sm_look_around_2_5, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}) # x:63 y:133 OperatableStateMachine.add('pose with 180 rotation', GenPoseEuler(x=0, y=0, z=0, roll=0, pitch=0, yaw=3.14), transitions={'done': 'rotation'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'pose'}) # x:30 y:458 _sm_get_waving_people_8 = OperatableStateMachine(outcomes=['finished'], output_keys=['waving_person']) with _sm_get_waving_people_8: # x:47 y:45 OperatableStateMachine.add('set name', SetKey(Value="person"), transitions={'done': 'list people'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'name'}) # x:40 y:236 OperatableStateMachine.add('filter waving', FilterKey(filter_function=lambda x: x[0].pose.right_arm_up or x[0].pose.left_arm_up, input_keys=["input_list"]), transitions={'not_empty': 'get first', 'empty': 'list people'}, autonomy={'not_empty': Autonomy.Off, 'empty': Autonomy.Off}, remapping={'input_list': 'entity_list', 'output_list': 'waving_peoples'}) # x:53 y:326 OperatableStateMachine.add('get first', CalculationState(calculation=lambda x: x[0]), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'waving_peoples', 'output_value': 'waving_person'}) # x:40 y:133 OperatableStateMachine.add('list people', list_entities_by_name(frontality_level=0.5, distance_max=5), transitions={'found': 'filter waving', 'none_found': 'list people'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'name', 'entity_list': 'entity_list', 'number': 'number'}) # x:30 y:458, x:130 y:458, x:230 y:458, x:330 y:458, x:430 y:458 _sm_look_for_waving_people_9 = ConcurrencyContainer(outcomes=['finished', 'failed'], output_keys=['waving_person'], conditions=[ ('failed', [('turn around', 'failed')]), ('failed', [('turn around', 'finished')]), ('finished', [('get waving people', 'finished')]) ]) with _sm_look_for_waving_people_9: # x:268 y:118 OperatableStateMachine.add('get waving people', _sm_get_waving_people_8, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'waving_person': 'waving_person'}) # x:60 y:126 OperatableStateMachine.add('turn around', _sm_turn_around_7, transitions={'finished': 'failed', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:441 y:583, x:93 y:568 _sm_repeate_if_first_commande_10 = OperatableStateMachine(outcomes=['finished', 'repeate'], input_keys=['commandNumber'], output_keys=['commandNumber']) with _sm_repeate_if_first_commande_10: # x:242 y:97 OperatableStateMachine.add('if first command', CheckConditionState(predicate=lambda x: x == 1), transitions={'true': 'set second command', 'false': 'finished'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'commandNumber'}) # x:73 y:329 OperatableStateMachine.add('set second command', SetKey(Value=2), transitions={'done': 'repeate'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'commandNumber'}) # x:1053 y:687, x:1068 y:149 _sm_take_objects_and_bring_the_order_to_customer_11 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['barPosition', 'orderList', 'robotPositionToCustomer']) with _sm_take_objects_and_bring_the_order_to_customer_11: # x:73 y:26 OperatableStateMachine.add('set indexkey', SetKey(Value=0), transitions={'done': 'one element by one element from the list'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'indexKey'}) # x:591 y:684 OperatableStateMachine.add('increment indexKey', CalculationState(calculation=lambda x: x+1), transitions={'done': 'check if end of the list'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'indexKey', 'output_value': 'indexKey'}) # x:796 y:679 OperatableStateMachine.add('check if end of the list', FlexibleCheckConditionState(predicate=lambda x: len(x[0]) <= x[1], input_keys=["orderList", "indexKey"]), transitions={'true': 'finished', 'false': 'Action_Move_2'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'orderList': 'orderList', 'indexKey': 'indexKey'}) # x:31 y:243 OperatableStateMachine.add('Action_find', self.use_behavior(Action_findSM, 'take objects and bring the order to customer/Action_find'), transitions={'done': 'get entity ID', 'failed': 'say cannot find'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'className': 'item', 'entity': 'entity'}) # x:76 y:183 OperatableStateMachine.add('say search and grip', SaraSay(sentence=lambda x: "I am now searching for the "+x[0]+".", input_keys=["item"], emotion=0, block=True), transitions={'done': 'Action_find'}, autonomy={'done': Autonomy.Off}, remapping={'item': 'item'}) # x:258 y:261 OperatableStateMachine.add('say cannot find', SaraSay(sentence="I cannot find the item.", input_keys=[], emotion=0, block=True), transitions={'done': 'place arm'}, autonomy={'done': Autonomy.Off}) # x:63 y:393 OperatableStateMachine.add('Action_pick', self.use_behavior(Action_pickSM, 'take objects and bring the order to customer/Action_pick'), transitions={'success': 'say go to the customer', 'unreachable': 'say cannot pick', 'not found': 'say cannot pick', 'dropped': 'say cannot pick'}, autonomy={'success': Autonomy.Inherit, 'unreachable': Autonomy.Inherit, 'not found': Autonomy.Inherit, 'dropped': Autonomy.Inherit}, remapping={'objectID': 'entityID'}) # x:61 y:548 OperatableStateMachine.add('Action_Move', self.use_behavior(Action_MoveSM, 'take objects and bring the order to customer/Action_Move'), transitions={'finished': 'find table and place', 'failed': 'say cant get back to customer'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'robotPositionToCustomer'}) # x:83 y:326 OperatableStateMachine.add('get entity ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'Action_pick'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity', 'output_value': 'entityID'}) # x:568 y:407 OperatableStateMachine.add('say put in the gripper', SaraSay(sentence=lambda x: "Please, put the "+x[0]+" in my gripper. I will close it in 3, 2, 1, now.", input_keys=["item"], emotion=0, block=True), transitions={'done': 'close gripper'}, autonomy={'done': Autonomy.Off}, remapping={'item': 'item'}) # x:243 y:387 OperatableStateMachine.add('say cannot pick', SaraSay(sentence="I can not pick the item.", input_keys=[], emotion=0, block=True), transitions={'done': 'place arm'}, autonomy={'done': Autonomy.Off}) # x:388 y:502 OperatableStateMachine.add('say thank you', SaraSay(sentence="Thank you.", input_keys=[], emotion=0, block=True), transitions={'done': 'place arm 2'}, autonomy={'done': Autonomy.Off}) # x:72 y:465 OperatableStateMachine.add('say go to the customer', SaraSay(sentence="I will serve it to the customer.", input_keys=[], emotion=0, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:522 y:505 OperatableStateMachine.add('close gripper', SetGripperState(width=0, effort=1), transitions={'object': 'say thank you', 'no_object': 'open gripper'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:357 y:569 OperatableStateMachine.add('say cant get back to customer', SaraSay(sentence="I am not able to go back to the customer.", input_keys=[], emotion=0, block=True), transitions={'done': 'increment indexKey'}, autonomy={'done': Autonomy.Off}) # x:541 y:317 OperatableStateMachine.add('open gripper', SetGripperState(width=0.10, effort=1), transitions={'object': 'say put in the gripper', 'no_object': 'say put in the gripper'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:63 y:110 OperatableStateMachine.add('one element by one element from the list', FlexibleCalculationState(calculation=lambda x: x[0][x[1]].data, input_keys=["orderList", "indexKey"]), transitions={'done': 'say search and grip'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList', 'indexKey': 'indexKey', 'output_value': 'item'}) # x:376 y:324 OperatableStateMachine.add('place arm', RunTrajectory(file="receive_object", duration=0), transitions={'done': 'open gripper'}, autonomy={'done': Autonomy.Off}) # x:638 y:184 OperatableStateMachine.add('Action_Move_2', self.use_behavior(Action_MoveSM, 'take objects and bring the order to customer/Action_Move_2'), transitions={'finished': 'increment indexKey', 'failed': 'say cannot go back to bar'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'barPosition'}) # x:817 y:178 OperatableStateMachine.add('say cannot go back to bar', SaraSay(sentence="I am not able to go back to the barman. I will try a new command.", input_keys=[], emotion=0, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:85 y:740 OperatableStateMachine.add('find table and place', _sm_find_table_and_place_0, transitions={'finished': 'retour repos', 'failed': 'retour repos', 'no_table': 'retour repos'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'no_table': Autonomy.Inherit}) # x:248 y:505 OperatableStateMachine.add('place arm 2', RunTrajectory(file="transport", duration=0), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:362 y:679 OperatableStateMachine.add('retour repos', RunTrajectory(file="repos", duration=0), transitions={'done': 'increment indexKey'}, autonomy={'done': Autonomy.Off}) # x:242 y:352, x:1086 y:243 _sm_get_the_order_12 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['orderList']) with _sm_get_the_order_12: # x:68 y:39 OperatableStateMachine.add('length 1', CheckConditionState(predicate=lambda x: len(x) == 1), transitions={'true': 'say order', 'false': 'length 2'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'orderList'}) # x:236 y:41 OperatableStateMachine.add('length 2', CheckConditionState(predicate=lambda x: len(x) ==2), transitions={'true': 'say order_2', 'false': 'say order_3'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'orderList'}) # x:228 y:111 OperatableStateMachine.add('say order_2', SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+" and one "+x[0][1].data+", please.", input_keys=["orderList"], emotion=0, block=True), transitions={'done': 'wait 5'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList'}) # x:374 y:109 OperatableStateMachine.add('say order_3', SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+", one "+x[0][1].data+" and one "+x[0][2].data+", please.", input_keys=["orderList"], emotion=0, block=True), transitions={'done': 'wait 5'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList'}) # x:67 y:117 OperatableStateMachine.add('say order', SaraSay(sentence=lambda x: "For this order, I would like to have one "+x[0][0].data+", please.", input_keys=["orderList"], emotion=0, block=True), transitions={'done': 'wait 5'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList'}) # x:224 y:256 OperatableStateMachine.add('wait 5', WaitState(wait_time=0), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:216 y:528, x:572 y:238 _sm_go_to_the_barman_13 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['barPosition', 'barmanID']) with _sm_go_to_the_barman_13: # x:30 y:102 OperatableStateMachine.add('Action_Move', self.use_behavior(Action_MoveSM, 'go to the barman/Action_Move'), transitions={'finished': 'if barman id is 0', 'failed': 'say cannot'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'barPosition'}) # x:365 y:99 OperatableStateMachine.add('say cannot', SaraSay(sentence="I can not reach my destination. I will take another order. If you have one, please raise your hand.", input_keys=[], emotion=0, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:30 y:320 OperatableStateMachine.add('Action_findPersonByID', self.use_behavior(Action_findPersonByIDSM, 'go to the barman/Action_findPersonByID', default_keys=['className']), transitions={'found': 'finished', 'not_found': 'finished'}, autonomy={'found': Autonomy.Inherit, 'not_found': Autonomy.Inherit}, remapping={'personID': 'barmanID', 'personEntity': 'personEntity'}) # x:115 y:230 OperatableStateMachine.add('if barman id is 0', CheckConditionState(predicate=lambda x: x == 0), transitions={'true': 'finished', 'false': 'Action_findPersonByID'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'barmanID'}) # x:898 y:561, x:871 y:123 _sm_ask_and_save_order_14 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['customerID'], output_keys=['orderList']) with _sm_ask_and_save_order_14: # x:70 y:112 OperatableStateMachine.add('say ready', SaraSay(sentence="Hello.", input_keys=[], emotion=0, block=True), transitions={'done': 'set question'}, autonomy={'done': Autonomy.Off}) # x:69 y:187 OperatableStateMachine.add('set question', SetKey(Value="What do you want to order?"), transitions={'done': 'ask AND look person'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'question'}) # x:411 y:138 OperatableStateMachine.add('say cannot', SaraSay(sentence="I am not able to understand your order. I will take another order.", input_keys=[], emotion=0, block=True), transitions={'done': 'set question'}, autonomy={'done': Autonomy.Off}) # x:59 y:272 OperatableStateMachine.add('ask AND look person', _sm_ask_and_look_person_4, transitions={'finished': 'nlu restaurant', 'failed': 'say cannot'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'personID': 'customerID', 'question': 'question', 'answer': 'answer'}) # x:411 y:453 OperatableStateMachine.add('nlu restaurant', SaraNLUrestaurant(), transitions={'understood': 'confirm order', 'fail': 'say cannot'}, autonomy={'understood': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'sentence': 'answer', 'orderList': 'orderList'}) # x:657 y:417 OperatableStateMachine.add('confirm order', _sm_confirm_order_3, transitions={'finished': 'say bring', 'failed': 'set question'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'orderList': 'orderList'}) # x:852 y:402 OperatableStateMachine.add('say bring', SaraSay(sentence=lambda x: "understood. I will bring you the "+str(x[0][0].data), input_keys=["orderList"], emotion=0, block=False), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'orderList': 'orderList'}) # x:677 y:318, x:735 y:160 _sm_move_to_table_and_save_position_15 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['customerPosition'], output_keys=['robotPositionToCustomer']) with _sm_move_to_table_and_save_position_15: # x:69 y:24 OperatableStateMachine.add('set distance to person', SetKey(Value=0.7), transitions={'done': 'compute robot pose to customer'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distanceToPerson'}) # x:344 y:115 OperatableStateMachine.add('say cannot', SaraSay(sentence="I can not reach my destination. I will take another order. If you have one, please raise your hand.", input_keys=[], emotion=0, block=True), transitions={'done': 'save pose to not get flexbe problems'}, autonomy={'done': Autonomy.Off}) # x:497 y:129 OperatableStateMachine.add('save pose to not get flexbe problems', Get_Robot_Pose(), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'pose'}) # x:47 y:98 OperatableStateMachine.add('compute robot pose to customer', Get_Reacheable_Waypoint(), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'pose_in': 'customerPosition', 'distance': 'distanceToPerson', 'pose_out': 'robotPositionToCustomer'}) # x:78 y:199 OperatableStateMachine.add('Action_Move', self.use_behavior(Action_MoveSM, 'move to table and save position/Action_Move'), transitions={'finished': 'finished', 'failed': 'say cannot'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'robotPositionToCustomer'}) # x:112 y:525, x:363 y:46 _sm_detect_people_waving_16 = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['customerPosition', 'customerID']) with _sm_detect_people_waving_16: # x:84 y:25 OperatableStateMachine.add('say looking', SaraSay(sentence="I am now looking for customers. If you want to order something, please raise your hand.", input_keys=[], emotion=0, block=False), transitions={'done': 'Look for waving people'}, autonomy={'done': Autonomy.Off}) # x:73 y:375 OperatableStateMachine.add('reachable position_2', Get_Reacheable_Waypoint(), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'pose_in': 'customerPosition', 'distance': 'distance', 'pose_out': 'customerPosition'}) # x:71 y:291 OperatableStateMachine.add('set distance to customer_2', SetKey(Value=0.5), transitions={'done': 'reachable position_2'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'distance'}) # x:94 y:216 OperatableStateMachine.add('get attributes', GetAttribute(attributes=["ID", "position"]), transitions={'done': 'set distance to customer_2'}, autonomy={'done': Autonomy.Off}, remapping={'object': 'waving_person', 'ID': 'customerID', 'position': 'customerPosition'}) # x:63 y:120 OperatableStateMachine.add('Look for waving people', _sm_look_for_waving_people_9, transitions={'finished': 'get attributes', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'waving_person': 'waving_person'}) # x:874 y:499, x:747 y:100 _sm_save_bar_position_and_initiation_17 = OperatableStateMachine(outcomes=['finished', 'failed'], output_keys=['barPosition', 'commandNumber', 'barmanID']) with _sm_save_bar_position_and_initiation_17: # x:66 y:96 OperatableStateMachine.add('set question barman', SetKey(Value="Are you the bartender?"), transitions={'done': 'Action_FindPersonByQuestion'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'questionBarman'}) # x:488 y:85 OperatableStateMachine.add('say failed to find the barman', SaraSay(sentence="I am not able to find the barman. I will get the order and come back here to get the items.", input_keys=[], emotion=0, block=True), transitions={'done': 'barman ID to 0'}, autonomy={'done': Autonomy.Off}) # x:83 y:406 OperatableStateMachine.add('save barman ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'get current pose'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'personFound', 'output_value': 'barmanID'}) # x:512 y:183 OperatableStateMachine.add('barman ID to 0', SetKey(Value=0), transitions={'done': 'get current pose'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'barmanID'}) # x:488 y:391 OperatableStateMachine.add('get current pose', Get_Robot_Pose(), transitions={'done': 'set first command'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'barPosition'}) # x:55 y:214 OperatableStateMachine.add('Action_FindPersonByQuestion', self.use_behavior(Action_FindPersonByQuestionSM, 'save bar position and initiation/Action_FindPersonByQuestion'), transitions={'found': 'save barman ID', 'failed': 'say failed to find the barman', 'not_found': 'say failed to find the barman'}, autonomy={'found': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'not_found': Autonomy.Inherit}, remapping={'question': 'questionBarman', 'entityFound': 'personFound'}) # x:489 y:478 OperatableStateMachine.add('set first command', SetKey(Value=1), transitions={'done': 'say instructions'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'commandNumber'}) # x:689 y:477 OperatableStateMachine.add('say instructions', SaraSay(sentence="Good! I wil now start to serve people.", input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:30 y:40 OperatableStateMachine.add('save bar position and initiation', _sm_save_bar_position_and_initiation_17, transitions={'finished': 'detect people waving', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'barPosition': 'barPosition', 'commandNumber': 'commandNumber', 'barmanID': 'barmanID'}) # x:131 y:141 OperatableStateMachine.add('detect people waving', _sm_detect_people_waving_16, transitions={'finished': 'move to table and save position', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'customerPosition': 'customerPosition', 'customerID': 'customerID'}) # x:233 y:250 OperatableStateMachine.add('move to table and save position', _sm_move_to_table_and_save_position_15, transitions={'finished': 'ask and save order', 'failed': 'detect people waving'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'customerPosition': 'customerPosition', 'robotPositionToCustomer': 'robotPositionToCustomer'}) # x:286 y:337 OperatableStateMachine.add('ask and save order', _sm_ask_and_save_order_14, transitions={'finished': 'go to the barman', 'failed': 'detect people waving'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'customerID': 'customerID', 'orderList': 'orderList'}) # x:264 y:434 OperatableStateMachine.add('go to the barman', _sm_go_to_the_barman_13, transitions={'finished': 'get the order', 'failed': 'detect people waving'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'barPosition': 'barPosition', 'barmanID': 'barmanID'}) # x:278 y:526 OperatableStateMachine.add('get the order', _sm_get_the_order_12, transitions={'finished': 'take objects and bring the order to customer', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'orderList': 'orderList'}) # x:260 y:626 OperatableStateMachine.add('take objects and bring the order to customer', _sm_take_objects_and_bring_the_order_to_customer_11, transitions={'finished': 'repeate if first commande', 'failed': 'repeate if first commande'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'barPosition': 'barPosition', 'orderList': 'orderList', 'robotPositionToCustomer': 'robotPositionToCustomer'}) # x:118 y:735 OperatableStateMachine.add('repeate if first commande', _sm_repeate_if_first_commande_10, transitions={'finished': 'say finish', 'repeate': 'detect people waving'}, autonomy={'finished': Autonomy.Inherit, 'repeate': Autonomy.Inherit}, remapping={'commandNumber': 'commandNumber'}) # x:431 y:743 OperatableStateMachine.add('say finish', SaraSay(sentence="I reach the end of my scenario. I will go back to the bar.", input_keys=[], emotion=0, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:616 y:735 OperatableStateMachine.add('Action_Move', self.use_behavior(Action_MoveSM, 'Action_Move'), transitions={'finished': 'finished', 'failed': 'finished'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'barPosition'}) 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:317 y:570, x:757 y:138, x:589 y:443 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail']) # 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_turntofindanotherpersononce_0 = OperatableStateMachine( outcomes=['end', 'finished', 'failed']) with _sm_turntofindanotherpersononce_0: # x:32 y:135 OperatableStateMachine.add( 'sayNeverMind', SaraSay(sentence="OK. Never mind", input_keys=[], emotion=1, block=True), transitions={'done': 'loopFindOtherPerson'}, autonomy={'done': Autonomy.Off}) # x:156 y:151 OperatableStateMachine.add('loopFindOtherPerson', ForLoop(repeat=1), transitions={ 'do': 'SetRotation', 'end': 'end' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index3'}) # x:30 y:40 OperatableStateMachine.add( 'action_turn', self.use_behavior(action_turnSM, 'turnToFindAnotherPersonOnce/action_turn'), transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'rotation': 'rotation'}) # x:194 y:86 OperatableStateMachine.add('SetRotation', SetKey(Value=3.1416), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'rotation'}) # x:196 y:458, x:495 y:340 _sm_listenandstorethequestion_1 = OperatableStateMachine( outcomes=['done', 'end'], output_keys=['operatorQuestion']) with _sm_listenandstorethequestion_1: # x:30 y:51 OperatableStateMachine.add( 'sayAskQuestion', SaraSay(sentence="Ask me your question please.", input_keys=[], emotion=1, block=True), transitions={'done': 'getQuestion'}, autonomy={'done': Autonomy.Off}) # x:50 y:194 OperatableStateMachine.add('getQuestion', GetSpeech(watchdog=7), transitions={ 'done': 'done', 'nothing': 'looping2', 'fail': 'looping2' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'operatorQuestion'}) # x:250 y:262 OperatableStateMachine.add('looping2', ForLoop(repeat=1), transitions={ 'do': 'sayRepeat2', 'end': 'end' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index2'}) # x:273 y:94 OperatableStateMachine.add( 'sayRepeat2', SaraSay(sentence="I didn't understand. Can you repeat please?", input_keys=[], emotion=1, block=True), transitions={'done': 'getQuestion'}, autonomy={'done': Autonomy.Off}) # x:137 y:546, x:475 y:361, x:494 y:244 _sm_ifquestionforsara_2 = OperatableStateMachine( outcomes=['true', 'false', 'end']) with _sm_ifquestionforsara_2: # x:30 y:40 OperatableStateMachine.add( 'AskIfQuestion', SaraSay(sentence="Do you have a question for me?", input_keys=[], emotion=1, block=True), transitions={'done': 'getResponse'}, autonomy={'done': Autonomy.Off}) # x:23 y:108 OperatableStateMachine.add('getResponse', GetSpeech(watchdog=7), transitions={ 'done': 'ifYes', 'nothing': 'looping', 'fail': 'looping' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'response'}) # x:261 y:216 OperatableStateMachine.add('looping', ForLoop(repeat=1), transitions={ 'do': 'sayRepeat', 'end': 'end' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:254 y:63 OperatableStateMachine.add( 'sayRepeat', SaraSay(sentence= "I didn't understand. Do you have a question for me?", input_keys=[], emotion=1, block=True), transitions={'done': 'getResponse'}, autonomy={'done': Autonomy.Off}) # x:73 y:394 OperatableStateMachine.add( 'ifYes', CheckConditionState(predicate=lambda x: "yes" in x or "sure" in x or "of course" in x), transitions={ 'true': 'true', 'false': 'false' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'response'}) with _state_machine: # x:30 y:40 OperatableStateMachine.add( 'SetPersonClass', SetKey(Value="person"), transitions={'done': 'Action_findPerson'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'personClass'}) # x:20 y:117 OperatableStateMachine.add('Action_findPerson', self.use_behavior( Action_findPersonSM, 'Action_findPerson'), transitions={ 'done': 'IfQuestionForSARA', 'pas_done': 'noPerson' }, autonomy={ 'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit }, remapping={ 'className': 'personClass', 'entity': 'entity' }) # x:374 y:60 OperatableStateMachine.add( 'noPerson', SaraSay(sentence="I can't find any person here.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}) # x:332 y:298 OperatableStateMachine.add( 'NotUnderstandEnd', SaraSay(sentence="Sorry. I can't understand your answer.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause2'}, autonomy={'done': Autonomy.Off}) # x:39 y:384 OperatableStateMachine.add('SendToNLU', SaraNLUspr(), transitions={ 'understood': 'log answer', 'not_understood': 'CannotAnswer', 'fail': 'CannotAnswer' }, autonomy={ 'understood': Autonomy.Off, 'not_understood': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'sentence': 'operatorQuestion', 'answer': 'answer' }) # x:334 y:408 OperatableStateMachine.add( 'CannotAnswer', SaraSay(sentence="Sorry. I can't answer your question.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause3'}, autonomy={'done': Autonomy.Off}) # x:23 y:208 OperatableStateMachine.add('IfQuestionForSARA', _sm_ifquestionforsara_2, transitions={ 'true': 'ListenAndStoreTheQuestion', 'false': 'turnToFindAnotherPersonOnce', 'end': 'NotUnderstandEnd' }, autonomy={ 'true': Autonomy.Inherit, 'false': Autonomy.Inherit, 'end': Autonomy.Inherit }) # x:16 y:298 OperatableStateMachine.add( 'ListenAndStoreTheQuestion', _sm_listenandstorethequestion_1, transitions={ 'done': 'SendToNLU', 'end': 'NotUnderstandEnd' }, autonomy={ 'done': Autonomy.Inherit, 'end': Autonomy.Inherit }, remapping={'operatorQuestion': 'operatorQuestion'}) # x:299 y:169 OperatableStateMachine.add('turnToFindAnotherPersonOnce', _sm_turntofindanotherpersononce_0, transitions={ 'end': 'cause1', 'finished': 'Action_findPerson', 'failed': 'cause1' }, autonomy={ 'end': Autonomy.Inherit, 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:541 y:89 OperatableStateMachine.add( 'cause1', SetKey(Value="I did not find any person"), transitions={'done': 'SetRosParam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:523 y:253 OperatableStateMachine.add( 'cause2', SetKey(Value="I did not understand the question."), transitions={'done': 'SetRosParam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:515 y:350 OperatableStateMachine.add( 'cause3', SetKey(Value="I was unable to answer the question."), transitions={'done': 'SetRosParam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:630 y:196 OperatableStateMachine.add( 'SetRosParam', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:40 y:469 OperatableStateMachine.add('log answer', LogKeyState( text="{}", severity=Logger.REPORT_HINT), transitions={'done': 'Say_Answer'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'answer'}) # x:42 y:550 OperatableStateMachine.add('Say_Answer', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:702 y:576, x:764 y:158, x:766 y:33 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Place", "table"] _state_machine.userdata.Empty = None _state_machine.userdata.IdlePos = "IdlePose" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:44 y:28 OperatableStateMachine.add( 'gripper contain', GetRosParam(ParamName="behavior/GripperContent"), transitions={ 'done': 'if contain something', 'failed': 'cause1' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'content'}) # x:222 y:497 OperatableStateMachine.add('Action_place', self.use_behavior( Action_placeSM, 'Action_place'), transitions={ 'finished': 'idlearm', 'failed': 'cause3' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pos': 'MapPosition'}) # x:39 y:367 OperatableStateMachine.add( 'genPoseArm', GenPoseEuler(x=0.75, y=-0.25, z=0.85, roll=0, pitch=0, yaw=0), transitions={'done': 'referential from robot to map'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'position'}) # x:8 y:433 OperatableStateMachine.add('referential from robot to map', TF_transformation(in_ref="base_link", out_ref="map"), transitions={ 'done': 'log pose', 'fail': 'log tf error' }, autonomy={ 'done': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'in_pos': 'position', 'out_pos': 'MapPosition' }) # x:25 y:98 OperatableStateMachine.add( 'if contain something', CheckConditionState(predicate=lambda x: x != ''), transitions={ 'true': 'cond', 'false': 'say nothing in gripper' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'content'}) # x:209 y:98 OperatableStateMachine.add( 'say nothing in gripper', SaraSay(sentence="It seems I have nothing in my gripper", input_keys=[], emotion=1, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}) # x:28 y:236 OperatableStateMachine.add( 'construction phrase', FlexibleCalculationState( calculation=lambda x: "I will place this " + str(x[ 0]) + " on the " + str(x[1][1]), input_keys=["content", "Action"]), transitions={'done': 'Say_Place_object'}, autonomy={'done': Autonomy.Off}, remapping={ 'content': 'content', 'Action': 'Action', 'output_value': 'sentence' }) # x:33 y:167 OperatableStateMachine.add( 'cond', CheckConditionState(predicate=lambda x: x[1] != ''), transitions={ 'true': 'construction phrase', 'false': 'cause2' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'Action'}) # x:257 y:413 OperatableStateMachine.add('log tf error', LogState(text="tf error", severity=Logger.REPORT_HINT), transitions={'done': 'cause3'}, autonomy={'done': Autonomy.Off}) # x:42 y:502 OperatableStateMachine.add( 'log pose', LogKeyState(text="the placement pose will be: {}", severity=Logger.REPORT_HINT), transitions={'done': 'Action_place'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'MapPosition'}) # x:493 y:535 OperatableStateMachine.add( 'empty hand', SetRosParam(ParamName="behavior/GripperContent"), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Empty'}) # x:448 y:54 OperatableStateMachine.add( 'cause1', SetKey(Value="I didn't have any object in my gripper"), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:422 y:149 OperatableStateMachine.add( 'cause2', SetKey(Value="I didn't know where to place the object."), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:575 y:158 OperatableStateMachine.add( 'setrosparamcause', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:449 y:325 OperatableStateMachine.add( 'cause3', SetKey( Value="I was unable to calculate how to place the object." ), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:342 y:583 OperatableStateMachine.add('idlearm', MoveitMove(move=True, waitForExecution=False, group="RightArm", watchdog=15), transitions={ 'done': 'empty hand', 'failed': 'empty hand' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'IdlePos'}) # x:706 y:460 OperatableStateMachine.add( 'Say_Place_It_This_Place', SaraSay(sentence=lambda x: "I will place this " + x + " right there.", input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:35 y:301 OperatableStateMachine.add('Say_Place_object', SaraSay(sentence=lambda x: x, input_keys=[], emotion=0, block=True), transitions={'done': 'genPoseArm'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:732 y:366, x:29 y:248 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['name']) _state_machine.userdata.name = "cupboard" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:781 y:57, x:292 y:451 _sm_openit_0 = OperatableStateMachine(outcomes=['finished', 'failed']) with _sm_openit_0: # x:252 y:38 OperatableStateMachine.add('init tryal', SetKey(Value=3), transitions={'done': 'movearm'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'try'}) # x:451 y:168 OperatableStateMachine.add('isOpened', CupboardDoorDetector(timeout=30), transitions={ 'open': 'yay', 'closed': 'subtryal' }, autonomy={ 'open': Autonomy.Off, 'closed': Autonomy.Off }) # x:464 y:305 OperatableStateMachine.add( 'subtryal', CalculationState(calculation=lambda x: x - 1), transitions={'done': 'is ok'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'try', 'output_value': 'try' }) # x:47 y:300 OperatableStateMachine.add( 'is ok', CheckConditionState(predicate=lambda x: x > 0), transitions={ 'true': 'saytryagain', 'false': 'say cant' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'try'}) # x:53 y:179 OperatableStateMachine.add('saytryagain', SaraSay(sentence="Let's try again!", input_keys=[], emotion=1, block=True), transitions={'done': 'movearm'}, autonomy={'done': Autonomy.Off}) # x:44 y:438 OperatableStateMachine.add('say cant', SaraSay(sentence="I can't open it.", input_keys=[], emotion=2, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:455 y:40 OperatableStateMachine.add('yay', SaraSay(sentence="Yay, I did it!", input_keys=[], emotion=1, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:247 y:175 OperatableStateMachine.add('movearm', RunTrajectory(file="OuvrePorte3", duration=0), transitions={'done': 'isOpened'}, autonomy={'done': Autonomy.Off}) with _state_machine: # x:310 y:31 OperatableStateMachine.add('Action_Move', self.use_behavior( Action_MoveSM, 'Action_Move'), transitions={ 'finished': 'reach', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'name'}) # x:326 y:351 OperatableStateMachine.add( 'sayWillOpen', SaraSay(sentence="The door is closed. I will open it.", input_keys=[], emotion=1, block=True), transitions={'done': 'OpenIt'}, autonomy={'done': Autonomy.Off}) # x:320 y:229 OperatableStateMachine.add('checkdoor', CupboardDoorDetector(timeout=5), transitions={ 'open': 'finished', 'closed': 'sayWillOpen' }, autonomy={ 'open': Autonomy.Off, 'closed': Autonomy.Off }) # x:338 y:126 OperatableStateMachine.add('reach', SaraSay( sentence="I reached the cupboard!", input_keys=[], emotion=1, block=True), transitions={'done': 'checkdoor'}, autonomy={'done': Autonomy.Off}) # x:320 y:485 OperatableStateMachine.add('OpenIt', _sm_openit_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) return _state_machine
def create(self): # x:976 y:173, x:618 y:48 _state_machine = OperatableStateMachine(outcomes=['done', 'failed']) _state_machine.userdata.nameFilter = "" _state_machine.userdata.roomQuestion = "what room shall i clean, master ?" _state_machine.userdata.waypointGenerationDistance = 0.5 _state_machine.userdata.placeholder = "" _state_machine.userdata.doorName = "crowd" _state_machine.userdata.firstTimeInRoom = "yes" _state_machine.userdata.waypointToCheckDict = {"bedroom": ["bedroomWP1","bedroomWP2"]} _state_machine.userdata.placedObjects = 0 _state_machine.userdata.misplacedObject = "" _state_machine.userdata.skipDoorEntrance = True # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:153 y:437 _sm_deusexplacerecovery_0 = OperatableStateMachine(outcomes=['finished'], input_keys=['misplacedObject']) with _sm_deusexplacerecovery_0: # x:85 y:72 OperatableStateMachine.add('getContainerId', CalculationState(calculation=lambda x: x[1].containerId), transitions={'done': 'getContainerEntity'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'misplacedObject', 'output_value': 'containerId'}) # x:234 y:149 OperatableStateMachine.add('getContainerEntity', WonderlandGetEntityByID(), transitions={'found': 'ItGoesThere', 'not_found': 'finished', 'error': 'finished'}, autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off, 'error': Autonomy.Off}, remapping={'id': 'containerId', 'entity': 'containerEntity', 'depth_position': 'depth_position', 'depth_waypoint': 'depth_waypoint'}) # x:366 y:284 OperatableStateMachine.add('ItGoesThere', SaraSay(sentence=lambda x: "Looks like I dropped "+str(x[0][0].name)+". Can you put it in the "+ x[1].name+"?", input_keys=["misplacedObject","containerEntity"], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'misplacedObject': 'misplacedObject', 'containerEntity': 'containerEntity'}) # x:717 y:285 _sm_cantgotodestination_1 = OperatableStateMachine(outcomes=['finished'], input_keys=['misplacedObject']) with _sm_cantgotodestination_1: # x:48 y:60 OperatableStateMachine.add('getContainerId', CalculationState(calculation=lambda x: x[1].containerId), transitions={'done': 'getContainerEntity'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'misplacedObject', 'output_value': 'containerId'}) # x:463 y:245 OperatableStateMachine.add('Action_place', self.use_behavior(sara_flexbe_behaviors__Action_placeSM, 'PutObjectInDesiredContainer/CantGoToDestination/Action_place'), transitions={'finished': 'finished', 'failed': 'finished'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pos': 'droppingPose'}) # x:26 y:149 OperatableStateMachine.add('getContainerEntity', WonderlandGetEntityByID(), transitions={'found': 'ICantGetThere', 'not_found': 'ICantGetThereNCONT', 'error': 'ICantGetThereNCONT'}, autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off, 'error': Autonomy.Off}, remapping={'id': 'containerId', 'entity': 'containerEntity', 'depth_position': 'depth_position', 'depth_waypoint': 'depth_waypoint'}) # x:249 y:176 OperatableStateMachine.add('ICantGetThere', SaraSay(sentence=lambda x:"I can't find a way to "+str(x[0].name), input_keys=["containerEntity"], emotion=0, block=True), transitions={'done': 'GetDroppingPose'}, autonomy={'done': Autonomy.Off}, remapping={'containerEntity': 'containerEntity'}) # x:247 y:114 OperatableStateMachine.add('ICantGetThereNCONT', SaraSay(sentence="I can't find a way to the container", input_keys=[], emotion=0, block=True), transitions={'done': 'GetDroppingPose'}, autonomy={'done': Autonomy.Off}) # x:475 y:148 OperatableStateMachine.add('GetDroppingPose', GenPoseEuler(x=0.4, y=0, z=0.3, roll=0.0, pitch=0.0, yaw=0.0), transitions={'done': 'Action_place'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'droppingPose'}) # x:632 y:527 _sm_putdownobject_2 = OperatableStateMachine(outcomes=['done'], input_keys=['misplacedObject']) with _sm_putdownobject_2: # x:70 y:215 OperatableStateMachine.add('GetDroppingPose', GenPoseEuler(x=0.3, y=0, z=1.2, roll=0.0, pitch=0.0, yaw=0.0), transitions={'done': 'Action_place'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'droppingPose'}) # x:269 y:215 OperatableStateMachine.add('Action_place', self.use_behavior(sara_flexbe_behaviors__Action_placeSM, 'PutObjectInDesiredContainer/PutDownObject/Action_place'), transitions={'finished': 'done', 'failed': 'DeusExPlaceRecovery'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pos': 'droppingPose'}) # x:592 y:127 OperatableStateMachine.add('DeusExPlaceRecovery', _sm_deusexplacerecovery_0, transitions={'finished': 'done'}, autonomy={'finished': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject'}) # x:518 y:44, x:531 y:152 _sm_gotodesiredcontainer_3 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['misplacedObject']) with _sm_gotodesiredcontainer_3: # x:86 y:79 OperatableStateMachine.add('GetContainerWaypoint', CalculationState(calculation=lambda x:x[1].waypoint), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'misplacedObject', 'output_value': 'targetWaypoint'}) # x:275 y:79 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'PutObjectInDesiredContainer/GotoDesiredContainer/Action_Move'), transitions={'finished': 'done', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'targetWaypoint'}) # x:300 y:141, x:893 y:63 _sm_receiveitem_4 = OperatableStateMachine(outcomes=['failed', 'success']) with _sm_receiveitem_4: # x:69 y:47 OperatableStateMachine.add('opengripper', SetGripperState(width=0.25, effort=1), transitions={'object': 'setTarget1', 'no_object': 'setTarget1'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:280 y:263 OperatableStateMachine.add('Torque_Reader', ReadTorque(watchdog=10, Joint="right_elbow_pitch_joint", Threshold=1, min_time=1), transitions={'threshold': 'close_gripper', 'watchdog': 'Torque_Reader', 'fail': 'failed'}, autonomy={'threshold': Autonomy.Off, 'watchdog': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'torque': 'torque'}) # x:668 y:149 OperatableStateMachine.add('setTarget2', SetKey(Value="PostGripPose"), transitions={'done': 'Go_to_Pose'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:51 y:261 OperatableStateMachine.add('Go_to_receive_bag_pose', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={'done': 'Torque_Reader', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'target': 'target'}) # x:450 y:274 OperatableStateMachine.add('close_gripper', SetGripperState(width=0, effort=1), transitions={'object': 'thank you', 'no_object': 'failed'}, autonomy={'object': Autonomy.Off, 'no_object': Autonomy.Off}, remapping={'object_size': 'object_size'}) # x:640 y:273 OperatableStateMachine.add('thank you', SaraSay(sentence="Thank you", input_keys=[], emotion=1, block=True), transitions={'done': 'setTarget2'}, autonomy={'done': Autonomy.Off}) # x:50 y:141 OperatableStateMachine.add('setTarget1', SetKey(Value="Help_me_carry"), transitions={'done': 'Go_to_receive_bag_pose'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'target'}) # x:660 y:57 OperatableStateMachine.add('Go_to_Pose', MoveitMove(move=True, waitForExecution=True, group="RightArm"), transitions={'done': 'success', 'failed': 'failed'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'target': 'target'}) # x:169 y:514, x:508 y:495 _sm_deusexpickrecovery_5 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['misplacedObject']) with _sm_deusexpickrecovery_5: # x:81 y:50 OperatableStateMachine.add('GimmeTheObjict', SaraSay(sentence=lambda x: "Could you give me the "+x[0][0].name+", please ?", input_keys=["misplacedObject"], emotion=0, block=True), transitions={'done': 'ReceiveItem'}, autonomy={'done': Autonomy.Off}, remapping={'misplacedObject': 'misplacedObject'}) # x:213 y:130 OperatableStateMachine.add('ReceiveItem', _sm_receiveitem_4, transitions={'failed': 'Oh f**k', 'success': 'finished'}, autonomy={'failed': Autonomy.Inherit, 'success': Autonomy.Inherit}) # x:355 y:262 OperatableStateMachine.add('Oh f**k', SaraSay(sentence="Oh. Nevermind then.", input_keys=[], emotion=2, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:676 y:610, x:1158 y:270 _sm_grabmisplacedobject_6 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['misplacedObject']) with _sm_grabmisplacedobject_6: # x:72 y:35 OperatableStateMachine.add('GetObjectId', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'Action_pick'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'misplacedObject', 'output_value': 'misplacedObjectID'}) # x:307 y:89 OperatableStateMachine.add('oops', SaraSay(sentence="Oops. I dropped the object!", input_keys=[], emotion=5, block=True), transitions={'done': 'DeusExPickRecovery'}, autonomy={'done': Autonomy.Off}) # x:344 y:157 OperatableStateMachine.add('oops_2', SaraSay(sentence="Oops. Seems I can't reach the object!", input_keys=[], emotion=3, block=True), transitions={'done': 'DeusExPickRecovery'}, autonomy={'done': Autonomy.Off}) # x:362 y:239 OperatableStateMachine.add('oops_3', SaraSay(sentence="Oops. I can't see the object!", input_keys=[], emotion=3, block=True), transitions={'done': 'DeusExPickRecovery'}, autonomy={'done': Autonomy.Off}) # x:42 y:275 OperatableStateMachine.add('Action_pick', self.use_behavior(sara_flexbe_behaviors__Action_pickSM, 'PickMisplacedObject/GrabMisplacedObject/Action_pick', default_keys=['Entity']), transitions={'success': 'done', 'unreachable': 'oops_2', 'not found': 'oops_3', 'dropped': 'oops'}, autonomy={'success': Autonomy.Inherit, 'unreachable': Autonomy.Inherit, 'not found': Autonomy.Inherit, 'dropped': Autonomy.Inherit}, remapping={'objectID': 'misplacedObjectID', 'Entity': 'Entity'}) # x:600 y:177 OperatableStateMachine.add('DeusExPickRecovery', _sm_deusexpickrecovery_5, transitions={'finished': 'done', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject'}) # x:30 y:426 _sm_scanaround_7 = OperatableStateMachine(outcomes=['done']) with _sm_scanaround_7: # x:172 y:46 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:304 y:122 OperatableStateMachine.add('right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:165 y:187 OperatableStateMachine.add('center2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w4'}, autonomy={'done': Autonomy.Off}) # x:195 y:276 OperatableStateMachine.add('w1', WaitState(wait_time=1), transitions={'done': 'center3'}, autonomy={'done': Autonomy.Off}) # x:309 y:49 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'right'}, autonomy={'done': Autonomy.Off}) # x:308 y:187 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'center2'}, autonomy={'done': Autonomy.Off}) # x:56 y:191 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'left'}, autonomy={'done': Autonomy.Off}) # x:40 y:270 OperatableStateMachine.add('left', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:317 y:277 OperatableStateMachine.add('center3', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:289 y:803, x:694 y:198, x:4 y:400 _sm_checkatwaypoints_8 = OperatableStateMachine(outcomes=['found', 'noneFound', 'failed'], input_keys=['waypointToCheckDict', 'cleaningRoom', 'nameFilter'], output_keys=['misplacedObject']) with _sm_checkatwaypoints_8: # x:60 y:44 OperatableStateMachine.add('GetNbOfWaypoints', FlexibleCalculationState(calculation=lambda x: len(x[1][x[0]]), input_keys=["cleaningRoom","waypointToCheckDict"]), transitions={'done': 'StartLoop'}, autonomy={'done': Autonomy.Off}, remapping={'cleaningRoom': 'cleaningRoom', 'waypointToCheckDict': 'waypointToCheckDict', 'output_value': 'lenWaypointDict'}) # x:137 y:471 OperatableStateMachine.add('ScanAround', _sm_scanaround_7, transitions={'done': 'StopCheckingForUnknownObj'}, autonomy={'done': Autonomy.Inherit}) # x:163 y:729 OperatableStateMachine.add('GetFirstElement', CalculationState(calculation=lambda x:x[0]), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity_list', 'output_value': 'misplacedObject'}) # x:354 y:538 OperatableStateMachine.add('ListEntities', list_entities_by_name(frontality_level=0.5, distance_max=10), transitions={'found': 'CheckMisplacedObjects', 'none_found': 'CheckAtEachLocation'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'nameFilter', 'entity_list': 'entity_list', 'number': 'number'}) # x:23 y:198 OperatableStateMachine.add('EverythingsFine', SaraSay(sentence="I'll check at another location.", input_keys=[], emotion=1, block=True), transitions={'done': 'GetLocation'}, autonomy={'done': Autonomy.Off}) # x:184 y:289 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'CheckForMisplacedObjects/CheckAtWaypoints/Action_Move'), transitions={'finished': 'CheckForUnknownObjs', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'waypointToGo'}) # x:511 y:338 OperatableStateMachine.add('CheckAtEachLocation', ForLoopWithInput(repeat=10), transitions={'do': 'CheckIfLastLocationOfList', 'end': 'noneFound'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index_in': 'index_in', 'index_out': 'locationToGet'}) # x:318 y:58 OperatableStateMachine.add('StartLoop', SetKey(Value=-1), transitions={'done': 'CheckAtEachLocation'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'index_in'}) # x:9 y:262 OperatableStateMachine.add('GetLocation', FlexibleCalculationState(calculation=lambda x: x[1][x[2]][x[0]], input_keys=["index_out","waypointToCheckDict","cleaningRoom"]), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'index_out': 'locationToGet', 'waypointToCheckDict': 'waypointToCheckDict', 'cleaningRoom': 'cleaningRoom', 'output_value': 'waypointToGo'}) # x:173 y:195 OperatableStateMachine.add('CheckIfLastLocationOfList', FlexibleCheckConditionState(predicate=lambda x: x[0]-1 >= x[1], input_keys=["lenWaypointDict","locationToGet"]), transitions={'true': 'noneFound', 'false': 'EverythingsFine'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'lenWaypointDict': 'lenWaypointDict', 'locationToGet': 'locationToGet'}) # x:418 y:665 OperatableStateMachine.add('CheckMisplacedObjects', CheckMisplacedObjects(position_tolerance=0.1, default_destination="bin"), transitions={'all_expected': 'CheckAtEachLocation', 'unexpected': 'GetFirstElement'}, autonomy={'all_expected': Autonomy.Off, 'unexpected': Autonomy.Off}, remapping={'entities': 'entity_list', 'expected_objects': 'expected_objects', 'unexpected_objects': 'unexpected_objects'}) # x:119 y:402 OperatableStateMachine.add('CheckForUnknownObjs', SetSegmentationRosParam(ValueTableSegmentation=True, ValueObjectSegmentation=True), transitions={'done': 'ScanAround'}, autonomy={'done': Autonomy.Off}) # x:114 y:561 OperatableStateMachine.add('StopCheckingForUnknownObj', SetSegmentationRosParam(ValueTableSegmentation=False, ValueObjectSegmentation=False), transitions={'done': 'ListEntities'}, autonomy={'done': Autonomy.Off}) # x:547 y:290 _sm_scanaround_9 = OperatableStateMachine(outcomes=['done'], input_keys=['input_value']) with _sm_scanaround_9: # x:172 y:46 OperatableStateMachine.add('center', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w2'}, autonomy={'done': Autonomy.Off}) # x:304 y:122 OperatableStateMachine.add('right', SaraSetHeadAngle(pitch=0.1, yaw=-1.5), transitions={'done': 'w3'}, autonomy={'done': Autonomy.Off}) # x:165 y:187 OperatableStateMachine.add('center2', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'w4'}, autonomy={'done': Autonomy.Off}) # x:195 y:276 OperatableStateMachine.add('w1', WaitState(wait_time=1), transitions={'done': 'center3'}, autonomy={'done': Autonomy.Off}) # x:367 y:47 OperatableStateMachine.add('w2', WaitState(wait_time=4), transitions={'done': 'right'}, autonomy={'done': Autonomy.Off}) # x:308 y:187 OperatableStateMachine.add('w3', WaitState(wait_time=4), transitions={'done': 'center2'}, autonomy={'done': Autonomy.Off}) # x:56 y:191 OperatableStateMachine.add('w4', WaitState(wait_time=4), transitions={'done': 'left'}, autonomy={'done': Autonomy.Off}) # x:40 y:270 OperatableStateMachine.add('left', SaraSetHeadAngle(pitch=0.1, yaw=1.5), transitions={'done': 'w1'}, autonomy={'done': Autonomy.Off}) # x:317 y:277 OperatableStateMachine.add('center3', SaraSetHeadAngle(pitch=0.1, yaw=0), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:293 y:350, x:857 y:186 _sm_ask_10 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['roomQuestion'], output_keys=['roomAnswer']) with _sm_ask_10: # x:79 y:95 OperatableStateMachine.add('say question', SaraSay(sentence=lambda x: x[0], input_keys=["question"], emotion=0, block=True), transitions={'done': 'get answer'}, autonomy={'done': Autonomy.Off}, remapping={'question': 'roomQuestion'}) # x:92 y:304 OperatableStateMachine.add('get answer', GetSpeech(watchdog=10), transitions={'done': 'finished', 'nothing': 'retry ask', 'fail': 'retry ask'}, autonomy={'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'words': 'roomAnswer'}) # x:345 y:202 OperatableStateMachine.add('retry ask', ForLoop(repeat=2), transitions={'do': 'say not understand', 'end': 'say failed'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:232 y:90 OperatableStateMachine.add('say not understand', SaraSay(sentence="Sorry, I did not understand your answer.", input_keys=[], emotion=0, block=True), transitions={'done': 'say question'}, autonomy={'done': Autonomy.Off}) # x:604 y:151 OperatableStateMachine.add('say failed', SaraSay(sentence="Sorry, I can't understand your answer.", input_keys=[], emotion=0, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:30 y:365, x:424 y:392 _sm_nlu_11 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['roomAnswer'], output_keys=['cleanupRoom']) with _sm_nlu_11: # x:212 y:89 OperatableStateMachine.add('NLU', SaraNLUgetRoom(), transitions={'understood': 'done', 'not_understood': 'failed', 'fail': 'failed'}, autonomy={'understood': Autonomy.Off, 'not_understood': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'sentence': 'roomAnswer', 'answer': 'cleanupRoom'}) # x:736 y:312 _sm_putobjectindesiredcontainer_12 = OperatableStateMachine(outcomes=['finished'], input_keys=['misplacedObject', 'placedObjects'], output_keys=['placedObjects']) with _sm_putobjectindesiredcontainer_12: # x:138 y:153 OperatableStateMachine.add('GotoDesiredContainer', _sm_gotodesiredcontainer_3, transitions={'done': 'PutDownObject', 'failed': 'CantGoToDestination'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject'}) # x:277 y:313 OperatableStateMachine.add('PutDownObject', _sm_putdownobject_2, transitions={'done': 'OneMoreObjectPlaced'}, autonomy={'done': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject'}) # x:339 y:80 OperatableStateMachine.add('CantGoToDestination', _sm_cantgotodestination_1, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject'}) # x:468 y:381 OperatableStateMachine.add('OneMoreObjectPlaced', CalculationState(calculation=lambda x: x+1), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'placedObjects', 'output_value': 'placedObjects'}) # x:609 y:528, x:593 y:240 _sm_pickmisplacedobject_13 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['misplacedObject', 'waypointGenerationDistance']) with _sm_pickmisplacedobject_13: # x:82 y:71 OperatableStateMachine.add('GetObjectPosition', CalculationState(calculation=lambda x: x[0].position), transitions={'done': 'FindAWay'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'misplacedObject', 'output_value': 'misplacedObjectPosition'}) # x:67 y:304 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'PickMisplacedObject/Action_Move'), transitions={'finished': 'GrabMisplacedObject', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'misplacedObjectWaypoint'}) # x:82 y:202 OperatableStateMachine.add('FindAWay', Get_Reacheable_Waypoint(), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'pose_in': 'misplacedObjectPosition', 'distance': 'waypointGenerationDistance', 'pose_out': 'misplacedObjectWaypoint'}) # x:58 y:514 OperatableStateMachine.add('GrabMisplacedObject', _sm_grabmisplacedobject_6, transitions={'done': 'finished', 'failed': 'failed'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject'}) # x:957 y:154, x:535 y:369, x:800 y:304 _sm_checkformisplacedobjects_14 = OperatableStateMachine(outcomes=['noneLeft', 'found', 'failed'], input_keys=['nameFilter', 'waypointToCheckDict', 'cleaningRoom', 'placedObjects'], output_keys=['misplacedObject']) with _sm_checkformisplacedobjects_14: # x:241 y:59 OperatableStateMachine.add('CheckIf5Placed', CheckConditionState(predicate=lambda x: x >= 5), transitions={'true': 'OkItsClean', 'false': 'RetryOnce'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'placedObjects'}) # x:115 y:477 OperatableStateMachine.add('CheckMisplacedObjects', CheckMisplacedObjects(position_tolerance=0.1, default_destination="bin"), transitions={'all_expected': 'EverythingsFine', 'unexpected': 'GetFirstElement'}, autonomy={'all_expected': Autonomy.Off, 'unexpected': Autonomy.Off}, remapping={'entities': 'entity_list', 'expected_objects': 'expected_objects', 'unexpected_objects': 'unexpected_objects'}) # x:76 y:251 OperatableStateMachine.add('ScanAround', _sm_scanaround_9, transitions={'done': 'ListEntities'}, autonomy={'done': Autonomy.Inherit}, remapping={'input_value': 'nameFilter'}) # x:300 y:486 OperatableStateMachine.add('GetFirstElement', CalculationState(calculation=lambda x:x[0]), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity_list', 'output_value': 'misplacedObject'}) # x:77 y:360 OperatableStateMachine.add('ListEntities', list_entities_by_name(frontality_level=0.5, distance_max=10), transitions={'found': 'CheckMisplacedObjects', 'none_found': 'RetryOnce'}, autonomy={'found': Autonomy.Off, 'none_found': Autonomy.Off}, remapping={'name': 'nameFilter', 'entity_list': 'entity_list', 'number': 'number'}) # x:305 y:355 OperatableStateMachine.add('EverythingsFine', SaraSay(sentence="Everything seems in order. I'll check again.", input_keys=[], emotion=1, block=True), transitions={'done': 'RetryOnce'}, autonomy={'done': Autonomy.Off}) # x:738 y:175 OperatableStateMachine.add('OkItsClean', SaraSay(sentence="I cleaned the room!", input_keys=[], emotion=1, block=True), transitions={'done': 'noneLeft'}, autonomy={'done': Autonomy.Off}) # x:488 y:226 OperatableStateMachine.add('CheckAtWaypoints', _sm_checkatwaypoints_8, transitions={'found': 'found', 'noneFound': 'OkItsClean', 'failed': 'failed'}, autonomy={'found': Autonomy.Inherit, 'noneFound': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'waypointToCheckDict': 'waypointToCheckDict', 'cleaningRoom': 'cleaningRoom', 'nameFilter': 'nameFilter', 'misplacedObject': 'misplacedObject'}) # x:268 y:226 OperatableStateMachine.add('RetryOnce', ForLoop(repeat=1), transitions={'do': 'ScanAround', 'end': 'CheckAtWaypoints'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:644 y:459, x:651 y:336 _sm_gotoroom_15 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['cleanupRoom', 'firstTimeInRoom'], output_keys=['firstTimeInRoom']) with _sm_gotoroom_15: # x:141 y:121 OperatableStateMachine.add('IsItMyFirstTime', DecisionState(outcomes=["no", "yes"], conditions=lambda x: x), transitions={'no': 'GoingBackToTheRoom', 'yes': 'OkImGoingToTheRoom'}, autonomy={'no': Autonomy.Off, 'yes': Autonomy.Off}, remapping={'input_value': 'firstTimeInRoom'}) # x:130 y:220 OperatableStateMachine.add('GoingBackToTheRoom', SaraSay(sentence=lambda x: "I'm going back to the "+str(x[0])+".", input_keys=["cleanupRoom"], emotion=0, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'cleanupRoom': 'cleanupRoom'}) # x:342 y:171 OperatableStateMachine.add('OkImGoingToTheRoom', SaraSay(sentence=lambda x: "Okay, I'm going to the "+str(x[0])+".", input_keys=["cleanupRoom"], emotion=0, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}, remapping={'cleanupRoom': 'cleanupRoom'}) # x:330 y:448 OperatableStateMachine.add('IWentThere', SetKey(Value="no"), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'firstTimeInRoom'}) # x:320 y:326 OperatableStateMachine.add('Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'GoToRoom/Action_Move'), transitions={'finished': 'IWentThere', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'pose': 'cleanupRoom'}) # x:709 y:520, x:850 y:66 _sm_enterarena_16 = OperatableStateMachine(outcomes=['done', 'failed'], input_keys=['roomQuestion', 'doorName', 'skipDoorEntrance'], output_keys=['cleanupRoom']) with _sm_enterarena_16: # x:77 y:28 OperatableStateMachine.add('Init_Sequence', self.use_behavior(sara_flexbe_behaviors__Init_SequenceSM, 'EnterArena/Init_Sequence'), transitions={'finished': 'SkipEntrance', 'failed': 'failed'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}) # x:278 y:473 OperatableStateMachine.add('NLU', _sm_nlu_11, transitions={'done': 'done', 'failed': 'Sorry'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'roomAnswer': 'roomAnswer', 'cleanupRoom': 'cleanupRoom'}) # x:467 y:87 OperatableStateMachine.add('Action_Pass_Door', self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM, 'EnterArena/Action_Pass_Door'), transitions={'Done': 'RetryOnce', 'Fail': 'failed'}, autonomy={'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit}, remapping={'DoorName': 'doorName'}) # x:234 y:259 OperatableStateMachine.add('RetryOnce', ForLoop(repeat=2), transitions={'do': 'Ask', 'end': 'failed'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:295 y:354 OperatableStateMachine.add('Sorry', SaraSay(sentence="Sorry, I misunderstood.", input_keys=[], emotion=0, block=True), transitions={'done': 'RetryOnce'}, autonomy={'done': Autonomy.Off}) # x:213 y:119 OperatableStateMachine.add('SkipEntrance', CheckConditionState(predicate=lambda x: x), transitions={'true': 'RetryOnce', 'false': 'Action_Pass_Door'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'skipDoorEntrance'}) # x:51 y:354 OperatableStateMachine.add('Ask', _sm_ask_10, transitions={'finished': 'NLU', 'failed': 'RetryOnce'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'roomQuestion': 'roomQuestion', 'roomAnswer': 'roomAnswer'}) with _state_machine: # x:131 y:130 OperatableStateMachine.add('EnterArena', _sm_enterarena_16, transitions={'done': 'GoToRoom', 'failed': 'failed'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'roomQuestion': 'roomQuestion', 'doorName': 'doorName', 'skipDoorEntrance': 'skipDoorEntrance', 'cleanupRoom': 'cleanupRoom'}) # x:286 y:217 OperatableStateMachine.add('GoToRoom', _sm_gotoroom_15, transitions={'done': 'CheckForMisplacedObjects', 'failed': 'failed'}, autonomy={'done': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'cleanupRoom': 'cleanupRoom', 'firstTimeInRoom': 'firstTimeInRoom'}) # x:652 y:203 OperatableStateMachine.add('CheckForMisplacedObjects', _sm_checkformisplacedobjects_14, transitions={'noneLeft': 'done', 'found': 'PickMisplacedObject', 'failed': 'failed'}, autonomy={'noneLeft': Autonomy.Inherit, 'found': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'nameFilter': 'nameFilter', 'waypointToCheckDict': 'waypointToCheckDict', 'cleaningRoom': 'cleanupRoom', 'placedObjects': 'placedObjects', 'misplacedObject': 'misplacedObject'}) # x:557 y:403 OperatableStateMachine.add('PickMisplacedObject', _sm_pickmisplacedobject_13, transitions={'finished': 'PutObjectInDesiredContainer', 'failed': 'GoToRoom'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject', 'waypointGenerationDistance': 'waypointGenerationDistance'}) # x:234 y:476 OperatableStateMachine.add('PutObjectInDesiredContainer', _sm_putobjectindesiredcontainer_12, transitions={'finished': 'GoToRoom'}, autonomy={'finished': Autonomy.Inherit}, remapping={'misplacedObject': 'misplacedObject', 'placedObjects': 'placedObjects'}) return _state_machine
def create(self): # x:30 y:365, x:242 y:161 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:241 y:23 OperatableStateMachine.add('log', LogState( text="logged to get started :-) ", severity=Logger.REPORT_HINT), transitions={'done': 'listennow'}, autonomy={'done': Autonomy.Off}) # x:936 y:282 OperatableStateMachine.add('wait4', WaitState(wait_time=3), transitions={'done': 'sub_is_listen2'}, autonomy={'done': Autonomy.Off}) # x:913 y:34 OperatableStateMachine.add('sub_is_listening', SubscriberState( topic='/is_robot_listening', blocking=True, clear=False), transitions={ 'received': 'check listen or not', 'unavailable': 'check listen or not' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:1122 y:153 OperatableStateMachine.add( 'check listen or not', CheckConditionState( predicate=lambda message: message.data == "not listening"), transitions={ 'true': 'listen2', 'false': 'sub_is_listening' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'message'}) # x:709 y:49 OperatableStateMachine.add( 'wait2', WaitState(wait_time=1), transitions={'done': 'sub_is_listening'}, autonomy={'done': Autonomy.Off}) # x:522 y:205 OperatableStateMachine.add( 'chk', CheckConditionState( predicate=lambda message: message.data == "not listening"), transitions={ 'true': 'fin', 'false': 'sub_is_listen2' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'message'}) # x:328 y:237 OperatableStateMachine.add('fin', LogState(text="finished interaction", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:460 y:24 OperatableStateMachine.add('listennow', ListeningState(), transitions={ 'continue': 'wait2', 'failed': 'wait2' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }) # x:1048 y:421 OperatableStateMachine.add('listen2', ListeningState(), transitions={ 'continue': 'wait4', 'failed': 'wait4' }, autonomy={ 'continue': Autonomy.Off, 'failed': Autonomy.Off }) # x:730 y:302 OperatableStateMachine.add('sub_is_listen2', SubscriberState( topic='/is_robot_listening', blocking=True, clear=False), transitions={ 'received': 'chk', 'unavailable': 'chk' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) return _state_machine
def create(self): # x:763 y:423, x:866 y:181, x:896 y:268 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Follow", "rachel"] _state_machine.userdata.distance = 1 # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:368 _sm_wait_stop_0 = OperatableStateMachine(outcomes=['finished']) with _sm_wait_stop_0: # x:112 y:116 OperatableStateMachine.add('Get Command', GetSpeech(watchdog=1000), transitions={'done': 'Command Stop', 'nothing': 'Get Command', 'fail': 'Get Command'}, autonomy={'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'words': 'words'}) # x:115 y:270 OperatableStateMachine.add('Command Stop', CheckConditionState(predicate=lambda x: True in [True for match in ['stop', 'arrived', 'reached', 'here', 'ok'] if match in x]), transitions={'true': 'finished', 'false': 'Get Command'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'words'}) # x:395 y:153, x:390 y:47, x:359 y:362, x:304 y:368 _sm_follow_loop_1 = ConcurrencyContainer(outcomes=['finished', 'error'], input_keys=['ID', 'distance'], conditions=[ ('finished', [('Wait Stop', 'finished')]), ('error', [('Action_follow', 'failed')]) ]) with _sm_follow_loop_1: # x:190 y:37 OperatableStateMachine.add('Action_follow', self.use_behavior(Action_followSM, 'Follow Loop/Action_follow'), transitions={'failed': 'error'}, autonomy={'failed': Autonomy.Inherit}, remapping={'ID': 'ID'}) # x:190 y:139 OperatableStateMachine.add('Wait Stop', _sm_wait_stop_0, transitions={'finished': 'finished'}, autonomy={'finished': Autonomy.Inherit}) with _state_machine: # x:85 y:51 OperatableStateMachine.add('GetName', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Get Person ID'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Action', 'output_value': 'name'}) # x:97 y:141 OperatableStateMachine.add('Get Person ID', GetRosParam(ParamName="/behavior/FoundPerson/Id"), transitions={'done': 'Get Entity Location', 'failed': 'Say_Lost'}, autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off}, remapping={'Value': 'personId'}) # x:92 y:225 OperatableStateMachine.add('Get Entity Location', GetEntityByID(), transitions={'found': 'Tell_Follow', 'not_found': 'Say_Lost'}, autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'ID': 'personId', 'Entity': 'Entity'}) # x:231 y:389 OperatableStateMachine.add('Tell Way', SaraSay(sentence="Show me the way !", input_keys=[], emotion=1, block=True), transitions={'done': 'Follow Loop'}, autonomy={'done': Autonomy.Off}) # x:391 y:374 OperatableStateMachine.add('Follow Loop', _sm_follow_loop_1, transitions={'finished': 'Stop_Follow', 'error': 'Cant_Follow'}, autonomy={'finished': Autonomy.Inherit, 'error': Autonomy.Inherit}, remapping={'ID': 'personId', 'distance': 'distance'}) # x:533 y:140 OperatableStateMachine.add('cause1', SetKey(Value="I lost the person I was following."), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:540 y:219 OperatableStateMachine.add('cause2', SetKey(Value="I was unable to follow the person."), transitions={'done': 'setrosparamcause'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:697 y:178 OperatableStateMachine.add('setrosparamcause', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:96 y:341 OperatableStateMachine.add('Tell_Follow', SaraSay(sentence=lambda x: "I will follow you, " + x[0] + " !", input_keys=["name"], emotion=0, block=True), transitions={'done': 'Tell Way'}, autonomy={'done': Autonomy.Off}, remapping={'name': 'name'}) # x:571 y:411 OperatableStateMachine.add('Stop_Follow', SaraSay(sentence=lambda x: "Ok " + x[0] + ", I will stop to follow you !", input_keys=["name"], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'name': 'name'}) # x:359 y:262 OperatableStateMachine.add('Cant_Follow', SaraSay(sentence=lambda x: "I can't follow you, " + x[0] + " !", input_keys=["name"], emotion=0, block=True), transitions={'done': 'cause2'}, autonomy={'done': Autonomy.Off}, remapping={'name': 'name'}) # x:345 y:149 OperatableStateMachine.add('Say_Lost', SaraSay(sentence=lambda x: "I have lost " + x[0] + " !", input_keys=["name"], emotion=0, block=True), transitions={'done': 'cause1'}, autonomy={'done': Autonomy.Off}, remapping={'name': 'name'}) 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:711 y:84, x:836 y:453, x:709 y:633 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["Find", "cup"] # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] with _state_machine: # x:67 y:70 OperatableStateMachine.add( 'cond', CheckConditionState( predicate=lambda x: x[1] is not None and x[1] != ''), transitions={ 'true': 'ReadParameters', 'false': 'say no object given' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'Action'}) # x:325 y:70 OperatableStateMachine.add( 'say no object given', SaraSay(sentence="You didn't told me what to find.", input_keys=[], emotion=1, block=True), transitions={'done': 'Cause1'}, autonomy={'done': Autonomy.Off}) # x:68 y:363 OperatableStateMachine.add('Action_find', self.use_behavior( Action_findSM, 'Action_find'), transitions={ 'done': 'Say_FInded_Object', 'failed': 'Say_Not_Found' }, autonomy={ 'done': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={ 'className': 'name', 'entity': 'entity' }) # x:77 y:159 OperatableStateMachine.add( 'ReadParameters', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'SAy_Find_Object'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'Action', 'output_value': 'name' }) # x:334 y:279 OperatableStateMachine.add( 'Get Time', CalculationState(calculation=lambda x: x.lastUpdateTime.secs), transitions={'done': 'Get Id'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'currentTime' }) # x:405 y:146 OperatableStateMachine.add( 'Get Id', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'Set Time'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity', 'output_value': 'id' }) # x:502 y:268 OperatableStateMachine.add( 'Set Time', SetRosParam(ParamName="/behavior/FoundEntity/lastUpdate"), transitions={'done': 'Set Id'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'currentTime'}) # x:645 y:323 OperatableStateMachine.add( 'Set Id', SetRosParam(ParamName="/behavior/FoundEntity/Id"), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'id'}) # x:835 y:14 OperatableStateMachine.add( 'Cause1', SetKey(Value="You didn't told me what to find."), transitions={'done': 'set cause in rosparam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:646 y:460 OperatableStateMachine.add( 'cause2', SetKey(Value="I didn't find the object"), transitions={'done': 'set cause in rosparam'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:769 y:347 OperatableStateMachine.add( 'set cause in rosparam', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:47 y:258 OperatableStateMachine.add( 'SAy_Find_Object', SaraSay(sentence=lambda x: "I'm now looking for the " + x, input_keys=[], emotion=0, block=True), transitions={'done': 'Action_find'}, autonomy={'done': Autonomy.Off}) # x:296 y:403 OperatableStateMachine.add( 'Say_Not_Found', SaraSay(sentence=lambda x: "I did not find the " + x + ".", input_keys=[], emotion=0, block=True), transitions={'done': 'cause2'}, autonomy={'done': Autonomy.Off}) # x:212 y:130 OperatableStateMachine.add( 'Say_FInded_Object', SaraSay(sentence=lambda x: "I just find the " + x.name, input_keys=[], emotion=0, block=True), transitions={'done': 'Get Time'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:30 y:365, x:130 y:365 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:54 y:385, x:100 y:523 _sm_safety_monitor_0 = OperatableStateMachine( outcomes=['finished', 'failed']) with _sm_safety_monitor_0: # x:289 y:57 OperatableStateMachine.add('subscribe to safety topic 1', SubscriberState(topic='/chatter', blocking=True, clear=False), transitions={ 'received': 'check if topic publishes warning', 'unavailable': 'failed' }, autonomy={ 'received': Autonomy.Off, 'unavailable': Autonomy.Off }, remapping={'message': 'message'}) # x:719 y:64 OperatableStateMachine.add( 'fine continue log', LogState(text="continue log", severity=Logger.REPORT_HINT), transitions={'done': 'subscribe to safety topic 1'}, autonomy={'done': Autonomy.Off}) # x:690 y:202 OperatableStateMachine.add( 'check if topic publishes warning', CheckConditionState( predicate=lambda message: message.data == "1"), transitions={ 'true': 'fine continue log', 'false': 'stop now log' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'message'}) # x:451 y:481 OperatableStateMachine.add('stop now log', LogState( text="stopping - message receieved", severity=Logger.REPORT_HINT), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:30 y:456, x:130 y:456 _sm_normal_activity_1 = OperatableStateMachine( outcomes=['finished', 'failed']) with _sm_normal_activity_1: # x:338 y:227 OperatableStateMachine.add('normalactivity', LogState(text="normal activity", severity=Logger.REPORT_HINT), transitions={'done': 'wait1'}, autonomy={'done': Autonomy.Off}) # x:618 y:312 OperatableStateMachine.add('wait1', WaitState(wait_time=1), transitions={'done': 'normalactivity'}, autonomy={'done': Autonomy.Off}) # x:113 y:364, x:465 y:451, x:737 y:425, x:254 y:385, x:221 y:448 _sm_container_2 = ConcurrencyContainer( outcomes=['finished', 'failed'], conditions=[('failed', [('normal_activity', 'failed'), ('safety_monitor', 'failed')]), ('finished', [('normal_activity', 'finished')]), ('finished', [('safety_monitor', 'finished')])]) with _sm_container_2: # x:149 y:54 OperatableStateMachine.add('normal_activity', _sm_normal_activity_1, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) # x:676 y:54 OperatableStateMachine.add('safety_monitor', _sm_safety_monitor_0, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) with _state_machine: # x:317 y:100 OperatableStateMachine.add('Container', _sm_container_2, transitions={ 'finished': 'finished', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }) return _state_machine
def create(self): # x:391 y:586, x:438 y:304 _state_machine = OperatableStateMachine( outcomes=['finished', 'failed']) _state_machine.userdata.StartPosition = "Operator" _state_machine.userdata.PositionBras = "IdlePose" _state_machine.userdata.title = "GPSR" _state_machine.userdata.EntryName = "door1/enter" _state_machine.userdata.ExitName = "door2/exit" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:135 y:542, x:527 y:395 _sm_validate_0 = OperatableStateMachine(outcomes=['done', 'bad'], input_keys=['sentence']) with _sm_validate_0: # x:56 y:62 OperatableStateMachine.add( 'Say_Command', 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:44 y:165 OperatableStateMachine.add('get speech', GetSpeech(watchdog=10), transitions={ 'done': 'is yes', 'nothing': 'say repeate', 'fail': 'say repeate' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'words'}) # x:91 y:307 OperatableStateMachine.add( 'is yes', CheckConditionState(predicate=lambda x: "yes" in x or "right" in x or "sure" in x), transitions={ 'true': 'done', 'false': 'say repeate' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'words'}) # x:423 y:332 OperatableStateMachine.add( 'say repeate', SaraSay(sentence="Please, repeat your command.", input_keys=[], emotion=1, block=True), transitions={'done': 'bad'}, autonomy={'done': Autonomy.Off}) # x:30 y:324 _sm_look_at_op_1 = OperatableStateMachine(outcomes=['fail']) with _sm_look_at_op_1: # x:61 y:31 OperatableStateMachine.add('set name', SetKey(Value="person"), transitions={'done': 'list persons'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'name'}) # x:44 y:110 OperatableStateMachine.add('list persons', list_entities_by_name( frontality_level=0.5, distance_max=10), transitions={ 'found': 'get closest', 'none_found': 'list persons' }, autonomy={ 'found': Autonomy.Off, 'none_found': Autonomy.Off }, remapping={ 'name': 'name', 'entity_list': 'entity_list', 'number': 'number' }) # x:43 y:211 OperatableStateMachine.add( 'get closest', CalculationState(calculation=lambda x: x[0].ID), transitions={'done': 'look at'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'entity_list', 'output_value': 'ID' }) # x:210 y:198 OperatableStateMachine.add('look at', KeepLookingAt(), transitions={'failed': 'list persons'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:307 y:35, x:335 y:491 _sm_get_commands_2 = OperatableStateMachine( outcomes=['fail', 'understood'], output_keys=['ActionForms', 'sentence']) with _sm_get_commands_2: # x:50 y:48 OperatableStateMachine.add( 'say ready', SaraSay(sentence="I'm ready for your commands.", input_keys=[], emotion=1, block=False), transitions={'done': 'GetSpeech'}, autonomy={'done': Autonomy.Off}) # x:32 y:408 OperatableStateMachine.add('SaraNLUgpsr', SaraNLUgpsr(), transitions={ 'understood': 'say understood', 'not_understood': 'say sorry', 'fail': 'say sorry' }, autonomy={ 'understood': Autonomy.Off, 'not_understood': Autonomy.Off, 'fail': Autonomy.Off }, remapping={ 'sentence': 'sentence', 'ActionForms': 'ActionForms' }) # x:597 y:223 OperatableStateMachine.add( 'say sorry', SaraSay( sentence="Sorry, I could not understand what you said.", input_keys=[], emotion=1, block=True), transitions={'done': 'GetSpeech'}, autonomy={'done': Autonomy.Off}) # x:30 y:491 OperatableStateMachine.add('say understood', SaraSay(sentence="Ok", input_keys=[], emotion=1, block=True), transitions={'done': 'understood'}, autonomy={'done': Autonomy.Off}) # x:44 y:131 OperatableStateMachine.add('GetSpeech', GetSpeech(watchdog=5), transitions={ 'done': 'validate', 'nothing': 'GetSpeech', 'fail': 'fail' }, autonomy={ 'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off }, remapping={'words': 'sentence'}) # x:29 y:259 OperatableStateMachine.add('validate', _sm_validate_0, transitions={ 'done': 'SaraNLUgpsr', 'bad': 'GetSpeech' }, autonomy={ 'done': Autonomy.Inherit, 'bad': Autonomy.Inherit }, remapping={'sentence': 'sentence'}) # x:122 y:413 _sm_end_3 = OperatableStateMachine(outcomes=['done'], input_keys=['ExitName']) with _sm_end_3: # x:30 y:40 OperatableStateMachine.add( 'win', SaraSay(sentence="Thank you. I'm going now.", input_keys=[], emotion=1, block=True), transitions={'done': 'Action_Pass_Door'}, autonomy={'done': Autonomy.Off}) # x:33 y:163 OperatableStateMachine.add( 'Action_Pass_Door', self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM, 'End/Action_Pass_Door'), transitions={ 'Done': 'say yay', 'Fail': 'done' }, autonomy={ 'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit }, remapping={'DoorName': 'ExitName'}) # x:210 y:262 OperatableStateMachine.add( 'say yay', SaraSay(sentence="I did it. I'm the best robot.", input_keys=[], emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:320 y:82, x:322 y:143, x:265 y:407, x:306 y:225, x:430 y:324 _sm_interact_operator_4 = ConcurrencyContainer( outcomes=['fail', 'understood'], output_keys=['ActionForms', 'sentence'], conditions=[('understood', [('Get Commands', 'understood')]), ('fail', [('Get Commands', 'fail')]), ('fail', [('look at op', 'fail')])]) with _sm_interact_operator_4: # x:95 y:45 OperatableStateMachine.add('Get Commands', _sm_get_commands_2, transitions={ 'fail': 'fail', 'understood': 'understood' }, autonomy={ 'fail': Autonomy.Inherit, 'understood': Autonomy.Inherit }, remapping={ 'ActionForms': 'ActionForms', 'sentence': 'sentence' }) # x:99 y:198 OperatableStateMachine.add('look at op', _sm_look_at_op_1, transitions={'fail': 'fail'}, autonomy={'fail': Autonomy.Inherit}) # x:325 y:387, x:314 y:190 _sm_fail_state_5 = OperatableStateMachine( outcomes=['finished', 'failed'], input_keys=['StartPosition']) with _sm_fail_state_5: # x:36 y:29 OperatableStateMachine.add( 'say failed', SaraSay(sentence="I failed. I'm going back to tell my master.", input_keys=[], emotion=1, block=True), transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Off}) # x:42 y:301 OperatableStateMachine.add( 'get error', GetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={ 'done': 'Say_Error', 'failed': 'finished' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'Value': 'Error'}) # x:48 y:414 OperatableStateMachine.add( 'Say_Error', SaraSay(sentence=lambda x: "Sorry, I failed because " + x, input_keys=[], emotion=0, block=True), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}) # x:25 y:172 OperatableStateMachine.add( 'Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'Fail state/Action_Move'), transitions={ 'finished': 'get error', 'failed': 'failed' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'StartPosition'}) # x:588 y:141, x:590 y:545, x:642 y:410 _sm_do_the_actions_6 = OperatableStateMachine( outcomes=['finished', 'failed', 'critical fail'], input_keys=['ActionForms', 'OriginalPose']) with _sm_do_the_actions_6: # x:85 y:33 OperatableStateMachine.add('set i', SetKey(Value=0), transitions={'done': 'is form?'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'index'}) # x:48 y:339 OperatableStateMachine.add( 'GetForm', FlexibleCalculationState(calculation=lambda x: x[0][x[1]], input_keys=["ActionForms", "index"]), transitions={'done': 'Action_Executor'}, autonomy={'done': Autonomy.Off}, remapping={ 'ActionForms': 'ActionForms', 'index': 'index', 'output_value': 'ActionForm' }) # x:57 y:131 OperatableStateMachine.add( 'is form?', FlexibleCheckConditionState( predicate=lambda x: x[1] < len(x[0]), input_keys=["ActionForms", "index"]), transitions={ 'true': 'set setp', 'false': 'finished' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={ 'ActionForms': 'ActionForms', 'index': 'index' }) # x:204 y:237 OperatableStateMachine.add( '++i', CalculationState(calculation=lambda x: x + 1), transitions={'done': 'is form?'}, autonomy={'done': Autonomy.Off}, remapping={ 'input_value': 'index', 'output_value': 'index' }) # x:72 y:479 OperatableStateMachine.add( 'Action_Executor', self.use_behavior(sara_flexbe_behaviors__Action_ExecutorSM, 'Do the actions/Action_Executor'), transitions={ 'finished': '++i', 'failed': 'failed', 'critical_fail': 'critical fail' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical_fail': Autonomy.Inherit }, remapping={'Action': 'ActionForm'}) # x:45 y:241 OperatableStateMachine.add('set setp', StoryboardSetStepKey(), transitions={'done': 'GetForm'}, autonomy={'done': Autonomy.Off}, remapping={'step': 'index'}) # x:424 y:175 _sm_initialisation_7 = OperatableStateMachine( outcomes=['done'], input_keys=['PositionBras', 'EntryName']) with _sm_initialisation_7: # x:62 y:79 OperatableStateMachine.add( 'Action_Pass_Door', self.use_behavior(sara_flexbe_behaviors__Action_Pass_DoorSM, 'Initialisation/Action_Pass_Door'), transitions={ 'Done': 'set step', 'Fail': 'done' }, autonomy={ 'Done': Autonomy.Inherit, 'Fail': Autonomy.Inherit }, remapping={'DoorName': 'EntryName'}) # x:70 y:306 OperatableStateMachine.add('set story', Set_Story(titre="GPSR", storyline=[]), transitions={'done': 'bras en lair'}, autonomy={'done': Autonomy.Off}) # x:288 y:375 OperatableStateMachine.add( 'say start', SaraSay(sentence="I'm ready to start the GPSR scenario.", input_keys=[], emotion=1, block=True), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}) # x:78 y:205 OperatableStateMachine.add('set step', Set_a_step(step=0), transitions={'done': 'set story'}, autonomy={'done': Autonomy.Off}) # x:101 y:395 OperatableStateMachine.add('bras en lair', MoveitMove(move=True, waitForExecution=False, group="RightArm"), transitions={ 'done': 'say start', 'failed': 'say start' }, autonomy={ 'done': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'target': 'PositionBras'}) with _state_machine: # x:33 y:103 OperatableStateMachine.add('Initialisation', _sm_initialisation_7, transitions={'done': 'Action_Move'}, autonomy={'done': Autonomy.Inherit}, remapping={ 'PositionBras': 'PositionBras', 'EntryName': 'EntryName' }) # x:767 y:196 OperatableStateMachine.add('Do the actions', _sm_do_the_actions_6, transitions={ 'finished': 'say succseed', 'failed': 'Fail state', 'critical fail': 'critical' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'critical fail': Autonomy.Inherit }, remapping={ 'ActionForms': 'ActionForms', 'OriginalPose': 'OriginalPose' }) # x:250 y:126 OperatableStateMachine.add( 'Fail state', _sm_fail_state_5, transitions={ 'finished': 'lift head', 'failed': 'critical' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'StartPosition': 'StartPosition'}) # x:250 y:257 OperatableStateMachine.add( 'critical', SaraSay( sentence="Critical failure! I require medical assistance.", input_keys=[], emotion=1, block=True), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}) # x:52 y:468 OperatableStateMachine.add('for 3', ForLoop(repeat=3), transitions={ 'do': 'GetOriginalPose', 'end': 'End' }, autonomy={ 'do': Autonomy.Off, 'end': Autonomy.Off }, remapping={'index': 'index'}) # x:747 y:475 OperatableStateMachine.add('say succseed', SaraSay( sentence="I succeed my mission.", input_keys=[], emotion=1, block=True), transitions={'done': 'for 3'}, autonomy={'done': Autonomy.Off}) # x:204 y:368 OperatableStateMachine.add('Interact operator', _sm_interact_operator_4, transitions={ 'fail': 'critical', 'understood': 'set step' }, autonomy={ 'fail': Autonomy.Inherit, 'understood': Autonomy.Inherit }, remapping={ 'ActionForms': 'ActionForms', 'sentence': 'sentence' }) # x:553 y:316 OperatableStateMachine.add('set story', StoryboardSetStoryFromAction(), transitions={'done': 'Do the actions'}, autonomy={'done': Autonomy.Off}, remapping={ 'titre': 'title', 'actionList': 'ActionForms' }) # x:404 y:377 OperatableStateMachine.add('set step', Set_a_step(step=0), transitions={'done': 'set story'}, autonomy={'done': Autonomy.Off}) # x:41 y:375 OperatableStateMachine.add( 'GetOriginalPose', Get_Robot_Pose(), transitions={'done': 'Interact operator'}, autonomy={'done': Autonomy.Off}, remapping={'pose': 'OriginalPose'}) # x:37 y:564 OperatableStateMachine.add('End', _sm_end_3, transitions={'done': 'finished'}, autonomy={'done': Autonomy.Inherit}, remapping={'ExitName': 'ExitName'}) # x:30 y:288 OperatableStateMachine.add('lift head', SaraSetHeadAngle(pitch=-0.3, yaw=0), transitions={'done': 'GetOriginalPose'}, autonomy={'done': Autonomy.Off}) # x:48 y:23 OperatableStateMachine.add('ContinueButton', ContinueButton(), transitions={ 'true': 'Initialisation', 'false': 'Initialisation' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }) # x:21 y:185 OperatableStateMachine.add( 'Action_Move', self.use_behavior(sara_flexbe_behaviors__Action_MoveSM, 'Action_Move'), transitions={ 'finished': 'lift head', 'failed': 'critical' }, autonomy={ 'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit }, remapping={'pose': 'StartPosition'}) 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:1333 y:90, x:1333 y:340 _state_machine = OperatableStateMachine( outcomes=['mission_successful', 'missing_failure']) _state_machine.userdata.fire_extinguish_position = lidar_flier.msg.lfGoal( ) _state_machine.userdata.lidar_flier_speed_goto = 1 _state_machine.userdata.fire_position = fire_detect.msg.firemanGoal() _state_machine.userdata.orbit_goal = lidar_flier.msg.lfGoal() _state_machine.userdata.flying_altitude = 5 _state_machine.userdata.is_mission_successful = False # Additional creation code can be added inside the following tags # [MANUAL_CREATE] param_prefix_namespace = "flexbe_behavior_launcher/" flying_altitude_list = rospy.get_param(param_prefix_namespace + 'main/flying_altitudes') uav_name_list = rospy.get_param(param_prefix_namespace + 'main/robot_name_list') robot_name = rospy.get_param(param_prefix_namespace + 'robot_name') lidar_flier_speed_goto = rospy.get_param( param_prefix_namespace + 'lidar_flier/lidar_flier_speed_goto') lidar_flier_speed_orbit = rospy.get_param( param_prefix_namespace + 'lidar_flier/lidar_flier_speed_orbit') lidar_flier_clockwise = rospy.get_param( param_prefix_namespace + 'lidar_flier/lidar_flier_clockwise') lidar_flier_stick_distance = rospy.get_param( param_prefix_namespace + 'lidar_flier/lidar_flier_stick_distance') window_position_estimation_reset_topic = rospy.get_param( param_prefix_namespace + 'main/window_position_estimation_reset_topic') window_position_estimation_reset_topic = "/" + robot_name + "/" + window_position_estimation_reset_topic Logger.loginfo("window_position_estimation_reset_topic: %s" % window_position_estimation_reset_topic) window_position_estimation_closest_window_topic = rospy.get_param( param_prefix_namespace + 'main/window_position_estimation_closest_window_topic') window_position_estimation_closest_window_topic = "/" + robot_name + "/" + window_position_estimation_closest_window_topic Logger.loginfo("window_position_estimation_closest_window_topic: %s" % window_position_estimation_closest_window_topic) land_service_topic = rospy.get_param(param_prefix_namespace + 'main/land_service_topic') land_service_topic = "/" + robot_name + "/" + land_service_topic Logger.loginfo("land_service_topic: %s" % land_service_topic) e_land_service_topic = rospy.get_param(param_prefix_namespace + 'main/e_land_service_topic') e_land_service_topic = "/" + robot_name + "/" + e_land_service_topic Logger.loginfo("e_land_service_topic: %s" % e_land_service_topic) fire_detection_topic = rospy.get_param(param_prefix_namespace + 'main/fire_detection_topic') fire_detection_topic = "/" + robot_name + "/" + fire_detection_topic Logger.loginfo("fire_detection_topic: %s" % fire_detection_topic) window_flyer_action_server_name = rospy.get_param( param_prefix_namespace + 'main/window_flyer_action_server_name') window_flyer_action_server_name = "/" + robot_name + "/" + window_flyer_action_server_name Logger.loginfo("window_flyer_action_server_name: %s" % window_flyer_action_server_name) lidar_flier_action_server_name = rospy.get_param( param_prefix_namespace + 'main/lidar_flier_action_server_name') lidar_flier_action_server_name = "/" + robot_name + "/" + lidar_flier_action_server_name Logger.loginfo("lidar_flier_action_server_name: %s" % lidar_flier_action_server_name) fire_extinguish_action_server_name = rospy.get_param( param_prefix_namespace + 'main/fire_extinguish_action_server_name') fire_extinguish_action_server_name = "/" + robot_name + "/" + fire_extinguish_action_server_name Logger.loginfo("fire_extinguish_action_server_name: %s" % fire_extinguish_action_server_name) control_manager_diagnostics_topic = rospy.get_param( param_prefix_namespace + 'main/control_manager_diagnostics_topic') control_manager_diagnostics_topic = "/" + robot_name + "/" + control_manager_diagnostics_topic Logger.loginfo("control_manager_diagnostics_topic: %s" % control_manager_diagnostics_topic) wait_for_start_service_topic = rospy.get_param( param_prefix_namespace + 'main/wait_for_start_service_topic') wait_for_start_service_topic = "/" + robot_name + "/" + wait_for_start_service_topic Logger.loginfo("wait_for_start_service_topic: %s" % wait_for_start_service_topic) control_manager_cmd_topic = rospy.get_param( param_prefix_namespace + 'main/control_manager_cmd_topic') control_manager_cmd_topic = "/" + robot_name + "/" + control_manager_cmd_topic Logger.loginfo("control_manager_cmd_topic: %s" % control_manager_cmd_topic) goto_altitude_service_topic = rospy.get_param( param_prefix_namespace + 'main/goto_altitude_service_topic') goto_altitude_service_topic = "/" + robot_name + "/" + goto_altitude_service_topic Logger.loginfo("goto_altitude_service_topic: %s" % goto_altitude_service_topic) constraint_switch_service_topic = rospy.get_param( param_prefix_namespace + 'main/constraint_switch_service_topic') constraint_switch_service_topic = "/" + robot_name + "/" + constraint_switch_service_topic Logger.loginfo("constraint_switch_service_topic: %s" % constraint_switch_service_topic) control_manager_switch_controller_service_topic = rospy.get_param( param_prefix_namespace + 'main/control_manager_switch_controller_service_topic') control_manager_switch_controller_service_topic = "/" + robot_name + "/" + control_manager_switch_controller_service_topic Logger.loginfo("control_manager_switch_controller_service_topic: %s" % control_manager_switch_controller_service_topic) outside_flying_controller = rospy.get_param( param_prefix_namespace + 'main/outside_flying_controller') outside_flying_controller = outside_flying_controller Logger.loginfo("outside_flying_controller: %s" % outside_flying_controller) inside_flying_controller = rospy.get_param( param_prefix_namespace + 'main/inside_flying_controller') inside_flying_controller = inside_flying_controller Logger.loginfo("inside_flying_controller: %s" % inside_flying_controller) extinguishing_controller = rospy.get_param( param_prefix_namespace + 'main/extinguishing_controller') extinguishing_controller = extinguishing_controller Logger.loginfo("extinguishing_controller: %s" % extinguishing_controller) outside_flying_constraints = rospy.get_param( param_prefix_namespace + 'main/outside_flying_constraints') outside_flying_constraints = outside_flying_constraints Logger.loginfo("outside_flying_constraints: %s" % outside_flying_constraints) inside_flying_constraints = rospy.get_param( param_prefix_namespace + 'main/inside_flying_constraints') inside_flying_constraints = inside_flying_constraints Logger.loginfo("inside_flying_constraints: %s" % inside_flying_constraints) extinguishing_constraints = rospy.get_param( param_prefix_namespace + 'main/extinguishing_constraints') extinguishing_constraints = extinguishing_constraints Logger.loginfo("extinguishing_constraints: %s" % extinguishing_constraints) flying_altitude = 3 for i in range(len(uav_name_list)): if uav_name_list[i] == robot_name: flying_altitude = flying_altitude_list[i] break orbit_goal = lidar_flier.msg.lfGoal() orbit_goal.altitude = flying_altitude orbit_goal.orbit = True orbit_goal.frame_id = str(robot_name + "/gps_origin") orbit_goal.speed = lidar_flier_speed_orbit orbit_goal.stick_distance = lidar_flier_stick_distance orbit_goal.clockwise = lidar_flier_clockwise _state_machine.userdata.flying_altitude = flying_altitude _state_machine.userdata.orbit_goal = orbit_goal _state_machine.userdata.lidar_flier_speed_goto = lidar_flier_speed_goto Logger.loginfo("fire_ch_sm.flying_altitude: %s" % _state_machine.userdata.flying_altitude) Logger.loginfo("fire_ch_sm.orbit_goal: %s" % _state_machine.userdata.orbit_goal) Logger.loginfo("fire_ch_sm.lidar_flier_speed_goto: %s" % _state_machine.userdata.lidar_flier_speed_goto) # [/MANUAL_CREATE] with _state_machine: # x:92 y:74 OperatableStateMachine.add( 'WaitForStart', ServiceWaitForStart( service_topic=wait_for_start_service_topic), transitions={ 'received_one': 'GotoAltitude', 'received_two': 'Land', 'received_three': 'Land', 'error': 'Land' }, autonomy={ 'received_one': Autonomy.Off, 'received_two': Autonomy.Off, 'received_three': Autonomy.Off, 'error': Autonomy.Off }, remapping={'start_value': 'start_value'}) # x:328 y:74 OperatableStateMachine.add( 'GotoAltitude', ServiceGoToAltitudeState( service_topic=goto_altitude_service_topic, control_manager_diagnostics_topic= control_manager_diagnostics_topic), transitions={ 'successed': 'SaveHomePosition', 'failed': 'Land' }, autonomy={ 'successed': Autonomy.Off, 'failed': Autonomy.Off }, remapping={'goal': 'flying_altitude'}) # x:641 y:74 OperatableStateMachine.add( 'SaveHomePosition', WaitForMsgState( topic=control_manager_cmd_topic, wait_time=5, function=self.callback_true, input_keys=["lidar_flier_speed_goto"], output_keys=["home_position"], output_function=self.home_position_output_function), transitions={ 'successed': 'fire_challenge_outside', 'failed': 'Land' }, autonomy={ 'successed': Autonomy.Off, 'failed': Autonomy.Off }, remapping={ 'lidar_flier_speed_goto': 'lidar_flier_speed_goto', 'home_position': 'home_position' }) # x:669 y:224 OperatableStateMachine.add( 'LidarFlierGotoHomePosition', LidarFlierActionState( action_server_name=lidar_flier_action_server_name), transitions={ 'successed': 'Land', 'no_valid_points_in_scan': 'Land', 'stop_requested': 'Land', 'server_not_initialized': 'Land', 'error': 'Land', 'preempted': 'Land' }, autonomy={ 'successed': Autonomy.Off, 'no_valid_points_in_scan': Autonomy.Off, 'stop_requested': Autonomy.Off, 'server_not_initialized': Autonomy.Off, 'error': Autonomy.Off, 'preempted': Autonomy.Off }, remapping={'goal': 'home_position'}) # x:442 y:374 OperatableStateMachine.add('Land', ServiceTriggerState( service_topic=land_service_topic, state_name="ControlManager"), transitions={ 'finished': 'WasMissionSuccessful', 'failed': 'Land' }, autonomy={ 'finished': Autonomy.Off, 'failed': Autonomy.Off }) # x:869 y:71 OperatableStateMachine.add('fire_challenge_outside', self.use_behavior( fire_challenge_outsideSM, 'fire_challenge_outside'), transitions={ 'no_detection': 'LidarFlierGotoHomePosition', 'water_depleted': 'SetMissionSuccess', 'mission_part_failure': 'LidarFlierGotoHomePosition', 'preempted': 'LidarFlierGotoHomePosition', 'target_lost': 'LidarFlierGotoHomePosition' }, autonomy={ 'no_detection': Autonomy.Inherit, 'water_depleted': Autonomy.Inherit, 'mission_part_failure': Autonomy.Inherit, 'preempted': Autonomy.Inherit, 'target_lost': Autonomy.Inherit }) # x:981 y:374 OperatableStateMachine.add( 'WasMissionSuccessful', CheckConditionState(predicate=lambda x: x), transitions={ 'true': 'mission_successful', 'false': 'missing_failure' }, autonomy={ 'true': Autonomy.Off, 'false': Autonomy.Off }, remapping={'input_value': 'is_mission_successful'}) # x:983 y:224 OperatableStateMachine.add( 'SetMissionSuccess', SetVariableToTrueState(key=['is_mission_successful'], function=self.set_success_variable), transitions={'done': 'LidarFlierGotoHomePosition'}, autonomy={'done': Autonomy.Off}, remapping={'is_mission_successful': 'is_mission_successful'}) return _state_machine
def create(self): # x:93 y:313, x:514 y:143 _state_machine = OperatableStateMachine(outcomes=['found', 'not_found'], input_keys=['className', 'personID'], output_keys=['personEntity']) _state_machine.userdata.className = "person" _state_machine.userdata.personID = 0 _state_machine.userdata.personEntity = "" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:707 y:760 _sm_rotation_0 = OperatableStateMachine(outcomes=['end'], output_keys=['personEntity']) with _sm_rotation_0: # x:51 y:38 OperatableStateMachine.add('Set 180 degres', SetKey(Value=3.1416), transitions={'done': 'set cpt to 0'}, 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': 'check is cpt is 1', 'failed': 'check is cpt is 1'}, 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:59 y:128 OperatableStateMachine.add('set cpt to 0', SetKey(Value=0), transitions={'done': 'Look Center'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'cpt'}) # x:400 y:499 OperatableStateMachine.add('check is cpt is 1', CheckConditionState(predicate=lambda x: x==1), transitions={'true': 'set entity to unknown', 'false': 'set cpt to 1'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'cpt'}) # x:414 y:210 OperatableStateMachine.add('set cpt to 1', SetKey(Value=1), transitions={'done': 'Look Right'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'cpt'}) # x:605 y:659 OperatableStateMachine.add('set entity to unknown', SetKey(Value="unknown"), transitions={'done': 'end'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'personEntity'}) # x:683 y:188 _sm_find_entity_1 = OperatableStateMachine(outcomes=['found'], input_keys=['personID'], output_keys=['personEntity']) with _sm_find_entity_1: # x:226 y:188 OperatableStateMachine.add('get person', GetEntityByID(), transitions={'found': 'found', 'not_found': 'WaitState'}, autonomy={'found': Autonomy.Off, 'not_found': Autonomy.Off}, remapping={'ID': 'personID', 'Entity': 'personEntity'}) # x:194 y:40 OperatableStateMachine.add('WaitState', WaitState(wait_time=1), transitions={'done': 'get person'}, autonomy={'done': Autonomy.Off}) # x:372 y:27, x:392 y:217, x:400 y:139, x:330 y:458 _sm_container_2 = ConcurrencyContainer(outcomes=['found', 'not_found'], input_keys=['className', 'personID'], output_keys=['personEntity'], 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={'personID': 'personID', 'personEntity': 'personEntity'}) # x:135 y:199 OperatableStateMachine.add('Rotation', _sm_rotation_0, transitions={'end': 'not_found'}, autonomy={'end': Autonomy.Inherit}, remapping={'personEntity': 'personEntity'}) 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': 'not_found'}, autonomy={'done': Autonomy.Off}) # 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', 'personID': 'personID', 'personEntity': 'personEntity'}) # x:67 y:222 OperatableStateMachine.add('WaitState', WaitState(wait_time=1), transitions={'done': 'found'}, autonomy={'done': Autonomy.Off}) return _state_machine
def create(self): # x:1083 y:361, x:1016 y:460, x:1004 y:649 _state_machine = OperatableStateMachine(outcomes=['finished', 'failed', 'critical_fail'], input_keys=['Action']) _state_machine.userdata.Action = ["FindPerson","philippe"] _state_machine.userdata.rotation = -1.57 _state_machine.userdata.className = "person" # Additional creation code can be added inside the following tags # [MANUAL_CREATE] # [/MANUAL_CREATE] # x:30 y:458 _sm_look_at_0 = OperatableStateMachine(outcomes=['finished'], input_keys=['entity']) with _sm_look_at_0: # x:142 y:112 OperatableStateMachine.add('get ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'look'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity', 'output_value': 'ID'}) # x:106 y:262 OperatableStateMachine.add('look', KeepLookingAt(), transitions={'failed': 'look'}, autonomy={'failed': Autonomy.Off}, remapping={'ID': 'ID'}) # x:297 y:737, x:531 y:481, x:378 y:120, x:739 y:299 _sm_ask_confirmation_1 = OperatableStateMachine(outcomes=['yes', 'no', 'error', 'noname'], input_keys=['name']) with _sm_ask_confirmation_1: # x:473 y:44 OperatableStateMachine.add('check if ask name', CheckConditionState(predicate=lambda x: x[1] != ""), transitions={'true': 'Repeat the question', 'false': 'wait 2'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'name'}) # x:35 y:303 OperatableStateMachine.add('Get Yes or No', GetSpeech(watchdog=5), transitions={'done': 'Repeat', 'nothing': 'Sara_Not_Understand', 'fail': 'Sara_Not_Understand'}, autonomy={'done': Autonomy.Off, 'nothing': Autonomy.Off, 'fail': Autonomy.Off}, remapping={'words': 'words'}) # x:34 y:486 OperatableStateMachine.add('Check Yes', CheckConditionState(predicate=lambda x: "yes" in x), transitions={'true': 'yes', 'false': 'Check No'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'words'}) # x:281 y:476 OperatableStateMachine.add('Check No', CheckConditionState(predicate=lambda x: "no" in x), transitions={'true': 'no', 'false': 'Sara_Not_Understand'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'words'}) # x:55 y:390 OperatableStateMachine.add('Repeat', LogKeyState(text="I heard: {}", severity=Logger.REPORT_HINT), transitions={'done': 'Check Yes'}, autonomy={'done': Autonomy.Off}, remapping={'data': 'words'}) # x:529 y:253 OperatableStateMachine.add('say hi', SaraSay(sentence="Hi there", input_keys=[], emotion=1, block=True), transitions={'done': 'noname'}, autonomy={'done': Autonomy.Off}) # x:534 y:159 OperatableStateMachine.add('wait 2', WaitState(wait_time=2), transitions={'done': 'say hi'}, autonomy={'done': Autonomy.Off}) # x:37 y:95 OperatableStateMachine.add('Repeat the question', ForLoop(repeat=5), transitions={'do': 'Ask_Person', 'end': 'error'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:31 y:193 OperatableStateMachine.add('Ask_Person', SaraSay(sentence=lambda x: "Are you " + x + "?", input_keys=[], emotion=0, block=True), transitions={'done': 'Get Yes or No'}, autonomy={'done': Autonomy.Off}) # x:264 y:269 OperatableStateMachine.add('Sara_Not_Understand', SaraSay(sentence=lambda x: "I did not understand.", input_keys=[], emotion=0, block=True), transitions={'done': 'Repeat the question'}, autonomy={'done': Autonomy.Off}) # x:305 y:322, x:301 y:53 _sm_if_need_the_one_2 = OperatableStateMachine(outcomes=['done', 'no_param'], input_keys=['Action'], output_keys=['person', 'name']) with _sm_if_need_the_one_2: # x:30 y:40 OperatableStateMachine.add('cond', CheckConditionState(predicate=lambda x: x[1] != None and x[1] != ''), transitions={'true': 'ReadParameters', 'false': 'no_param'}, autonomy={'true': Autonomy.Off, 'false': Autonomy.Off}, remapping={'input_value': 'Action'}) # x:95 y:129 OperatableStateMachine.add('ReadParameters', CalculationState(calculation=lambda x: x[1]), transitions={'done': 'Say_Find_Objects'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'Action', 'output_value': 'name'}) # x:122 y:297 OperatableStateMachine.add('set person', SetKey(Value="person"), transitions={'done': 'done'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'person'}) # x:97 y:213 OperatableStateMachine.add('Say_Find_Objects', SaraSay(sentence=lambda x: "I'm now looking for " + x, input_keys=[], emotion=0, block=True), transitions={'done': 'set person'}, autonomy={'done': Autonomy.Off}) # x:499 y:233, x:499 y:138, x:508 y:57, x:467 y:328, x:430 y:458, x:530 y:458, x:630 y:458, x:470 y:369, x:840 y:558 _sm_confirm_and_look_at_3 = ConcurrencyContainer(outcomes=['yes', 'no', 'error', 'noname'], input_keys=['name', 'entity'], conditions=[ ('yes', [('Ask Confirmation', 'yes')]), ('no', [('Ask Confirmation', 'no')]), ('error', [('Ask Confirmation', 'error')]), ('noname', [('Ask Confirmation', 'noname')]), ('error', [('look at', 'finished')]) ]) with _sm_confirm_and_look_at_3: # x:176 y:40 OperatableStateMachine.add('Ask Confirmation', _sm_ask_confirmation_1, transitions={'yes': 'yes', 'no': 'no', 'error': 'error', 'noname': 'noname'}, autonomy={'yes': Autonomy.Inherit, 'no': Autonomy.Inherit, 'error': Autonomy.Inherit, 'noname': Autonomy.Inherit}, remapping={'name': 'name'}) # x:30 y:122 OperatableStateMachine.add('look at', _sm_look_at_0, transitions={'finished': 'error'}, autonomy={'finished': Autonomy.Inherit}, remapping={'entity': 'entity'}) with _state_machine: # x:62 y:38 OperatableStateMachine.add('Action_findPerson', self.use_behavior(Action_findPersonSM, 'Action_findPerson'), transitions={'done': 'If need the one', 'pas_done': 'reset Head'}, autonomy={'done': Autonomy.Inherit, 'pas_done': Autonomy.Inherit}, remapping={'className': 'className', 'entity': 'entity'}) # x:512 y:255 OperatableStateMachine.add('Do not find person', SaraSay(sentence="I did not find a person.", input_keys=[], emotion=1, block=True), transitions={'done': 'cause2'}, autonomy={'done': Autonomy.Off}) # x:200 y:536 OperatableStateMachine.add('Retry', ForLoop(repeat=1), transitions={'do': 'Try again', 'end': 'reset Head'}, autonomy={'do': Autonomy.Off, 'end': Autonomy.Off}, remapping={'index': 'index'}) # x:73 y:456 OperatableStateMachine.add('Try again', SaraSay(sentence="Oh, I will try again.", input_keys=[], emotion=1, block=True), transitions={'done': 'action_turn'}, autonomy={'done': Autonomy.Off}) # x:63 y:310 OperatableStateMachine.add('action_turn', self.use_behavior(action_turnSM, 'action_turn'), transitions={'finished': 'Action_findPerson', 'failed': 'reset Head'}, autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit}, remapping={'rotation': 'rotation'}) # x:145 y:194 OperatableStateMachine.add('reset Head', SaraSetHeadAngle(pitch=0, yaw=0), transitions={'done': 'Do not find person'}, autonomy={'done': Autonomy.Off}) # x:997 y:258 OperatableStateMachine.add('set param', SetRosParam(ParamName="/behavior/FoundPerson/Id"), transitions={'done': 'finished'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'ID'}) # x:970 y:88 OperatableStateMachine.add('get ID', CalculationState(calculation=lambda x: x.ID), transitions={'done': 'set param'}, autonomy={'done': Autonomy.Off}, remapping={'input_value': 'entity', 'output_value': 'ID'}) # x:531 y:38 OperatableStateMachine.add('confirm and look at', _sm_confirm_and_look_at_3, transitions={'yes': 'Say_found', 'no': 'Retry', 'error': 'reset Head', 'noname': 'get ID'}, autonomy={'yes': Autonomy.Inherit, 'no': Autonomy.Inherit, 'error': Autonomy.Inherit, 'noname': Autonomy.Inherit}, remapping={'name': 'name', 'entity': 'entity'}) # x:662 y:325 OperatableStateMachine.add('cause2', SetKey(Value="I did not find any person."), transitions={'done': 'setrosparamfailure'}, autonomy={'done': Autonomy.Off}, remapping={'Key': 'Key'}) # x:820 y:377 OperatableStateMachine.add('setrosparamfailure', SetRosParam(ParamName="behavior/GPSR/CauseOfFailure"), transitions={'done': 'failed'}, autonomy={'done': Autonomy.Off}, remapping={'Value': 'Key'}) # x:782 y:169 OperatableStateMachine.add('say found person', SaraSay(sentence="I found a person.", input_keys=[], emotion=1, block=True), transitions={'done': 'get ID'}, autonomy={'done': Autonomy.Off}) # x:417 y:167 OperatableStateMachine.add('say hello', SaraSay(sentence="Hello.", input_keys=[], emotion=1, block=True), transitions={'done': 'confirm and look at'}, autonomy={'done': Autonomy.Off}) # x:278 y:47 OperatableStateMachine.add('If need the one', _sm_if_need_the_one_2, transitions={'done': 'say hello', 'no_param': 'say found person'}, autonomy={'done': Autonomy.Inherit, 'no_param': Autonomy.Inherit}, remapping={'Action': 'Action', 'person': 'person', 'name': 'name'}) # x:724 y:101 OperatableStateMachine.add('Say_found', SaraSay(sentence=lambda x: "I have found " + x + "!", input_keys=[], emotion=0, block=True), transitions={'done': 'get ID'}, autonomy={'done': Autonomy.Off}) return _state_machine