def test_invalid_userdata_output(self):
class SetInvalidOutput(EventState):
def __init__(self):
super(SetInvalidOutput, self).__init__(outcomes=['done'], output_keys=['output'])
def execute(self, userdata):
userdata.invalid = False
return 'done'
sm = OperatableStateMachine(outcomes=['done'])
with sm:
OperatableStateMachine.add('state', SetInvalidOutput(), transitions={'done': 'done'})
outcome = sm.execute(None)
self.assertIsNone(outcome)
self.assertIsInstance(sm._last_exception, UserDataError)
def test_missing_userdata(self):
class AccessValidInput(EventState):
def __init__(self):
super(AccessValidInput, self).__init__(outcomes=['done'], input_keys=['missing'])
def execute(self, userdata):
print(userdata.missing)
return 'done'
sm = OperatableStateMachine(outcomes=['done'])
with sm:
OperatableStateMachine.add('state', AccessValidInput(), transitions={'done': 'done'})
outcome = sm.execute(None)
self.assertIsNone(outcome)
self.assertIsInstance(sm._last_exception, UserDataError)
def create(self):
# x:541 y:178, x:142 y:179
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'], input_keys=['agv_id', 'shipment_type'])
_state_machine.userdata.agv_id = ''
_state_machine.userdata.shipment_type = ''
_state_machine.userdata.inspection_result = ''
_state_machine.userdata.succes = 0
_state_machine.userdata.agv_state = ''
_state_machine.userdata.agv_ready_state = 'ready_to_deliver'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:40 y:32
OperatableStateMachine.add('ShipmentReady',
NotifyShipmentReadyState(),
transitions={'continue': 'Wait', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'agv_id': 'agv_id', 'shipment_type': 'shipment_type', 'success': 'success', 'message': 'message'})
# x:222 y:29
OperatableStateMachine.add('Wait',
WaitState(wait_time=0.2),
transitions={'done': 'AgvState'},
autonomy={'done': Autonomy.Off})
# x:320 y:235
OperatableStateMachine.add('AGVReady',
EqualState(),
transitions={'true': 'finished', 'false': 'Wait'},
autonomy={'true': Autonomy.Off, 'false': Autonomy.Off},
remapping={'value_a': 'agv_state', 'value_b': 'agv_ready_state'})
# x:354 y:27
OperatableStateMachine.add('AgvState',
GetAgvStatusState(),
transitions={'continue': 'AGVReady', 'fail': 'failed'},
autonomy={'continue': Autonomy.Off, 'fail': Autonomy.Off},
remapping={'agv_id': 'agv_id', 'agv_state': 'agv_state'})
return _state_machine
def create(self):
# x:30 y:322
_state_machine = OperatableStateMachine(outcomes=['done'], input_keys=['pose'])
_state_machine.userdata.pose_position_y = 0
_state_machine.userdata.pose_position_x = 0
_state_machine.userdata.pose_orientation_y = 0
_state_machine.userdata.pose_orientation_z = 0
_state_machine.userdata.pose_orientation_w = 0
_state_machine.userdata.pose_orientation_x = 0
_state_machine.userdata.task_details_task_id = 0
_state_machine.userdata.params_distance = 0
_state_machine.userdata.pose_position_z = 0
_state_machine.userdata.goalId = ''
_state_machine.userdata.pose = PoseStamped()
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:195 y:14
OperatableStateMachine.add('DriveTo',
self.use_behavior(DriveToSM, 'DriveTo'),
transitions={'succeeded': 'confirm_victim', 'aborted': 'confirm_victim'},
autonomy={'succeeded': Autonomy.Inherit, 'aborted': Autonomy.Inherit},
remapping={'goalId': 'goalId'})
# x:337 y:152
OperatableStateMachine.add('wait',
Wait_getCloserVictim(),
transitions={'waiting': 'wait', 'restart': 'DriveTo', 'preempted': 'done'},
autonomy={'waiting': Autonomy.Off, 'restart': Autonomy.Off, 'preempted': Autonomy.Off},
remapping={'task_details_task_id': 'task_details_task_id', 'params_distance': 'params_distance', 'pose_position_x': 'pose_position_x', 'pose_position_y': 'pose_position_y', 'pose_position_z': 'pose_position_z', 'pose_orientation_x': 'pose_orientation_x', 'pose_orientation_y': 'pose_orientation_y', 'pose_orientation_z': 'pose_orientation_z', 'pose_orientation_w': 'pose_orientation_w'})
# x:54 y:157
OperatableStateMachine.add('confirm_victim',
confirm_victim(),
transitions={'succeeded': 'wait'},
autonomy={'succeeded': Autonomy.Off},
remapping={'task_details_task_id': 'task_details_task_id'})
return _state_machine
def create(self):
# x:123 y:357, x:288 y:339
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.Pose_Init = "IdlePose"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:42 y:58
OperatableStateMachine.add(
'Hi',
SaraSay(sentence="Hi, my name is Sara and I will start",
input_keys=[],
emotion=0,
block=True),
transitions={'done': 'setHead'},
autonomy={'done': Autonomy.Off})
# x:77 y:210
OperatableStateMachine.add('SetArm',
MoveitMove(move=True,
waitForExecution=True,
group="RightArm",
watchdog=15),
transitions={
'done': 'finished',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'target': 'Pose_Init'})
# x:72 y:130
OperatableStateMachine.add('setHead',
SaraSetHeadAngle(pitch=0, yaw=0),
transitions={'done': 'SetArm'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
control_goto_altitude_topic = "control_manager/goto_altitude"
control_mpc_tracker_diagnostic_topic = "control_manager/mpc_tracker/diagnostics"
# x:683 y:90, x:283 y:190
_state_machine = OperatableStateMachine(outcomes=['reached', 'failed'],
input_keys=['goal'])
_state_machine.userdata.goal = 3
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:97 y:74
OperatableStateMachine.add(
'Call_GoTo_Altitude',
ServiceGoToAltitudeState(
service_topic=control_goto_altitude_topic),
transitions={
'successed': 'Wait_Till_Reaching_Goal',
'failed': 'failed'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'goal': 'goal'})
# x:381 y:74
OperatableStateMachine.add(
'Wait_Till_Reaching_Goal',
WaitForMsgState(topic=control_mpc_tracker_diagnostic_topic,
wait_time=-1,
function=self.mpc_tracker_diagnostics_cb),
transitions={
'successed': 'reached',
'failed': 'failed'
},
autonomy={
'successed': Autonomy.Off,
'failed': Autonomy.Off
})
return _state_machine
def create(self):
# x:30 y:365, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.Direction = 'Battery_in'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:72 y:44
OperatableStateMachine.add('get_pose',
SubscriberState(topic='/pose',
blocking=True,
clear=False),
transitions={
'received':
'enter_charging_station',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'curr_pose'})
# x:226 y:130
OperatableStateMachine.add('enter_charging_station',
MoveBaseState(),
transitions={
'arrived': 'finished',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'Direction': 'Direction',
'curr_pose': 'curr_pose'
})
return _state_machine
def create(self):
# x:30 y:636, x:388 y:202
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.ln1 = 1
_state_machine.userdata.ln2 = 2
_state_machine.userdata.ln3 = 3
_state_machine.userdata.ln4 = 4
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Leg 1 - drive to post',
self.use_behavior(DrivetopostSM, 'Leg 1 - drive to post'),
transitions={'finished': 'Leg 2 - drive to post', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'leg_number': 'ln1'})
# x:30 y:138
OperatableStateMachine.add('Leg 2 - drive to post',
self.use_behavior(DrivetopostSM, 'Leg 2 - drive to post'),
transitions={'finished': 'Leg 3 - drive to post', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'leg_number': 'ln2'})
# x:30 y:236
OperatableStateMachine.add('Leg 3 - drive to post',
self.use_behavior(DrivetopostSM, 'Leg 3 - drive to post'),
transitions={'finished': 'Leg 4 - drive through gate', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'leg_number': 'ln3'})
# x:30 y:334
OperatableStateMachine.add('Leg 4 - drive through gate',
self.use_behavior(DrivethroughgateSM, 'Leg 4 - drive through gate'),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit},
remapping={'leg_number': 'ln4'})
return _state_machine
def create(self):
# x:705 y:41
_state_machine = OperatableStateMachine(outcomes=['finished'])
_state_machine.userdata.MaterialLocations = []
_state_machine.userdata.Part = 'piston_rod_part'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:34 y:25
OperatableStateMachine.add(
'StartAssignment',
StartAssignment(),
transitions={'continue': 'GetMaterialsLocations'},
autonomy={'continue': Autonomy.Off})
# x:362 y:28
OperatableStateMachine.add(
'MaterialsLocationMessage',
MessageState(),
transitions={'continue': 'EndAssignment'},
autonomy={'continue': Autonomy.Off},
remapping={'message': 'MaterialLocations'})
# x:541 y:29
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:176 y:26
OperatableStateMachine.add(
'GetMaterialsLocations',
GetMaterialLocationsState(),
transitions={'continue': 'MaterialsLocationMessage'},
autonomy={'continue': Autonomy.Off},
remapping={
'part': 'Part',
'material_locations': 'MaterialLocations'
})
return _state_machine
def create(self):
# x:628 y:51, x:130 y:406
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.config_name_left = 'Left_Home'
_state_machine.userdata.config_name_right = 'Right_Home'
_state_machine.userdata.config_name_gantry = 'Gantry_Home'
_state_machine.userdata.move_group_g = 'Gantry'
_state_machine.userdata.move_group_prefix_g = '/ariac/gantry'
_state_machine.userdata.action_topic = '/move_group'
_state_machine.userdata.robot_name = 'gantry'
_state_machine.userdata.joint_values = []
_state_machine.userdata.joint_names = []
_state_machine.userdata.move_group_l = 'Left_Arm'
_state_machine.userdata.move_group_r = 'Right_Arm'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('MoveLeftArm',
SrdfStateToMoveitAriac(),
transitions={'reached': 'MoveRightArm', '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_left', 'move_group': 'move_group_l', 'move_group_prefix': 'move_group_prefix_g', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:208 y:38
OperatableStateMachine.add('MoveRightArm',
SrdfStateToMoveitAriac(),
transitions={'reached': 'MoveGantryHome', '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_right', 'move_group': 'move_group_r', 'move_group_prefix': 'move_group_prefix_g', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
# x:385 y:37
OperatableStateMachine.add('MoveGantryHome',
SrdfStateToMoveitAriac(),
transitions={'reached': 'finished', '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_gantry', 'move_group': 'move_group_g', 'move_group_prefix': 'move_group_prefix_g', 'action_topic': 'action_topic', 'robot_name': 'robot_name', 'config_name_out': 'config_name_out', 'move_group_out': 'move_group_out', 'robot_name_out': 'robot_name_out', 'action_topic_out': 'action_topic_out', 'joint_values': 'joint_values', 'joint_names': 'joint_names'})
return _state_machine
def test_modify_input_key(self):
class ModifyInputKey(EventState):
def __init__(self):
super(ModifyInputKey, self).__init__(outcomes=['done'], input_keys=['only_input'])
def execute(self, userdata):
userdata.only_input['new'] = 'not_allowed'
return 'done'
sm = OperatableStateMachine(outcomes=['done'])
sm.userdata.only_input = {'existing': 'is_allowed'}
with sm:
OperatableStateMachine.add('state', ModifyInputKey(), transitions={'done': 'done'})
outcome = sm.execute(None)
self.assertIsNone(outcome)
self.assertIsInstance(sm._last_exception, UserDataError)
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]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('HSR_Wait',
WaitState(wait_time=3),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:832 y:94, x:692 y:405
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
_state_machine.userdata.joint_values_left = [4.824961332294557, 4.986228519472087, 2.016893253088309, 9.056585287421564, 1.7655888505129436, 2.5387597755555658]
_state_machine.userdata.joint_values_right = [4.761117371482087, 4.435244724700612, 1.6085220010066865, 3.2511244744831167, 1.706054283818386, 2.4325377312649925]
_state_machine.userdata.joint_names = ["m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3", "m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6"]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:106 y:117
OperatableStateMachine.add('sub',
SubscriberState(topic='/robotender', blocking=True, clear=False),
transitions={'received': 'decide', 'unavailable': 'failed'},
autonomy={'received': Autonomy.Off, 'unavailable': Autonomy.Off},
remapping={'message': 'message'})
# x:280 y:116
OperatableStateMachine.add('decide',
DecisionState(outcomes=['go_left','go_right'], conditions=lambda x: "go_right" if x.data=="R" else "go_left"),
transitions={'go_left': 'left', 'go_right': 'right'},
autonomy={'go_left': Autonomy.Off, 'go_right': Autonomy.Off},
remapping={'input_value': 'message'})
# x:460 y:22
OperatableStateMachine.add('left',
FeedbackJointStateToMoveit(move_group="arm", action_topic="/move_group", robot_name="m1n6s200", position_topic='/m1n6s200_driver/joint_states', delta=1E-4),
transitions={'reached': 'sub', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'joint_values': 'joint_values_left', 'joint_names': 'joint_names'})
# x:452 y:234
OperatableStateMachine.add('right',
FeedbackJointStateToMoveit(move_group="arm", action_topic="/move_group", robot_name="m1n6s200", position_topic='/m1n6s200_driver/joint_states', delta=1E-4),
transitions={'reached': 'sub', 'failed': 'failed'},
autonomy={'reached': Autonomy.Off, 'failed': Autonomy.Off},
remapping={'joint_values': 'joint_values_right', 'joint_names': 'joint_names'})
return _state_machine
def create(self):
# x:324 y:460, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:178 y:120
OperatableStateMachine.add('Forward',
TimedMoveState(linear=1,
angular=0,
duration=2),
transitions={'ok': 'Stop'},
autonomy={'ok': Autonomy.Off})
# x:335 y:207
OperatableStateMachine.add('FrontLeft',
TimedMoveState(linear=1,
angular=5,
duration=1),
transitions={'ok': 'Stop2'},
autonomy={'ok': Autonomy.Off})
# x:342 y:119
OperatableStateMachine.add('Stop',
TimedMoveState(linear=0,
angular=0,
duration=1),
transitions={'ok': 'FrontLeft'},
autonomy={'ok': Autonomy.Off})
# x:331 y:288
OperatableStateMachine.add('Stop2',
TimedMoveState(linear=0,
angular=0,
duration=1),
transitions={'ok': 'Forward'},
autonomy={'ok': Autonomy.Off})
return _state_machine
def create(self):
# x:483 y:240, x:483 y:90
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.Direction = 'Back'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:201 y:59
OperatableStateMachine.add('s1',
SubscriberState(topic='/pose',
blocking=True,
clear=False),
transitions={
'received': 'm1',
'unavailable': 'failed'
},
autonomy={
'received': Autonomy.Off,
'unavailable': Autonomy.Off
},
remapping={'message': 'curr_pose'})
# x:181 y:209
OperatableStateMachine.add('m1',
MoveBaseState(),
transitions={
'arrived': 'finished',
'failed': 'failed'
},
autonomy={
'arrived': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'Direction': 'Direction',
'curr_pose': 'curr_pose'
})
return _state_machine
def create(self):
# x:628 y:51, x:130 y:406
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.arm_id_r = 'Right_Arm'
_state_machine.userdata.arm_id_l = 'Left_Arm'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:91 y:39
OperatableStateMachine.add('Enable_gripper_L',
GripperControl(enable=False),
transitions={
'continue': 'Enable_gripper_R',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_l'})
# x:360 y:49
OperatableStateMachine.add('Enable_gripper_R',
GripperControl(enable=False),
transitions={
'continue': 'finished',
'failed': 'failed',
'invalid_id': 'failed'
},
autonomy={
'continue': Autonomy.Off,
'failed': Autonomy.Off,
'invalid_id': Autonomy.Off
},
remapping={'arm_id': 'arm_id_r'})
return _state_machine
def create(self):
# x:255 y:192, x:130 y:324
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:72 y:118
OperatableStateMachine.add('log',
LogState(text="test",
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:30 y:365
_state_machine = OperatableStateMachine(outcomes=['finished'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:66 y:154
OperatableStateMachine.add('Print_Msg',
LogState(text="My param is: " + self.msg, severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:332 y:44, x:189 y:128
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'], input_keys=['arm'])
_state_machine.userdata.arm = ''
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:137 y:39
OperatableStateMachine.add('Dummy',
DummyState(),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:185 y:376, x:483 y:51
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:113 y:43
OperatableStateMachine.add('Change_to_MANIPULATE',
ChangeControlModeActionState(target_mode=ChangeControlModeActionState.MANIPULATE),
transitions={'changed': 'finished', 'failed': 'failed'},
autonomy={'changed': Autonomy.High, 'failed': Autonomy.Low})
return _state_machine
def create(self):
# x:30 y:365, x:130 y:365
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'],
input_keys=['data'],
output_keys=['result'])
_state_machine.userdata.data = None
_state_machine.userdata.result = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:40 y:73
OperatableStateMachine.add(
'Modify Data',
CalculationState(calculation=lambda x: x * 2),
transitions={'done': 'Decide Param'},
autonomy={'done': Autonomy.Off},
remapping={
'input_value': 'data',
'output_value': 'result'
})
# x:37 y:201
OperatableStateMachine.add(
'Decide Param',
DecisionState(outcomes=['finished', 'failed'],
conditions=lambda x: 'finished'
if self.value == 'correct' else 'failed'),
transitions={
'finished': 'finished',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={'input_value': 'data'})
return _state_machine
def create(self):
# x:636 y:50, x:263 y:129
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.arm = 'arm1'
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:49 y:39
OperatableStateMachine.add(
'StartAssignment',
StartAssignment(),
transitions={
'continue': 'transport_part_from_belt_to_bin_state'
},
autonomy={'continue': Autonomy.Off})
# x:472 y:44
OperatableStateMachine.add('EndAssignment',
EndAssignment(),
transitions={'continue': 'finished'},
autonomy={'continue': Autonomy.Off})
# x:196 y:40
OperatableStateMachine.add(
'transport_part_from_belt_to_bin_state',
self.use_behavior(transport_part_from_belt_to_bin_stateSM,
'transport_part_from_belt_to_bin_state'),
transitions={
'finished': 'EndAssignment',
'failed': 'failed'
},
autonomy={
'finished': Autonomy.Inherit,
'failed': Autonomy.Inherit
},
remapping={'arm': 'arm'})
return _state_machine
def create(self):
# x:942 y:70, x:835 y:318
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.JV = [
5.034934007384111, 4.749860722186541, 2.0863536874817012,
3.548160146665105, -4.830528404237258, -3.8607153900995290
]
_state_machine.userdata.JN = [
"m1n6s200_joint_1", "m1n6s200_joint_2", "m1n6s200_joint_3",
"m1n6s200_joint_4", "m1n6s200_joint_5", "m1n6s200_joint_6"
]
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:142 y:64
OperatableStateMachine.add(
'Get_Values',
GetJointValuesState(
joints=[
"m1n6s200_joint_1", "m1n6s200_joint_2",
"m1n6s200_joint_3", "m1n6s200_joint_4",
"m1n6s200_joint_5", "m1n6s200_joint_6"
],
topic_name="/m1n6s200_driver/joint_states"),
transitions={'retrieved': 'Log_Values'},
autonomy={'retrieved': Autonomy.Off},
remapping={'joint_values': 'joint_values'})
# x:394 y:102
OperatableStateMachine.add('Log_Values',
LogKeyState(
text='%d',
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_values'})
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]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('Change Pitch',
BigPitchParameterState(topic=self.topic, angle=angle),
transitions={'Success': 'finished', 'Failure': 'failed'},
autonomy={'Success': Autonomy.Off, 'Failure': Autonomy.Off})
return _state_machine
def create(self):
# x:909 y:103, x:830 y:367
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:105 y:53
OperatableStateMachine.add('Entry',
LogState(text="Entering the create twist demo", severity=Logger.REPORT_HINT),
transitions={'done': 'Twist1'},
autonomy={'done': Autonomy.Off})
# x:577 y:345
OperatableStateMachine.add('EmergencyStop',
CreateTimedStopState(timeout=0.2, cmd_topic='/create_node/cmd_vel', odom_topic='/create_node/odom'),
transitions={'done': 'failed', 'failed': 'failed'},
autonomy={'done': Autonomy.Off, 'failed': Autonomy.Off})
# x:577 y:145
OperatableStateMachine.add('Stop',
CreateTimedStopState(timeout=0.24, cmd_topic='/create_node/cmd_vel', odom_topic='/create_node/odom'),
transitions={'done': 'Exit', 'failed': 'EmergencyStop'},
autonomy={'done': Autonomy.High, 'failed': Autonomy.Off})
# x:756 y:65
OperatableStateMachine.add('Exit',
LogState(text="Exiting the create twist demo", severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.High})
# x:73 y:166
OperatableStateMachine.add('Twist1',
CreateTimedTwistState(target_time=10.0, velocity=0.25, rotation_rate=0.05, cmd_topic='/create_node/cmd_vel', sensor_topic='/create_node/sensor_state'),
transitions={'done': 'Stop', 'failed': 'EmergencyStop'},
autonomy={'done': Autonomy.Low, 'failed': Autonomy.Off})
return _state_machine
def create(self):
# x:355 y:291, x:130 y:365
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:134 y:102
OperatableStateMachine.add('Drive Forward State',
GoForwardState(speed=self.my_speed, travel_dist=self.my_travel_dist, obstacle_dist=self.my_obstacle_dist),
transitions={'failed': 'failed', 'done': 'finished'},
autonomy={'failed': Autonomy.Off, 'done': Autonomy.Off})
return _state_machine
def create(self):
# x:30 y:365, x:347 y:355
_state_machine = OperatableStateMachine(outcomes=['finished', 'failed'])
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:108 y:151
OperatableStateMachine.add('test',
TestRendezvousState(goal_distance=self.goal_distance),
transitions={'continue': 'finished', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': 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]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add('abc',
ExampleState(target_time=1),
transitions={'continue': 'finished', 'failed': 'failed'},
autonomy={'continue': Autonomy.Off, 'failed': Autonomy.Off})
return _state_machine
def create(self):
# x:296 y:78, x:278 y:18
_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:130 y:365, x:230 y:365, x:330 y:365, x:430 y:365, x:530 y:365
_sm_dual_arm_container_0 = ConcurrencyContainer(outcomes=['finished', 'failed'], conditions=[
('finished', [('Single Arm Sample Left', 'finished')]),
('failed', [('Single Arm Sample Left', 'failed')]),
('finished', [('Single Arm Sample Right', 'finished')]),
('failed', [('Single Arm Sample Right', 'failed')])
])
with _sm_dual_arm_container_0:
# x:95 y:90
OperatableStateMachine.add('Single Arm Sample Left',
self.use_behavior(SingleArmSampleSM, 'Dual Arm Container/Single Arm Sample Left',
parameters={'robot_name': "left_arm", 'en_sim': True, 'pose_1': "0.2, 0.25, -0.5"}),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
# x:343 y:89
OperatableStateMachine.add('Single Arm Sample Right',
self.use_behavior(SingleArmSampleSM, 'Dual Arm Container/Single Arm Sample Right',
parameters={'robot_name': "right_arm", 'en_sim': True, 'pose_1': "0.2, -0.25, -0.5"}),
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
with _state_machine:
# x:93 y:27
OperatableStateMachine.add('Dual Arm Container',
_sm_dual_arm_container_0,
transitions={'finished': 'finished', 'failed': 'failed'},
autonomy={'finished': Autonomy.Inherit, 'failed': Autonomy.Inherit})
return _state_machine
def create(self):
# x:30 y:365
_state_machine = OperatableStateMachine(outcomes=['ok'],
input_keys=['target_pose'])
_state_machine.userdata.target_pose = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:81 y:35
OperatableStateMachine.add('SplitTargetPose',
SplitPoseState(),
transitions={'ok': 'ProportionalDrive'},
autonomy={'ok': Autonomy.Off},
remapping={
'target_pose':
'target_pose',
'target_position':
'target_position',
'target_orientation':
'target_orientation'
})
# x:88 y:249
OperatableStateMachine.add(
'CorrectTurn',
TurnCorrectionState(),
transitions={'ok': 'ok'},
autonomy={'ok': Autonomy.Off},
remapping={'target_orientation': 'target_orientation'})
# x:79 y:145
OperatableStateMachine.add(
'ProportionalDrive',
ProportionalDriveState(),
transitions={'ok': 'CorrectTurn'},
autonomy={'ok': Autonomy.Off},
remapping={'target_position': 'target_position'})
return _state_machine
