def create(self):
control_topic = 'control'
joint_trajectory_topic = 'motion/controller/joint_trajectory'
joy_topic = '/hmi/joystick'
moveit_action = 'move_group'
# x:68 y:631, x:130 y:353
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.unused = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:414 y:360, x:130 y:353, x:506 y:357, x:233 y:351, x:720 y:363, x:715 y:411
_sm_randmovements_0 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
input_keys=['unused'],
conditions=[('failed', [('WaitKey', 'unavailable')]),
('finished', [('RandMovements', 'done')]),
('finished', [('WaitKey', 'received')]),
('failed', [('RandMovements', 'failed')])])
with _sm_randmovements_0:
# x:148 y:122
OperatableStateMachine.add('WaitKey',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: any(x.buttons),
buffered=False,
clear=False),
transitions={
'received': 'finished',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:366 y:118
OperatableStateMachine.add('RandMovements',
SweetieBotRandHeadMovements(
controller='joint_state_head',
duration=120,
interval=[3, 5],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'unused'})
with _state_machine:
# x:108 y:53
OperatableStateMachine.add('RandMovements',
_sm_randmovements_0,
transitions={
'finished': 'HeadBasic',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'unused': 'unused'})
# x:351 y:150
OperatableStateMachine.add('HeadBasic',
SrdfStateToMoveit(
config_name='head_upright',
move_group='head',
action_topic=moveit_action,
robot_name=''),
transitions={
'reached': 'GoodBye',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:329 y:271
OperatableStateMachine.add(
'GoodBye',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='thank_you_for_attention_nopony_was_harmed',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='introduce_herself',
t3=[2, 2.0],
type3='voice/play_wav',
cmd3='am_i_really_a_good_actress',
t4=[2, 2.0],
type4='motion/joint_trajectory',
cmd4='head_suprised'),
transitions={
'success': 'Bow',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:288 y:547
OperatableStateMachine.add('RandMovements2',
SweetieBotRandHeadMovements(
controller='joint_state_head',
duration=120,
interval=[3, 5],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'unused'})
# x:319 y:387
OperatableStateMachine.add('Bow',
CompoundAction(
t1=[0, 2.0],
type1='motion/joint_trajectory',
cmd1='bow_begin',
t2=[1, 2.0],
type2='motion/joint_trajectory',
cmd2='bow_end',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'RandMovements2',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
return _state_machine
def create(self):
voice_topic = 'control'
torque_off_service = 'motion/controller/torque_off/set_operational'
joint_state_control_topic = 'motion/controller/joint_state/out_joints_src_reset'
joy_topic = '/hmi/joystick'
eyes_topic = 'control'
# x:68 y:535, x:786 y:525
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.be_evil = self.be_evil
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:554 y:394, x:130 y:397
_sm_waitforjoyevent_0 = OperatableStateMachine(
outcomes=['received', 'unavailable'])
with _sm_waitforjoyevent_0:
# x:288 y:158
OperatableStateMachine.add('JoyEvent',
SubscriberState(topic=joy_topic,
blocking=True,
clear=False),
transitions={
'received': 'CheckEvent',
'unavailable': 'unavailable'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:485 y:159
OperatableStateMachine.add(
'CheckEvent',
DecisionState(outcomes=['noactivity', 'activity'],
conditions=lambda msg: 'activity'
if any(msg.buttons) or any(msg.axes[0:5]) else
'noactivity'),
transitions={
'noactivity': 'JoyEvent',
'activity': 'received'
},
autonomy={
'noactivity': Autonomy.Off,
'activity': Autonomy.Off
},
remapping={'input_value': 'message'})
# x:538 y:386, x:347 y:384, x:134 y:383, x:831 y:385, x:906 y:386
_sm_idlebehavior_1 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
conditions=[('finished', [('RandomHeadMovements', 'done')]),
('failed', [('WaitForJoyEvent', 'unavailable')]),
('finished', [('WaitForJoyEvent', 'received')])])
with _sm_idlebehavior_1:
# x:474 y:110
OperatableStateMachine.add('RandomHeadMovements',
SweetieBotRandHeadMovements(
controller='joint_state_head',
duration=100,
interval=[1, 4],
max2356=[0.3, 0.3, 1, 1],
min2356=[-0.3, -0.3, -1, -1]),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
# x:144 y:133
OperatableStateMachine.add('WaitForJoyEvent',
_sm_waitforjoyevent_0,
transitions={
'received': 'finished',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Inherit,
'unavailable': Autonomy.Inherit
})
with _state_machine:
# x:79 y:99
OperatableStateMachine.add(
'CheckEvil',
DecisionState(outcomes=['good', 'evil'],
conditions=lambda x: 'evil' if x else 'good'),
transitions={
'good': 'NormalEyes',
'evil': 'RedEyes'
},
autonomy={
'good': Autonomy.Off,
'evil': Autonomy.Off
},
remapping={'input_value': 'be_evil'})
# x:550 y:467
OperatableStateMachine.add('AskForAssistance',
TextCommandState(type='voice/play_wav',
command='assistance',
topic=voice_topic),
transitions={'done': 'failed'},
autonomy={'done': Autonomy.Off})
# x:440 y:126
OperatableStateMachine.add('WaitForMovementFinish',
WaitState(wait_time=0.0),
transitions={'done': 'DoTricks'},
autonomy={'done': Autonomy.Off})
# x:251 y:503
OperatableStateMachine.add(
'StartingStance',
PublisherState(topic=joint_state_control_topic,
msg_type=JointState,
value={
'name': [
'joint51', 'joint52', 'joint53',
'eyes_pitch', 'eyes_yaw'
],
'position': [0.2, 0.0, 0.2, 0.0, 0.0]
}),
transitions={
'done': 'CheckEvil2',
'failed': 'AskForAssistance'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
})
# x:731 y:153
OperatableStateMachine.add('DoTricks',
self.use_behavior(
DoTricksSM, 'DoTricks'),
transitions={
'finished': 'NormalLook',
'failed': 'AskForAssistance'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'be_evil': 'be_evil'})
# x:27 y:219
OperatableStateMachine.add('RedEyes',
TextCommandState(type='eyes/emotion',
command='red_eyes',
topic=eyes_topic),
transitions={'done': 'IdleBehavior'},
autonomy={'done': Autonomy.Off})
# x:222 y:65
OperatableStateMachine.add('NormalEyes',
TextCommandState(type='eyes/emotion',
command='normal',
topic=eyes_topic),
transitions={'done': 'IdleBehavior'},
autonomy={'done': Autonomy.Off})
# x:481 y:305
OperatableStateMachine.add('IdleBehavior',
_sm_idlebehavior_1,
transitions={
'finished': 'StartingStance',
'failed': 'AskForAssistance'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
})
# x:181 y:360
OperatableStateMachine.add(
'CheckEvil2',
DecisionState(outcomes=['evil', 'good'],
conditions=lambda x: 'evil' if x else 'good'),
transitions={
'evil': 'EvilLook',
'good': 'WaitForMovementFinish'
},
autonomy={
'evil': Autonomy.Off,
'good': Autonomy.Off
},
remapping={'input_value': 'be_evil'})
# x:188 y:294
OperatableStateMachine.add(
'EvilLook',
TextCommandState(type='eyes/emotion',
command='evil_look',
topic=eyes_topic),
transitions={'done': 'WaitForMovementFinish'},
autonomy={'done': Autonomy.Off})
# x:880 y:311
OperatableStateMachine.add('NormalLook',
TextCommandState(type='eyes/emotion',
command='normal_look',
topic=eyes_topic),
transitions={'done': 'IdleBehavior'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:583 y:683, x:578 y:422
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.unused = None
_state_machine.userdata.rand_head_config = {'interval': [0.5, 1]}
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:278 y:24
OperatableStateMachine.add('SesectBehavior',
OperatorDecisionState(
outcomes=['dance', 'rand_moves'],
hint='Select behavior',
suggestion='dance'),
transitions={
'dance': 'TestMovement',
'rand_moves': 'RandHead'
},
autonomy={
'dance': Autonomy.Full,
'rand_moves': Autonomy.Full
})
# x:114 y:352
OperatableStateMachine.add(
'TurnOffJointStateController',
SetBoolState(
service='motion/controller/joint_state/set_operational',
value=False),
transitions={
'true': 'SingASong',
'false': 'failed',
'failure': 'failed'
},
autonomy={
'true': Autonomy.High,
'false': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={
'success': 'success',
'message': 'message'
})
# x:136 y:518
OperatableStateMachine.add('SingASong',
TextCommandState(type='voice/play_wav',
command='mmm_song',
topic='control'),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Full})
# x:248 y:213
OperatableStateMachine.add(
'TestMovement',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='dance14',
trajectory_param='/saved_msgs/joint_trajectory'),
transitions={
'success': 'TurnOffJointStateController',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Low,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:939 y:351
OperatableStateMachine.add(
'RandSelector',
DecisionState(
outcomes=['first', 'second'],
conditions=(lambda x: 'first'
if random.uniform(0, 1) < 0.5 else 'second')),
transitions={
'first': 'PlaceHead',
'second': 'TestMovement'
},
autonomy={
'first': Autonomy.Full,
'second': Autonomy.Full
},
remapping={'input_value': 'unused'})
# x:704 y:603
OperatableStateMachine.add(
'HeadShake',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='head_shake',
trajectory_param='/saved_msgs/joint_trajectory'),
transitions={
'success': 'finished',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:958 y:510
OperatableStateMachine.add('PlaceHead',
SrdfStateToMoveit(
config_name='head_basic',
move_group='head',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'HeadShake',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:725 y:125
OperatableStateMachine.add(
'RandHead',
SweetieBotRandHeadMovements(controller='joint_state_head',
duration=120,
interval=[3, 5],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={
'done': 'RandSelector',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'rand_head_config'})
return _state_machine
def create(self):
leap_topic = '/hmi/leap_motion/data'
eyes_cmd_topic = 'control'
voice_topic = 'control'
leap_pose_topic = '/hmi/leap_motion/pose'
# x:286 y:634, x:989 y:651
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.be_evil = False
_state_machine.userdata.pose = PoseStamped()
_state_machine.userdata.unused = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:353, x:343 y:356, x:209 y:349, x:843 y:453, x:473 y:355, x:638 y:361, x:907 y:552
_sm_randheadmovements_0 = ConcurrencyContainer(outcomes=['object_detected', 'failed'], input_keys=['unused'], conditions=[
('object_detected', [('WaitUntlObject', 'still_object')]),
('object_detected', [('WaitUntlObject', 'moving_object')]),
('failed', [('WaitUntlObject', 'no_object')]),
('object_detected', [('RandHeadMoveme', 'done')]),
('failed', [('RandHeadMoveme', 'failed')])
])
with _sm_randheadmovements_0:
# x:158 y:110
OperatableStateMachine.add('WaitUntlObject',
LeapMotionMonitor(leap_motion_topic=leap_topic, exit_states=['still_object', 'moving_object'], pose_topic=leap_pose_topic, parameters=[2.0,0.02,0.2]),
transitions={'no_object': 'failed', 'still_object': 'object_detected', 'moving_object': 'object_detected'},
autonomy={'no_object': Autonomy.Off, 'still_object': Autonomy.Off, 'moving_object': Autonomy.Off},
remapping={'pose': 'pose'})
# x:605 y:103
OperatableStateMachine.add('RandHeadMoveme',
SweetieBotRandHeadMovements(controller='joint_state_head', duration=120, interval=[1,4], max2356=[0.3,0.5,1,1], min2356=[-0.3,-0.1,-1,-1]),
transitions={'done': 'object_detected', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'config': 'unused'})
# x:30 y:353, x:357 y:355, x:671 y:353, x:140 y:353, x:237 y:356, x:530 y:353, x:742 y:441, x:730 y:353, x:452 y:353, x:732 y:406
_sm_waitnoobjecttimeout_1 = ConcurrencyContainer(outcomes=['no_object', 'timeout', 'failure', 'too_close'], conditions=[
('no_object', [('WaitNoObject', 'no_object')]),
('failure', [('WaitNoObject', 'moving_object')]),
('failure', [('WaitNoObject', 'still_object')]),
('failure', [('FollowObject', 'failed')]),
('timeout', [('Wait', 'done')]),
('too_close', [('FollowObject', 'too_close')])
])
with _sm_waitnoobjecttimeout_1:
# x:162 y:101
OperatableStateMachine.add('WaitNoObject',
LeapMotionMonitor(leap_motion_topic=leap_topic, exit_states=['no_object'], pose_topic=leap_pose_topic, parameters=[4.0,0.02,0.2]),
transitions={'no_object': 'no_object', 'still_object': 'failure', 'moving_object': 'failure'},
autonomy={'no_object': Autonomy.Off, 'still_object': Autonomy.Off, 'moving_object': Autonomy.Off},
remapping={'pose': 'pose'})
# x:463 y:95
OperatableStateMachine.add('FollowObject',
SweetieBotFollowHeadPoseSmart(pose_topic=leap_pose_topic, follow_joint_state_controller='joint_state_head', discomfort_time=4, neck_control_parameteres=[-0.13,0.3,0.20,0.2], deactivate=True, controlled_chains=['eyes','head']),
transitions={'failed': 'failure', 'too_close': 'too_close'},
autonomy={'failed': Autonomy.Off, 'too_close': Autonomy.Off})
# x:332 y:94
OperatableStateMachine.add('Wait',
WaitState(wait_time=self.play_timeout),
transitions={'done': 'timeout'},
autonomy={'done': Autonomy.Off})
# x:30 y:353, x:130 y:353, x:330 y:366, x:737 y:355, x:898 y:596, x:899 y:486, x:900 y:533, x:902 y:443, x:568 y:345
_sm_watchonleapmotionobject_2 = ConcurrencyContainer(outcomes=['no_object', 'still_object', 'failed', 'to_close'], output_keys=['pose'], conditions=[
('no_object', [('WaitTillObjectMOving', 'no_object')]),
('still_object', [('WaitTillObjectMOving', 'still_object')]),
('failed', [('WaitTillObjectMOving', 'moving_object')]),
('failed', [('HeadFollowsMovingObject', 'failed')]),
('to_close', [('HeadFollowsMovingObject', 'too_close')])
])
with _sm_watchonleapmotionobject_2:
# x:135 y:135
OperatableStateMachine.add('WaitTillObjectMOving',
LeapMotionMonitor(leap_motion_topic='/hmi/leap_motion/data', exit_states=['no_object', 'still_object'], pose_topic='/hmi/leap_motion/pose', parameters=[1.0,0.02,0.2]),
transitions={'no_object': 'no_object', 'still_object': 'still_object', 'moving_object': 'failed'},
autonomy={'no_object': Autonomy.High, 'still_object': Autonomy.High, 'moving_object': Autonomy.Off},
remapping={'pose': 'pose'})
# x:583 y:138
OperatableStateMachine.add('HeadFollowsMovingObject',
SweetieBotFollowHeadPoseSmart(pose_topic='/hmi/leap_motion/pose', follow_joint_state_controller='joint_state_head', discomfort_time=4.0, neck_control_parameteres=[-0.13,0.3,0.20,0.2], deactivate=True, controlled_chains=['head','eyes']),
transitions={'failed': 'failed', 'too_close': 'to_close'},
autonomy={'failed': Autonomy.Off, 'too_close': Autonomy.High})
# x:979 y:146, x:969 y:327
_sm_switchevil_3 = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['be_evil'], output_keys=['be_evil'])
with _sm_switchevil_3:
# x:269 y:101
OperatableStateMachine.add('CheckState',
DecisionState(outcomes=[ 'evil', 'good' ], conditions=lambda x: 'evil' if x else 'good'),
transitions={'evil': 'NewState1', 'good': 'NewState2'},
autonomy={'evil': Autonomy.Off, 'good': Autonomy.Off},
remapping={'input_value': 'be_evil'})
# x:423 y:54
OperatableStateMachine.add('NewState1',
CalculationState(calculation=lambda x: not random.random() < self.evil_to_good_switch_probability),
transitions={'done': 'SwitchEvilMode'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'be_evil', 'output_value': 'new_be_evil'})
# x:425 y:159
OperatableStateMachine.add('NewState2',
CalculationState(calculation=lambda x: random.random() < self.good_to_evil_switch_probability),
transitions={'done': 'SwitchEvilMode'},
autonomy={'done': Autonomy.Off},
remapping={'input_value': 'be_evil', 'output_value': 'new_be_evil'})
# x:678 y:91
OperatableStateMachine.add('SwitchEvilMode',
self.use_behavior(SwitchEvilModeSM, 'SwitchEvil/SwitchEvilMode'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil', 'new_be_evil': 'new_be_evil'})
with _state_machine:
# x:114 y:550
OperatableStateMachine.add('PlaceHead',
SrdfStateToMoveit(config_name='head_upright', move_group='head', action_topic='move_group', robot_name=''),
transitions={'reached': 'RandHeadMovements', 'planning_failed': 'failed', 'control_failed': 'failed', 'param_error': 'failed'},
autonomy={'reached': Autonomy.Off, 'planning_failed': Autonomy.Off, 'control_failed': Autonomy.Off, 'param_error': Autonomy.Off},
remapping={'config_name': 'config_name', 'move_group': 'move_group', 'robot_name': 'robot_name', 'action_topic': 'action_topic', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:500 y:88
OperatableStateMachine.add('Brohoof',
self.use_behavior(BrohoofSM, 'Brohoof'),
transitions={'finished': 'WaitNoObjectTimeout', 'unreachable': 'Greeting', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'unreachable': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'pose': 'pose'})
# x:699 y:28
OperatableStateMachine.add('Greeting',
self.use_behavior(GreetingSM, 'Greeting'),
transitions={'finished': 'WaitNoObjectTimeout', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:455 y:230
OperatableStateMachine.add('Play',
self.use_behavior(PlaySM, 'Play'),
transitions={'finished': 'WaitNoObjectTimeout', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:78 y:247
OperatableStateMachine.add('SwitchEvil',
_sm_switchevil_3,
transitions={'finished': 'WatchOnLeapMotionObject', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:387 y:6
OperatableStateMachine.add('CheckEvil',
DecisionState(outcomes=['evil','good'], conditions=lambda x: 'evil' if x else 'good'),
transitions={'evil': 'Greeting', 'good': 'Brohoof'},
autonomy={'evil': Autonomy.Off, 'good': Autonomy.Off},
remapping={'input_value': 'be_evil'})
# x:144 y:77
OperatableStateMachine.add('WatchOnLeapMotionObject',
_sm_watchonleapmotionobject_2,
transitions={'no_object': 'Play', 'still_object': 'CheckEvil', 'failed': 'failed', 'to_close': 'Bad'},
autonomy={'no_object': Autonomy.Inherit, 'still_object': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'to_close': Autonomy.Inherit},
remapping={'pose': 'pose'})
# x:283 y:293
OperatableStateMachine.add('Bad',
self.use_behavior(BadSM, 'Bad'),
transitions={'finished': 'SwitchEvil', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:646 y:375
OperatableStateMachine.add('WaitNoObjectTimeout',
_sm_waitnoobjecttimeout_1,
transitions={'no_object': 'PlaceHead', 'timeout': 'Play', 'failure': 'failed', 'too_close': 'Bad'},
autonomy={'no_object': Autonomy.Inherit, 'timeout': Autonomy.Inherit, 'failure': Autonomy.Inherit, 'too_close': Autonomy.Inherit})
# x:106 y:411
OperatableStateMachine.add('RandHeadMovements',
_sm_randheadmovements_0,
transitions={'object_detected': 'SwitchEvil', 'failed': 'failed'},
autonomy={'object_detected': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'unused': 'unused'})
return _state_machine
def create(self):
control_topic = 'control'
voice_topic = 'control'
joint_trajectory_action = 'motion/controller/joint_trajectory'
joy_topic = '/hmi/joystick'
moveit_action = 'move_group'
storage = 'joint_trajectory'
# x:157 y:271, x:501 y:342
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.joint_head_turned_right = [0.0, 0.6, 0.0, 0.0]
_state_machine.userdata.head_forward_joints = [0.15, 0.0, -0.15, 0.0]
_state_machine.userdata.unused = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:803 y:85, x:480 y:341
_sm_turnright_0 = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['joint_head_turned_right'])
with _sm_turnright_0:
# x:282 y:82
OperatableStateMachine.add(
'TurnHead',
MoveitToJointsState(
move_group='head',
joint_names=['joint51', 'joint52', 'joint53', 'joint54'],
action_topic=moveit_action),
transitions={
'reached': 'TurnBody',
'planning_failed': 'failed',
'control_failed': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off
},
remapping={'joint_config': 'joint_head_turned_right'})
# x:466 y:69
OperatableStateMachine.add('TurnBody',
CompoundAction(
t1=[0, 0.0],
type1='motion/joint_trajectory',
cmd1='turn_right',
t2=[1, 0.0],
type2='motion/joint_trajectory',
cmd2='turn_right',
t3=[2, 0.0],
type3='motion/joint_trajectory',
cmd3='turn_right',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'finished',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:30 y:353, x:148 y:355, x:529 y:348, x:443 y:347, x:840 y:329, x:752 y:347
_sm_waitbegining_1 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
input_keys=['unused'],
conditions=[('finished', [('WaitKey', 'received')]),
('failed', [('WaitKey', 'unavailable')]),
('failed', [('RandHeadMovemets', 'failed')]),
('finished', [('RandHeadMovemets', 'done')])])
with _sm_waitbegining_1:
# x:167 y:134
OperatableStateMachine.add('WaitKey',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: any(x.buttons),
buffered=False,
clear=False),
transitions={
'received': 'finished',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:442 y:126
OperatableStateMachine.add('RandHeadMovemets',
SweetieBotRandHeadMovements(
controller='joint_state_head',
duration=600,
interval=[3, 5],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'unused'})
with _state_machine:
# x:25 y:176
OperatableStateMachine.add('WaitBegining',
_sm_waitbegining_1,
transitions={
'finished': 'HeadPose',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'unused': 'unused'})
# x:343 y:49
OperatableStateMachine.add('WaitKey',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: any(x.buttons),
buffered=False,
clear=False),
transitions={
'received': 'TurnRight',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:785 y:172
OperatableStateMachine.add('AskAboutProto1',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='what_proto1',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='head_suprised_slow',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WaitKey2',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:799 y:264
OperatableStateMachine.add('WaitKey2',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: any(x.buttons),
buffered=False,
clear=False),
transitions={
'received': 'AskAboutProto1Again',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:797 y:379
OperatableStateMachine.add('AskAboutProto1Again',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='tell_me_more_about_proto1',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='head_lean_forward_begin',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WaitKey4',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:801 y:518
OperatableStateMachine.add(
'WaitKey4',
WaitForMessageState(topic=joy_topic,
condition=lambda x: any(x.buttons),
buffered=False,
clear=False),
transitions={
'received': 'AskWhatHappensToProto1',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:603 y:600
OperatableStateMachine.add('AskWhatHappensToProto1',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='what_happened_with_proto1',
t2=[0, 1.2],
type2='motion/joint_trajectory',
cmd2='hoof_wave',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WaitKey3',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:332 y:601
OperatableStateMachine.add('WaitKey3',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: any(x.buttons),
buffered=False,
clear=False),
transitions={
'received': 'SayItIsOk',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:554 y:51
OperatableStateMachine.add('TurnRight',
_sm_turnright_0,
transitions={
'finished': 'WatchPresentaion',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'joint_head_turned_right':
'joint_head_turned_right'
})
# x:81 y:597
OperatableStateMachine.add('SayItIsOk',
CompoundAction(
t1=[0, 0.0],
type1='motion/joint_trajectory',
cmd1='head_lean_forward_end',
t2=[0, 0.0],
type2='voice/play_wav',
cmd2='yes_exactly_tell_me_more',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WatchPresentaion_2',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:137 y:50
OperatableStateMachine.add(
'Hello',
CompoundAction(t1=[0, 0.0],
type1='motion/joint_trajectory',
cmd1='introduce_herself',
t2=[0, 0.0],
type2='voice/play_wav',
cmd2='hello_im_sweetie_bot_presentation',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WaitKey',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:166 y:159
OperatableStateMachine.add('HeadPose',
SrdfStateToMoveit(
config_name='head_basic',
move_group='head',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'Hello',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:776 y:30
OperatableStateMachine.add(
'WatchPresentaion',
self.use_behavior(WatchPresentaionSM,
'WatchPresentaion',
default_keys=['rand_head_config']),
transitions={
'finished': 'AskAboutProto1',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'head_pose_joints': 'joint_head_turned_right',
'rand_head_config': 'rand_head_config'
})
# x:92 y:465
OperatableStateMachine.add(
'WatchPresentaion_2',
self.use_behavior(WatchPresentaionSM,
'WatchPresentaion_2',
default_keys=['rand_head_config']),
transitions={
'finished': 'Greeting',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={
'head_pose_joints': 'head_forward_joints',
'rand_head_config': 'rand_head_config'
})
# x:73 y:332
OperatableStateMachine.add(
'Greeting',
ExecuteJointTrajectory(
action_topic=joint_trajectory_action,
trajectory_param='greeting',
trajectory_ns='/saved_msgs/joint_trajectory'),
transitions={
'success': 'finished',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
return _state_machine
def create(self):
storage = 'joint_trajectory/'
joint_state_control_topic = 'motion/controller/joint_state/out_joints_src_reset'
eyes_topic = 'control'
voice_topic = 'control'
joint_trajectory_action = 'motion/controller/joint_trajectory'
# x:322 y:652, x:984 y:622
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.counter = 0
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:220 y:573, x:535 y:559
_sm_seizure_0 = OperatableStateMachine(outcomes=['finished', 'failed'],
input_keys=['counter'])
with _sm_seizure_0:
# x:125 y:111
OperatableStateMachine.add('SayDoNotTouch',
TextCommandState(type='voice/play_wav',
command='05donottouch',
topic=voice_topic),
transitions={'done': 'Seizure'},
autonomy={'done': Autonomy.Off})
# x:389 y:247
OperatableStateMachine.add(
'AddCounter',
CalculationState(calculation=lambda x: x + 1),
transitions={'done': 'Seizure'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'counter',
'output_value': 'counter'
})
# x:412 y:87
OperatableStateMachine.add(
'Select',
DecisionState(
outcomes=['say1', 'say2', 'say3', 'end'],
conditions=lambda x: ['say1', 'say2', 'say3', 'end'][x]),
transitions={
'say1': 'SayWalk',
'say2': 'SayBlaster',
'say3': 'SayWin',
'end': 'Farewell'
},
autonomy={
'say1': Autonomy.Off,
'say2': Autonomy.Off,
'say3': Autonomy.Off,
'end': Autonomy.High
},
remapping={'input_value': 'counter'})
# x:612 y:81
OperatableStateMachine.add('SayWalk',
TextCommandState(type='voice/play_wav',
command='17walk',
topic=voice_topic),
transitions={'done': 'AddCounter'},
autonomy={'done': Autonomy.Off})
# x:681 y:173
OperatableStateMachine.add('SayBlaster',
TextCommandState(type='voice/play_wav',
command='18blaster',
topic=voice_topic),
transitions={'done': 'AddCounter'},
autonomy={'done': Autonomy.Off})
# x:686 y:240
OperatableStateMachine.add('SayWin',
TextCommandState(type='voice/play_wav',
command='08win',
topic=voice_topic),
transitions={'done': 'AddCounter'},
autonomy={'done': Autonomy.Off})
# x:85 y:238
OperatableStateMachine.add(
'Seizure',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='seizure',
trajectory_ns=storage),
transitions={
'success': 'Select',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:132 y:419
OperatableStateMachine.add(
'Farewell',
ExecuteJointTrajectory(action_topic=joint_trajectory_action,
trajectory_param='farewell',
trajectory_ns=storage),
transitions={
'success': 'finished',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
with _state_machine:
# x:15 y:642
OperatableStateMachine.add('RandomMovements',
SweetieBotRandHeadMovements(
controller='joint_state_head',
duration=100000,
interval=[2, 3],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={'done': 'StartPose'},
autonomy={'done': Autonomy.High})
# x:124 y:352
OperatableStateMachine.add('WaitSayIamAlive',
WaitState(wait_time=1),
transitions={'done': 'WarmUp'},
autonomy={'done': Autonomy.Off})
# x:102 y:154
OperatableStateMachine.add(
'LookAtHoof',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='look_on_hoof',
trajectory_ns=storage),
transitions={
'success': 'SayIamAlive',
'partial_movement': 'finished',
'invalid_pose': 'finished',
'failure': 'finished'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:96 y:484
OperatableStateMachine.add(
'WarmUp',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='little_shake_fast',
trajectory_ns=storage),
transitions={
'success': 'WaitBeforeDomination',
'partial_movement': 'finished',
'invalid_pose': 'finished',
'failure': 'finished'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:528 y:547
OperatableStateMachine.add('SayMenace',
TextCommandState(
type='voice/play_wav',
command='02technic',
topic='/sweetie_bot/voice/voice'),
transitions={'done': 'WaitSayMenace'},
autonomy={'done': Autonomy.Off})
# x:529 y:454
OperatableStateMachine.add('WaitSayMenace',
WaitState(wait_time=2.3),
transitions={'done': 'EvilLook'},
autonomy={'done': Autonomy.Off})
# x:396 y:219
OperatableStateMachine.add(
'Menace',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='menace',
trajectory_ns=storage),
transitions={
'success': 'TryingToAcquireControl',
'partial_movement': 'finished',
'invalid_pose': 'finished',
'failure': 'finished'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:431 y:107
OperatableStateMachine.add('TryingToAcquireControl',
WaitState(wait_time=10),
transitions={'done': 'SadLook'},
autonomy={'done': Autonomy.Off})
# x:429 y:301
OperatableStateMachine.add('SayBattleSystemActivation',
TextCommandState(
type='voice/play_wav',
command='03domination',
topic='/sweetie_bot/voice/voice'),
transitions={'done': 'Menace'},
autonomy={'done': Autonomy.Off})
# x:840 y:91
OperatableStateMachine.add('SayDominationFailed',
TextCommandState(
type='voice/play_wav',
command='04notfound',
topic='/sweetie_bot/voice/voice'),
transitions={'done': 'WaitFailed'},
autonomy={'done': Autonomy.Off})
# x:818 y:281
OperatableStateMachine.add(
'MenceCanceled',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='menace_canceled',
trajectory_ns=storage),
transitions={
'success': 'WaitBeforeHoofStamp',
'partial_movement': 'finished',
'invalid_pose': 'finished',
'failure': 'finished'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:885 y:188
OperatableStateMachine.add('WaitFailed',
WaitState(wait_time=1),
transitions={'done': 'MenceCanceled'},
autonomy={'done': Autonomy.Off})
# x:139 y:573
OperatableStateMachine.add('WaitBeforeDomination',
WaitState(wait_time=2),
transitions={'done': 'RedEyes'},
autonomy={'done': Autonomy.Off})
# x:856 y:396
OperatableStateMachine.add('WaitBeforeHoofStamp',
WaitState(wait_time=1),
transitions={'done': 'HoofStamp'},
autonomy={'done': Autonomy.Off})
# x:826 y:490
OperatableStateMachine.add(
'HoofStamp',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='hoof_stamp',
trajectory_ns=storage),
transitions={
'success': 'NormalLook',
'partial_movement': 'finished',
'invalid_pose': 'finished',
'failure': 'finished'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:385 y:567
OperatableStateMachine.add('RedEyes',
TextCommandState(type='eyes/emotion',
command='red_eyes',
topic=eyes_topic),
transitions={'done': 'SayMenace'},
autonomy={'done': Autonomy.Off})
# x:151 y:77
OperatableStateMachine.add(
'LookAround',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='look_around',
trajectory_ns=storage),
transitions={
'success': 'LookAtHoof',
'partial_movement': 'finished',
'invalid_pose': 'finished',
'failure': 'finished'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
},
remapping={'result': 'result'})
# x:375 y:18
OperatableStateMachine.add('Wait',
WaitState(wait_time=1),
transitions={'done': 'LookAround'},
autonomy={'done': Autonomy.Off})
# x:195 y:10
OperatableStateMachine.add('NormalEyes',
TextCommandState(type='eyes/emotion',
command='normal',
topic=eyes_topic),
transitions={'done': 'Wait'},
autonomy={'done': Autonomy.Off})
# x:519 y:374
OperatableStateMachine.add(
'EvilLook',
TextCommandState(type='eyes/emotion',
command='evil_look',
topic=eyes_topic),
transitions={'done': 'SayBattleSystemActivation'},
autonomy={'done': Autonomy.Off})
# x:696 y:85
OperatableStateMachine.add(
'SadLook',
TextCommandState(type='eyes/emotion',
command='sad_look',
topic=eyes_topic),
transitions={'done': 'SayDominationFailed'},
autonomy={'done': Autonomy.Off})
# x:817 y:569
OperatableStateMachine.add('NormalLook',
TextCommandState(type='eyes/emotion',
command='normal_look',
topic=eyes_topic),
transitions={'done': 'Seizure'},
autonomy={'done': Autonomy.High})
# x:1124 y:445
OperatableStateMachine.add('Seizure',
_sm_seizure_0,
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'counter': 'counter'})
# x:40 y:35
OperatableStateMachine.add(
'StartPose',
PublisherState(topic=joint_state_control_topic,
msg_type=JointState,
value={
'name': [
'joint51', 'joint52', 'joint53',
'eyes_pitch', 'eyes_yaw'
],
'position': [0.2, 0.0, 0, 0.0, 0.0]
}),
transitions={
'done': 'NormalEyes',
'failed': 'finished'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
})
# x:127 y:254
OperatableStateMachine.add('SayIamAlive',
TextCommandState(
type='voice/play_wav',
command='01alive',
topic='/sweetie_bot/voice/voice'),
transitions={'done': 'WaitSayIamAlive'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:902 y:403, x:697 y:491
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.be_evil = False
_state_machine.userdata.unused = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:32 y:127
OperatableStateMachine.add('SetStandingPose',
SetJointState(controller='motion/controller/joint_state_head', pose_param='body_nominal', pose_ns='saved_msgs/joint_state', tolerance=0.017, timeout=10.0, joint_topic="joint_states"),
transitions={'done': 'SelectTrick', 'failed': 'failed', 'timeout': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off, 'timeout': Autonomy.Off})
# x:371 y:161
OperatableStateMachine.add('Greeting',
self.use_behavior(GreetingSM, 'Greeting'),
transitions={'finished': 'RandHead', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:371 y:284
OperatableStateMachine.add('Play',
self.use_behavior(PlaySM, 'Play'),
transitions={'finished': 'RandHead', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:202 y:154
OperatableStateMachine.add('SelectTrick',
DecisionState(outcomes=['play','cheer','greet'], conditions=lambda x: random.choice(['play','cheer','greet'])),
transitions={'play': 'Play', 'cheer': 'Cheer', 'greet': 'Greeting'},
autonomy={'play': Autonomy.Off, 'cheer': Autonomy.Off, 'greet': Autonomy.Off},
remapping={'input_value': 'unused'})
# x:823 y:274
OperatableStateMachine.add('PrepareToWalk',
ExecuteJointTrajectory(action_topic='motion/controller/joint_trajectory', trajectory_param='crouch_begin', trajectory_ns='saved_msgs/joint_trajectory'),
transitions={'success': 'finished', 'partial_movement': 'failed', 'invalid_pose': 'failed', 'failure': 'failed'},
autonomy={'success': Autonomy.Off, 'partial_movement': Autonomy.Off, 'invalid_pose': Autonomy.Off, 'failure': Autonomy.Off})
# x:376 y:38
OperatableStateMachine.add('Cheer',
self.use_behavior(CheerSM, 'Cheer'),
transitions={'finished': 'RandHead', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'be_evil': 'be_evil'})
# x:774 y:45
OperatableStateMachine.add('RandHead',
SweetieBotRandHeadMovements(controller='joint_state_head', duration=8, interval=[3,5], max2356=[0.3,0.3,1.5,1.5], min2356=[-0.3,-0.3,-1.5,-1.5]),
transitions={'done': 'SetHeadNominal', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'config': 'unused'})
# x:809 y:138
OperatableStateMachine.add('SetHeadNominal',
SetJointState(controller='motion/controller/joint_state_head', pose_param='head_nominal', pose_ns='saved_msgs/joint_state', tolerance=0.017, timeout=10.0, joint_topic="joint_states"),
transitions={'done': 'PrepareToWalk', 'failed': 'failed', 'timeout': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off, 'timeout': Autonomy.Off})
return _state_machine
def create(self):
joy_topic = '/hmi/joystick'
action_button = 12
# x:105 y:208, x:418 y:227
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.rand_head_config = {
'min2356': [-0.5, 0.1, -1.0, -1.0],
'max2356': [0.5, 0.5, 1.0, 1.0]
}
_state_machine.userdata.head_pose_joints = [0.0, 0.0, 0.0, 0.0, 0.0]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:284, x:130 y:284, x:230 y:284, x:330 y:284, x:430 y:293, x:530 y:293
_sm_waitstart_0 = ConcurrencyContainer(
outcomes=['finished', 'failed'],
input_keys=['rand_head_config'],
conditions=[('finished', [('WaitKey1', 'received')]),
('failed', [('WaitKey1', 'unavailable')]),
('finished', [('RandMovements', 'done')]),
('failed', [('RandMovements', 'failed')])])
with _sm_waitstart_0:
# x:98 y:61
OperatableStateMachine.add(
'WaitKey1',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: x.buttons[action_button],
buffered=False,
clear=False),
transitions={
'received': 'finished',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:335 y:61
OperatableStateMachine.add(
'RandMovements',
SweetieBotRandHeadMovements(controller='joint_state_head',
duration=120,
interval=[1, 4],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'rand_head_config'})
with _state_machine:
# x:52 y:35
OperatableStateMachine.add('NormalEyes',
TextCommandState(type='eyes/emotion',
command='normal',
topic='control'),
transitions={'done': 'WaitStart'},
autonomy={'done': Autonomy.Off})
# x:642 y:281
OperatableStateMachine.add(
'HelloGoodThenEvil',
CompoundAction(t1=[0, 0.0],
type1='voice/play_wav',
cmd1='zdravstvuyte_dorogie_gosti',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='look_on_printer',
t3=[2, 0.0],
type3='motion/joint_trajectory',
cmd3='menace',
t4=[3, 4.0],
type4='motion/joint_trajectory',
cmd4='menace_canceled'),
transitions={
'success': 'MicrophoneTurnOffReaction',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:634 y:65
OperatableStateMachine.add('FakeApoplogy',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='a_tochno_sboi_pamayati',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='head_suprised',
t3=[0, 2.8],
type3='motion/joint_trajectory',
cmd3='head_node',
t4=[0, 6.5],
type4='motion/joint_trajectory',
cmd4='hoof_knock'),
transitions={
'success': 'WaitKey3',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:467 y:314
OperatableStateMachine.add(
'WaitKey4',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: x.buttons[action_button],
buffered=False,
clear=False),
transitions={
'received': 'HeadTurnRightEnd',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:253 y:412
OperatableStateMachine.add(
'FakeApologyAndMenace',
CompoundAction(t1=[0, 0.0],
type1='voice/play_wav',
cmd1='no_esli_snova_otkluchish_mikrofon',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='hoof_knock',
t3=[2, 0.0],
type3='motion/joint_trajectory',
cmd3='head_node',
t4=[0, 6.5],
type4='motion/joint_trajectory',
cmd4='begone'),
transitions={
'success': 'NormalLook',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:74 y:386
OperatableStateMachine.add(
'WaitKey5',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: x.buttons[action_button],
buffered=False,
clear=False),
transitions={
'received': 'LetsBegin',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:48 y:277
OperatableStateMachine.add('LetsBegin',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='nam_pora_nachinat',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='head_node',
t3=[1, 1.0],
type3='motion/joint_trajectory',
cmd3='point_right',
t4=[0, 7.0],
type4='motion/joint_trajectory',
cmd4='hoof_stamp'),
transitions={
'success': 'finished',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:439 y:35
OperatableStateMachine.add(
'WaitKey2',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: x.buttons[action_button],
buffered=False,
clear=False),
transitions={
'received': 'FakeApoplogy',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
# x:236 y:35
OperatableStateMachine.add('HelloEvil',
CompoundAction(
t1=[0, 0.0],
type1='voice/play_wav',
cmd1='zdravstvuyte_kojannie_meshki',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='greeting',
t3=[0, 0.0],
type3='eyes/emotion',
cmd3='red_eyes',
t4=[0, 5.5],
type4='motion/joint_trajectory',
cmd4='applause'),
transitions={
'success': 'WaitKey2',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:243 y:129
OperatableStateMachine.add('PlaceHead',
SrdfStateToMoveit(
config_name='head_basic',
move_group='head',
action_topic='move_group',
robot_name=''),
transitions={
'reached': 'HelloEvil',
'planning_failed': 'failed',
'control_failed': 'failed',
'param_error': 'failed'
},
autonomy={
'reached': Autonomy.Off,
'planning_failed': Autonomy.Off,
'control_failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'config_name': 'config_name',
'move_group': 'move_group',
'robot_name': 'robot_name',
'action_topic': 'action_topic',
'joint_values': 'joint_values',
'joint_names': 'joint_names'
})
# x:73 y:124
OperatableStateMachine.add(
'WaitStart',
_sm_waitstart_0,
transitions={
'finished': 'PlaceHead',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'rand_head_config': 'rand_head_config'})
# x:642 y:365
OperatableStateMachine.add('MicrophoneTurnOffReaction',
CompoundAction(
t1=[0, 0.0],
type1='eyes/emotion',
cmd1='evil_look',
t2=[0, 0.0],
type2='motion/joint_trajectory',
cmd2='head_turn_right_begin',
t3=[2, 0.0],
type3='motion/joint_trajectory',
cmd3='hoof_stamp',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WaitKey4',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:231 y:312
OperatableStateMachine.add('NormalLook',
CompoundAction(t1=[0, 0.0],
type1='eyes/emotion',
cmd1='normal',
t2=[1, 0.0],
type2='eyes/emotion',
cmd2='red_eyes',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'WaitKey5',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:452 y:401
OperatableStateMachine.add('HeadTurnRightEnd',
CompoundAction(
t1=[0, 0.0],
type1='motion/joint_trajectory',
cmd1='head_turn_right_end',
t2=[0, 0.0],
type2=None,
cmd2='',
t3=[0, 0.0],
type3=None,
cmd3='',
t4=[0, 0.0],
type4=None,
cmd4=''),
transitions={
'success': 'FakeApologyAndMenace',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'failure': Autonomy.Off
})
# x:628 y:165
OperatableStateMachine.add(
'WaitKey3',
WaitForMessageState(
topic=joy_topic,
condition=lambda x: x.buttons[action_button],
buffered=False,
clear=False),
transitions={
'received': 'HelloGoodThenEvil',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'message'})
return _state_machine
def create(self):
# x:30 y:353, x:488 y:318
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.unused = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:189 y:30
OperatableStateMachine.add(
'CheckCrouched',
CheckJointState(outcomes=['body_crouched', 'unknown'],
pose_ns='saved_msgs/joint_state',
tolerance=0.17,
joint_topic="joint_states",
timeout=1.0),
transitions={
'body_crouched': 'StandUp',
'unknown': 'RandHeadMovements'
},
autonomy={
'body_crouched': Autonomy.Off,
'unknown': Autonomy.Off
})
# x:196 y:256
OperatableStateMachine.add(
'HeadNominal',
SetJointState(controller='motion/controller/joint_state_head',
pose_param='head_nominal',
pose_ns='saved_msgs/joint_state',
tolerance=0.017,
timeout=10.0,
joint_topic="joint_states"),
transitions={
'done': 'finished',
'failed': 'failed',
'timeout': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off,
'timeout': Autonomy.Off
})
# x:445 y:78
OperatableStateMachine.add(
'StandUp',
ExecuteJointTrajectory(
action_topic='motion/controller/joint_trajectory',
trajectory_param='crouch_end',
trajectory_ns='saved_msgs/joint_trajectory'),
transitions={
'success': 'RandHeadMovements',
'partial_movement': 'failed',
'invalid_pose': 'failed',
'failure': 'failed'
},
autonomy={
'success': Autonomy.Off,
'partial_movement': Autonomy.Off,
'invalid_pose': Autonomy.Off,
'failure': Autonomy.Off
})
# x:176 y:144
OperatableStateMachine.add('RandHeadMovements',
SweetieBotRandHeadMovements(
controller='joint_state_head',
duration=self.timeout,
interval=[1, 4],
max2356=[0.3, 0.3, 1.5, 1.5],
min2356=[-0.3, -0.3, -1.5, -1.5]),
transitions={
'done': 'HeadNominal',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'config': 'unused'})
return _state_machine
def create(self):
# x:98 y:490, x:1089 y:260
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.be_evil = False
_state_machine.userdata.key_pressed_msg = KeyPressed()
_state_machine.userdata.config = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
# x:30 y:353, x:558 y:270, x:472 y:279, x:284 y:286, x:430 y:353, x:530 y:353, x:630 y:353
_sm_waitkeypressed_0 = ConcurrencyContainer(outcomes=['pressed', 'timeout', 'failed'], input_keys=['key_pressed_msg', 'config'], output_keys=['key_pressed_msg'], conditions=[
('failed', [('RandMovements', 'failed')]),
('timeout', [('RandMovements', 'done')]),
('pressed', [('WaitKey', 'received')]),
('failed', [('WaitKey', 'unavailable')])
])
with _sm_waitkeypressed_0:
# x:70 y:100
OperatableStateMachine.add('WaitKey',
WaitForMessageState(topic='/hmi/joy_decoder/keys_pressed', condition=lambda x: x.keys != [], buffered=False, clear=True),
transitions={'received': 'pressed', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'key_pressed_msg'})
# x:423 y:91
OperatableStateMachine.add('RandMovements',
SweetieBotRandHeadMovements(controller='joint_state_head', duration=self.timeout, interval=[3,5], max2356=[0.3,0.3,1.5,1.5], min2356=[-0.3,-0.3,-1.5,-1.5]),
transitions={'done': 'timeout', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'config': 'config'})
with _state_machine:
# x:217 y:310
OperatableStateMachine.add('SetHeadPoseNominal',
SetJointState(controller='motion/controller/joint_state_head', pose_param='nominal', pose_ns='saved_msgs/joint_state', tolerance=0.017, timeout=10.0, joint_topic="joint_states"),
transitions={'done': 'WaitKeyPressed', 'failed': 'failed', 'timeout': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off, 'timeout': Autonomy.Off})
# x:662 y:48
OperatableStateMachine.add('JoyAnimation',
self.use_behavior(JoyAnimationSM, 'JoyAnimation'),
transitions={'timeout': 'SetHeadPoseNominal', 'failed': 'failed', 'unknown_keys': 'JoyWalk'},
autonomy={'timeout': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'unknown_keys': Autonomy.Inherit},
remapping={'key_pressed_msg': 'key_pressed_msg', 'be_evil': 'be_evil'})
# x:841 y:220
OperatableStateMachine.add('JoyWalk',
self.use_behavior(JoyWalkSM, 'JoyWalk'),
transitions={'timeout': 'SetHeadPoseNominal', 'failed': 'failed', 'invalid_pose': 'failed', 'unknown_keys': 'CheckKeys'},
autonomy={'timeout': Autonomy.Inherit, 'failed': Autonomy.Inherit, 'invalid_pose': Autonomy.Inherit, 'unknown_keys': Autonomy.Inherit},
remapping={'key_pressed_msg': 'key_pressed_msg'})
# x:641 y:329
OperatableStateMachine.add('CheckKeys',
DecisionState(outcomes=['walk','animation','start', 'unknown'], conditions=self.decision),
transitions={'walk': 'JoyWalk', 'animation': 'JoyAnimation', 'start': 'RandomChoice', 'unknown': 'WaitKeyPressed'},
autonomy={'walk': Autonomy.Off, 'animation': Autonomy.Off, 'start': Autonomy.Off, 'unknown': Autonomy.Off},
remapping={'input_value': 'key_pressed_msg'})
# x:443 y:565
OperatableStateMachine.add('AutonomousBehavior2',
self.use_behavior(AutonomousBehavior2SM, 'AutonomousBehavior2'),
transitions={'finished': 'SetHeadPoseNominal', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:194 y:135
OperatableStateMachine.add('WaitKeyPressed',
_sm_waitkeypressed_0,
transitions={'pressed': 'JoyAnimation', 'timeout': 'RandomChoice', 'failed': 'failed'},
autonomy={'pressed': Autonomy.Inherit, 'timeout': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'key_pressed_msg': 'key_pressed_msg', 'config': 'config'})
# x:545 y:431
OperatableStateMachine.add('RandomChoice',
DecisionState(outcomes=['one','two'], conditions=lambda x: 'one' if random.random()<0.0 else 'two'),
transitions={'one': 'WaitKeyPressed', 'two': 'WaitKeyPressed'},
autonomy={'one': Autonomy.Off, 'two': Autonomy.Off},
remapping={'input_value': 'config'})
# x:647 y:599
OperatableStateMachine.add('AutonomousBehavior',
self.use_behavior(AutonomousBehaviorSM, 'AutonomousBehavior'),
transitions={'finished': 'SetHeadPoseNominal', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
return _state_machine