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
