def __init__(self, msg_pool, arg_key='tell_argument'):
input_keys = ['in_location_pose_in_map', 'in_target_object']
if arg_key:
input_keys.append(arg_key)
Concurrence.__init__(self, outcomes=[succeeded, aborted, 'move_failed', 'no_object_found'],
default_outcome=aborted,
input_keys=input_keys,
output_keys=['out_objects_data'],
outcome_map={succeeded: {'TELL_GOING_TO_SEARCH': succeeded,
'MOVE_AND_RECOGNIZE': succeeded},
'no_object_found': {'MOVE_AND_RECOGNIZE': 'no_object_found'},
'move_failed': {'MOVE_AND_RECOGNIZE': 'move_failed'}})
if type(msg_pool) is str:
msg_pool = [msg_pool]
if arg_key is None:
arg_key = 'tell_argument'
self.userdata.tell_argument = None # No argument set!
# Concurrence container to speak and search simultanously
with self:
Concurrence.add('TELL_GOING_TO_SEARCH', SpeakActionFromPoolStateMachine(msg_pool, arg_key=arg_key),
remapping={arg_key: arg_key})
Concurrence.add('MOVE_AND_RECOGNIZE', GoAndRecognizeSM(),
remapping={'in_target_object': 'in_target_object',
'in_location_pose_in_map': 'in_location_pose_in_map',
'out_objects_data': 'out_objects_data'})
def __init__(self,
behavior_pool=None,
textpool=None,
wait_before_speak=None):
input_keys = []
if not textpool:
input_keys = ['text']
use_bpool = True
if not isinstance(behavior_pool, list):
use_bpool = False
Concurrence.__init__(self,
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=input_keys,
default_outcome='aborted',
outcome_map={
'succeeded': {
'GESTURE_SPEECH': 'succeeded',
'GESTURE_MOVE': 'succeeded'
}
})
with self:
Concurrence.add(
'GESTURE_MOVE',
ExecuteBehavior(behavior_pool)
if not use_bpool else ExecuteBehaviorFromPoolSM(behavior_pool))
Concurrence.add(
'GESTURE_SPEECH',
SpeechState(wait_before_speak=wait_before_speak,
text=textpool,
blocking=True) if not textpool else
SpeechFromPoolSM(wait_before_speak=wait_before_speak,
pool=textpool,
blocking=True))
def __init__(self):
Concurrence.__init__(
self,
outcomes=['succeeded', 'aborted', 'preempted'],
default_outcome='succeeded',
input_keys=['log_mission'],
output_keys=['log_mission'],
child_termination_cb=self.child_termination_cb,
outcome_map={'succeeded': {
'WAITING': 'finished'
}})
self.register_start_cb(self.start_pause)
self.register_termination_cb(self.termination_pause)
_min_time_to_go_recharge = 15.0
@cb_interface(input_keys=['log_mission'],
outcomes=['insufficient_time', 'recharge', 'preempted'])
def check_waiting_time(ud):
navigation_duration = datetime.datetime.now(
) - ud.log_mission['start_time']
if datetime.timedelta(seconds=ud.log_mission['patrol']['min_duration']) - navigation_duration \
> datetime.timedelta(seconds=_min_time_to_go_recharge):
return 'recharge'
else:
return 'succeeded'
self.recharge = StateMachine(
input_keys=['log_mission'],
output_keys=['log_mission'],
outcomes=['succeeded', 'aborted', 'preempted'])
with self.recharge:
StateMachine.add('CHECK_WAITING_TIME', CBState(check_waiting_time),
{
'recharge': 'RECHARGE',
'insufficient_time': 'succeeded'
})
StateMachine.add('RECHARGE', Recharge())
with self:
Concurrence.add('WAITING', Waiting())
Concurrence.add('CONDITIONAL_RECHARGE', self.recharge)
def __init__(self):
Concurrence.__init__(
self,
outcomes=['exit', 'restart', 'aborted', 'preempted'],
default_outcome='restart',
output_keys=['control_infos'],
child_termination_cb=self.child_termination_cb,
outcome_cb=self.outcome_cb)
self.userdata.control_infos = {}
with self:
Concurrence.add(
'CHANGE_CONTROL_MONITOR',
MonitorState("/change_control",
String,
self.change_control_cb,
input_keys=['control_infos'],
output_keys=['control_infos']))
Concurrence.add('TIMER', Timer())
Concurrence.add('CONTROL', Control())
def __init__(self):
Concurrence.__init__(self, default_outcome='aborted',
input_keys=['text'],
output_keys=['text'],
outcomes=['succeeded', 'preempted', 'aborted'],
child_termination_cb=self.getfinish_Cb,
outcome_cb=self.outcome_Cb)
jointLoop = StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with jointLoop:
StateMachine.add('NEXT_MOVE',
RandomSelectionFromPoolState(self._movementList),
remapping={'selected_item': 'joint_angles'},
transitions={'succeeded': 'MOVEMENT'}
)
StateMachine.add('MOVEMENT',
JointAngleState(['HeadPitch', 'HeadYaw', 'RElbowYaw', 'LElbowYaw']),
transitions={'succeeded': 'NEXT_DELAY'}
)
StateMachine.add('NEXT_DELAY',
RandomSelectionFromPoolState(self._delayList),
remapping={'selected_item': 'timeout'},
transitions={'succeeded': 'DELAY'}
)
StateMachine.add('DELAY',
TimeOutState(),
transitions={'succeeded': 'NEXT_MOVE'}
)
with self:
Concurrence.add('MOVING',
jointLoop)
Concurrence.add('GET_USER_ANSWER',
GetUserAnswer(),
remapping={'text': 'text'})
