def create(self):
# x:95 y:629, x:806 y:420
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.action_topic = "/move_group"
_state_machine.userdata.trajectory_action_topic = "ur5e/arm_controller/follow_joint_trajectory"
_state_machine.userdata.move_group = "manipulator"
_state_machine.userdata.config_up = "up"
_state_machine.userdata.config_default = "default"
_state_machine.userdata.components = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:74 y:38
OperatableStateMachine.add(
'InitializeMoveit',
SetupProxyMoveItClientState(
robot_description="/robot_description",
robot_description_semantic=None,
move_group_capabilities="/move_group",
action_type_and_topics=[[
"MoveGroupAction", ["/move_group"]
]],
enter_wait_duration=0.0),
transitions={
'connected': 'LoggerConnected',
'topics_unavailable': 'failed',
'param_error': 'failed'
},
autonomy={
'connected': Autonomy.Off,
'topics_unavailable': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'robot_name': 'robot_name',
'move_groups': 'move_groups'
})
# x:255 y:130
OperatableStateMachine.add(
'LoggerConnected',
LogState(text="filler", severity=Logger.REPORT_HINT),
transitions={'done': 'GetDefaultPosition'},
autonomy={'done': Autonomy.Off})
# x:73 y:222
OperatableStateMachine.add('GetDefaultPosition',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'PlanMovement',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'robot_name': 'robot_name',
'selected_move_group': 'move_group',
'config_name': 'config_default',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:76 y:369
OperatableStateMachine.add(
'PlanMovement',
JointValuesToMoveItPlanState(timeout=5.0,
enter_wait_duration=0.0,
action_topic=None),
transitions={
'planned': 'ExecuteMovement',
'failed': 'failed',
'topics_unavailable': 'failed',
'param_error': 'failed'
},
autonomy={
'planned': Autonomy.Off,
'failed': Autonomy.Off,
'topics_unavailable': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'action_topic': 'action_topic',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values',
'joint_trajectory': 'joint_trajectory'
})
# x:76 y:472
OperatableStateMachine.add('ExecuteMovement',
ExecuteKnownTrajectoryState(
timeout=3.0,
max_delay=5.0,
wait_duration=0.25,
action_topic="/execute_trajectory"),
transitions={
'done': 'GetUpPosition',
'failed': 'failed',
'param_error': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'action_topic': 'action_topic',
'trajectory': 'joint_trajectory',
'status_text': 'status_text',
'goal_names': 'goal_names',
'goal_values': 'goal_values'
})
# x:494 y:131
OperatableStateMachine.add('GetUpPosition',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'PlanMovement2',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'robot_name': 'robot_name',
'selected_move_group': 'move_group',
'config_name': 'config_up',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:906 y:225
OperatableStateMachine.add(
'PlanMovement2',
JointValuesToMoveItPlanState(timeout=5.0,
enter_wait_duration=0.5,
action_topic=None),
transitions={
'planned': 'ExecuteMovement2',
'failed': 'failed',
'topics_unavailable': 'failed',
'param_error': 'failed'
},
autonomy={
'planned': Autonomy.Off,
'failed': Autonomy.Off,
'topics_unavailable': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'action_topic': 'action_topic',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values',
'joint_trajectory': 'joint_trajectory'
})
# x:906 y:449
OperatableStateMachine.add('ExecuteMovement2',
ExecuteKnownTrajectoryState(
timeout=3.0,
max_delay=5.0,
wait_duration=0.25,
action_topic="/execute_trajectory"),
transitions={
'done': 'finished',
'failed': 'failed',
'param_error': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'action_topic': 'action_topic',
'trajectory': 'joint_trajectory',
'status_text': 'status_text',
'goal_names': 'goal_names',
'goal_values': 'goal_values'
})
return _state_machine
def create(self):
# x:1423 y:790, x:784 y:398
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.action_topic = "/move_group"
_state_machine.userdata.trajectory_action_topic = "/m1n6s200_driver/arm_controller/follow_joint_trajectory"
_state_machine.userdata.move_group = "arm"
_state_machine.userdata.config_vertical = "Vertical"
_state_machine.userdata.config_home = "Home"
_state_machine.userdata.config_retract = "Retract"
_state_machine.userdata.components = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add(
'MoveIt',
SetupProxyMoveItClientState(
robot_description="/robot_description",
robot_description_semantic=None,
move_group_capabilities="/move_group",
action_type_and_topics=[[
"MoveGroupAction", ["/move_group"]
]],
enter_wait_duration=0.0),
transitions={
'connected': 'Connected',
'topics_unavailable': 'failed',
'param_error': 'failed'
},
autonomy={
'connected': Autonomy.High,
'topics_unavailable': Autonomy.Full,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'move_groups': 'move_groups'
})
# x:146 y:145
OperatableStateMachine.add('Connected',
LogState(text="Connected",
severity=Logger.REPORT_HINT),
transitions={'done': 'Vertical'},
autonomy={'done': Autonomy.Off})
# x:481 y:644
OperatableStateMachine.add('Vertical',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'GoalTolerances',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Low,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'selected_move_group': 'move_group',
'config_name': 'config_vertical',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:909 y:136
OperatableStateMachine.add(
'Plan',
JointValuesToMoveItPlanState(timeout=5.0,
enter_wait_duration=0.5,
action_topic=None),
transitions={
'planned': 'ExecTraj',
'failed': 'failed',
'topics_unavailable': 'failed',
'param_error': 'failed'
},
autonomy={
'planned': Autonomy.High,
'failed': Autonomy.Full,
'topics_unavailable': Autonomy.Full,
'param_error': Autonomy.Full
},
remapping={
'action_topic': 'action_topic',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values',
'joint_trajectory': 'joint_trajectory'
})
# x:461 y:479
OperatableStateMachine.add('GoalTolerances',
SetJointTrajectoryTolerancesState(
position_constraints=[0.05],
velocity_constraints=[0.0],
acceleration_constraints=[0.0]),
transitions={
'configured': 'PathTolerances',
'param_error': 'failed'
},
autonomy={
'configured': Autonomy.Off,
'param_error': Autonomy.Full
},
remapping={
'joint_names':
'joint_names',
'joint_tolerances':
'joint_goal_tolerances'
})
# x:544 y:189
OperatableStateMachine.add('PathTolerances',
SetJointTrajectoryTolerancesState(
position_constraints=[0.08],
velocity_constraints=[0.0],
acceleration_constraints=[0.0]),
transitions={
'configured': 'DumpGoalTolerance',
'param_error': 'failed'
},
autonomy={
'configured': Autonomy.Off,
'param_error': Autonomy.Full
},
remapping={
'joint_names':
'joint_names',
'joint_tolerances':
'joint_path_tolerances'
})
# x:389 y:34
OperatableStateMachine.add(
'DumpGoalTolerance',
LogKeyState(text="Goal tolerance {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'DumpPathTolerance'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_goal_tolerances'})
# x:653 y:50
OperatableStateMachine.add(
'DumpPathTolerance',
LogKeyState(text="Path tolerance {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'Plan'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_path_tolerances'})
# x:760 y:584
OperatableStateMachine.add('Home',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'Plan',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'selected_move_group': 'move_group',
'config_name': 'config_home',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:551 y:799
OperatableStateMachine.add(
'Decision',
OperatorDecisionState(outcomes=["home", "vertical", "stop"],
hint=None,
suggestion=None),
transitions={
'home': 'Home',
'vertical': 'Vertical',
'stop': 'finished'
},
autonomy={
'home': Autonomy.Off,
'vertical': Autonomy.Off,
'stop': Autonomy.Full
})
# x:1085 y:601
OperatableStateMachine.add('ExecTraj',
ExecuteKnownTrajectoryState(
timeout=3.0,
max_delay=5.0,
wait_duration=0.25,
action_topic="/execute_trajectory"),
transitions={
'done': 'Decision',
'failed': 'failed',
'param_error': 'ErrLog'
},
autonomy={
'done': Autonomy.High,
'failed': Autonomy.High,
'param_error': Autonomy.Off
},
remapping={
'action_topic': 'action_topic',
'trajectory': 'joint_trajectory',
'status_text': 'status_text',
'goal_names': 'goal_names',
'goal_values': 'goal_values'
})
# x:999 y:715
OperatableStateMachine.add('ErrLog',
LogState(
text="Param error on exec traj",
severity=Logger.REPORT_HINT),
transitions={'done': 'StatusLog'},
autonomy={'done': Autonomy.Off})
# x:897 y:761
OperatableStateMachine.add('StatusLog',
LogKeyState(
text=" Status {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'Decision'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'status_text'})
return _state_machine
def create(self):
# x:1500 y:655, x:784 y:398
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.action_topic = "/move_group"
_state_machine.userdata.trajectory_action_topic = "/m1n6s200_driver/arm_controller/follow_joint_trajectory"
_state_machine.userdata.move_group_arm = "arm"
_state_machine.userdata.config_vertical = "Vertical"
_state_machine.userdata.config_home = "Home"
_state_machine.userdata.config_retract = "Retract"
_state_machine.userdata.components = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add(
'MoveIt',
SetupProxyMoveItClientState(
robot_description="/robot_description",
robot_description_semantic=None,
move_group_capabilities="/move_group",
action_type_and_topics=None,
enter_wait_duration=0.0),
transitions={
'connected': 'Connected',
'topics_unavailable': 'failed',
'param_error': 'failed'
},
autonomy={
'connected': Autonomy.High,
'topics_unavailable': Autonomy.Full,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'move_groups': 'move_groups'
})
# x:146 y:145
OperatableStateMachine.add('Connected',
LogState(text="Connected",
severity=Logger.REPORT_HINT),
transitions={'done': 'RobotName'},
autonomy={'done': Autonomy.Off})
# x:645 y:678
OperatableStateMachine.add('Vertical',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'GoalTolerances',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Low,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'selected_move_group':
'move_group_arm',
'config_name': 'config_vertical',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:823 y:29
OperatableStateMachine.add(
'Plan',
JointValuesToMoveItPlanState(timeout=5.0,
enter_wait_duration=0.5,
action_topic=None),
transitions={
'planned': 'Execute',
'failed': 'failed',
'topics_unavailable': 'failed',
'param_error': 'failed'
},
autonomy={
'planned': Autonomy.High,
'failed': Autonomy.Full,
'topics_unavailable': Autonomy.Full,
'param_error': Autonomy.Full
},
remapping={
'action_topic': 'action_topic',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values',
'joint_trajectory': 'joint_trajectory'
})
# x:1215 y:121
OperatableStateMachine.add(
'Execute',
TrajectoryToFollowJointTrajectoryActionState(
goal_time_tolerance=3.0,
max_delay=-1.0,
wait_duration=2.0,
timeout=5.0,
action_topic=None),
transitions={
'reached': 'Decision',
'goal_failed': 'GoalFailedLog',
'path_failed': 'PathTolLog',
'invalid_request': 'failed',
'param_error': 'failed',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Full,
'goal_failed': Autonomy.Off,
'path_failed': Autonomy.Off,
'invalid_request': Autonomy.Full,
'param_error': Autonomy.Full,
'failed': Autonomy.Full
},
remapping={
'trajectory_action_topic': 'trajectory_action_topic',
'joint_trajectory': 'joint_trajectory',
'joint_goal_tolerances': 'joint_goal_tolerances',
'joint_path_tolerances': 'joint_path_tolerances',
'status_text': 'status_text',
'goal_names': 'goal_names',
'goal_values': 'goal_values'
})
# x:616 y:559
OperatableStateMachine.add('GoalTolerances',
SetJointTrajectoryTolerancesState(
position_constraints=[0.05],
velocity_constraints=[0.0],
acceleration_constraints=[0.0]),
transitions={
'configured': 'LogValues',
'param_error': 'failed'
},
autonomy={
'configured': Autonomy.Off,
'param_error': Autonomy.Full
},
remapping={
'joint_names':
'joint_names',
'joint_tolerances':
'joint_goal_tolerances'
})
# x:544 y:189
OperatableStateMachine.add('PathTolerances',
SetJointTrajectoryTolerancesState(
position_constraints=[0.08],
velocity_constraints=[0.0],
acceleration_constraints=[0.0]),
transitions={
'configured': 'DumpGoalTolerance',
'param_error': 'failed'
},
autonomy={
'configured': Autonomy.Off,
'param_error': Autonomy.Full
},
remapping={
'joint_names':
'joint_names',
'joint_tolerances':
'joint_path_tolerances'
})
# x:389 y:34
OperatableStateMachine.add(
'DumpGoalTolerance',
LogKeyState(text="Goal tolerance {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'DumpPathTolerance'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_goal_tolerances'})
# x:560 y:50
OperatableStateMachine.add(
'DumpPathTolerance',
LogKeyState(text="Path tolerance {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'Plan'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_path_tolerances'})
# x:889 y:579
OperatableStateMachine.add('Home',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'Plan',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'selected_move_group':
'move_group_arm',
'config_name': 'config_home',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:1265 y:670
OperatableStateMachine.add('Decision',
OperatorDecisionState(outcomes=[
"home", "vertical", "PS", "stop"
],
hint=None,
suggestion=None),
transitions={
'home': 'Home',
'vertical': 'Vertical',
'PS': 'ApplyPS',
'stop': 'finished'
},
autonomy={
'home': Autonomy.Off,
'vertical': Autonomy.Off,
'PS': Autonomy.Low,
'stop': Autonomy.Full
})
# x:48 y:261
OperatableStateMachine.add(
'ClearOM',
ClearOctomapState(timeout=5.0,
wait_duration=5,
action_topic="/clear_octomap"),
transitions={
'done': 'QueryPlanners',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.High
},
remapping={'action_topic': 'action_topic'})
# x:47 y:402
OperatableStateMachine.add(
'QueryPlanners',
QueryPlannersState(timeout=5.0,
wait_duration=0.001,
action_topic="/query_planner_interface"),
transitions={
'done': 'LogPlanners',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Off,
'failed': Autonomy.Off
},
remapping={
'action_topic': 'action_topic',
'planner_interfaces': 'planner_interfaces'
})
# x:108 y:527
OperatableStateMachine.add(
'LogPlanners',
LogKeyState(text="Available Planners {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'GetPlanningScene'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'planner_interfaces'})
# x:87 y:641
OperatableStateMachine.add('GetPlanningScene',
GetPlanningSceneState(
components=1023,
timeout=5.0,
wait_duration=5,
action_topic="/get_planning_scene"),
transitions={
'done': 'LogPS',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Low
},
remapping={
'action_topic': 'action_topic',
'scene': 'scene'
})
# x:1411 y:534
OperatableStateMachine.add(
'ApplyPS',
ApplyPlanningSceneState(timeout=5.0,
wait_duration=5,
action_topic="/apply_planning_scene"),
transitions={
'done': 'Decision',
'failed': 'failed'
},
autonomy={
'done': Autonomy.Low,
'failed': Autonomy.Low
},
remapping={'action_topic': 'action_topic'})
# x:363 y:684
OperatableStateMachine.add('LogPS',
LogKeyState(
text="Planning Scence {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'Vertical'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'scene'})
# x:1157 y:336
OperatableStateMachine.add('PathTolLog',
LogState(text="Path tolerance failure",
severity=Logger.REPORT_HINT),
transitions={'done': 'Decision'},
autonomy={'done': Autonomy.High})
# x:1254 y:288
OperatableStateMachine.add(
'GoalFailedLog',
LogState(text="Failed to reach goal tolerance",
severity=Logger.REPORT_HINT),
transitions={'done': 'Decision'},
autonomy={'done': Autonomy.High})
# x:279 y:190
OperatableStateMachine.add('RobotName',
LogKeyState(
text="Robot Name ({})",
severity=Logger.REPORT_HINT),
transitions={'done': 'ClearOM'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'robot_name'})
# x:632 y:279
OperatableStateMachine.add('LogValues',
LogKeyState(
text="Joint Values {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'PathTolerances'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_values'})
return _state_machine
def create(self):
# x:1490 y:191, x:764 y:433
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.action_topic = "/move_group"
_state_machine.userdata.trajectory_action_topic = "/m1n6s200_driver/arm_controller/follow_joint_trajectory"
_state_machine.userdata.config_vertical = "Vertical"
_state_machine.userdata.config_home = "Home"
_state_machine.userdata.config_retract = "Retract"
_state_machine.userdata.components = None
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add(
'MoveIt',
SetupProxyMoveItClientState(
robot_description="/robot_description",
robot_description_semantic=None,
move_group_capabilities=[[
"/move_group", ["arm", "gripper"]
]],
action_type_and_topics=None,
enter_wait_duration=0.0),
transitions={
'connected': 'Connected',
'topics_unavailable': 'failed',
'param_error': 'failed'
},
autonomy={
'connected': Autonomy.High,
'topics_unavailable': Autonomy.Full,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'move_groups': 'move_groups'
})
# x:52 y:151
OperatableStateMachine.add('Connected',
LogState(text="Connected",
severity=Logger.REPORT_HINT),
transitions={'done': 'SetMG'},
autonomy={'done': Autonomy.Off})
# x:690 y:610
OperatableStateMachine.add('DumpJVUser',
LogKeyState(
text="Joint Values {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'GetJVList'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'user_joint_values'})
# x:243 y:287
OperatableStateMachine.add('GetNames',
GetJointNamesFromMoveGroupState(),
transitions={
'retrieved': 'UserNames',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'robot_name': 'robot_name',
'selected_move_group':
'selected_move_group',
'move_group': 'move_group',
'joint_names': 'user_joint_names'
})
# x:51 y:257
OperatableStateMachine.add(
'SetMG',
UserDataState(data="arm"),
transitions={'done': 'GetNames'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'selected_move_group'})
# x:438 y:420
OperatableStateMachine.add(
'GetJVUser',
GetCurrentJointValuesUserState(
timeout=5.0,
joint_states_topic='/m1n6s200_driver/joint_states'),
transitions={
'retrieved': 'DumpJVUser',
'timeout': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'timeout': Autonomy.High
},
remapping={
'joint_names': 'no_ee_names',
'joint_values': 'user_joint_values'
})
# x:853 y:612
OperatableStateMachine.add(
'GetJVList',
GetCurrentJointValuesListState(
joint_names=[
'm1n6s200_joint_1', 'm1n6s200_joint_2',
'm1n6s200_joint_3', 'm1n6s200_joint_4'
],
timeout=None,
joint_states_topic='/m1n6s200_driver/joint_states'),
transitions={
'retrieved': 'ListNames',
'timeout': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'timeout': Autonomy.High
},
remapping={
'joint_names': 'list_joint_names',
'joint_values': 'list_joint_values'
})
# x:572 y:156
OperatableStateMachine.add('ListNames',
LogKeyState(
text='List Names {}',
severity=Logger.REPORT_HINT),
transitions={'done': 'ListValues'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'list_joint_names'})
# x:647 y:42
OperatableStateMachine.add('ListValues',
LogKeyState(
text='List Values {}',
severity=Logger.REPORT_HINT),
transitions={'done': 'SetConfigName'},
autonomy={'done': Autonomy.Full},
remapping={'data': 'list_joint_values'})
# x:188 y:388
OperatableStateMachine.add('UserNames',
LogKeyState(
text='User names {}',
severity=Logger.REPORT_HINT),
transitions={'done': 'JustArmJoints'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'user_joint_names'})
# x:222 y:484
OperatableStateMachine.add(
'JustArmJoints',
UserDataState(data=[
'm1n6s200_joint_1', 'm1n6s200_joint_2', 'm1n6s200_joint_3',
'm1n6s200_joint_4', 'm1n6s200_joint_5', 'm1n6s200_joint_6'
]),
transitions={'done': 'GetJVUser'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'no_ee_names'})
# x:1000 y:34
OperatableStateMachine.add('GetRetract',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'JVTrajectory',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'robot_name': 'robot_name',
'selected_move_group': 'move_group',
'config_name': 'config_name',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:802 y:44
OperatableStateMachine.add('SetConfigName',
UserDataState(data="Retract"),
transitions={'done': 'GetRetract'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'config_name'})
# x:1035 y:139
OperatableStateMachine.add(
'JVTrajectory',
JointValuesToTrajectoryState(duration=10.0),
transitions={
'done': 'SetGoal',
'param_error': 'failed'
},
autonomy={
'done': Autonomy.Off,
'param_error': Autonomy.Full
},
remapping={
'joint_names': 'joint_names',
'joint_values': 'joint_values',
'joint_trajectory': 'joint_trajectory'
})
# x:1131 y:464
OperatableStateMachine.add(
'FJTA',
TrajectoryToFollowJointTrajectoryActionState(
goal_time_tolerance=3.0,
max_delay=-1.0,
wait_duration=2.0,
timeout=1.0,
action_topic=None),
transitions={
'reached': 'JV2MoveAction',
'goal_failed': 'GoalTolFailure',
'path_failed': 'PathTolFailed',
'invalid_request': 'failed',
'param_error': 'failed',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Full,
'goal_failed': Autonomy.Off,
'path_failed': Autonomy.Off,
'invalid_request': Autonomy.Full,
'param_error': Autonomy.Full,
'failed': Autonomy.Full
},
remapping={
'trajectory_action_topic': 'trajectory_action_topic',
'joint_trajectory': 'joint_trajectory',
'joint_goal_tolerances': 'joint_goal_tolerances',
'joint_path_tolerances': 'joint_path_tolerances',
'status_text': 'status_text',
'goal_names': 'goal_names',
'goal_values': 'goal_values'
})
# x:1051 y:230
OperatableStateMachine.add('SetGoal',
SetJointTrajectoryTolerancesState(
position_constraints=[0.02],
velocity_constraints=[0.0],
acceleration_constraints=[0.0]),
transitions={
'configured': 'SetPath',
'param_error': 'failed'
},
autonomy={
'configured': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'joint_names':
'joint_names',
'joint_tolerances':
'joint_goal_tolerances'
})
# x:1062 y:349
OperatableStateMachine.add('SetPath',
SetJointTrajectoryTolerancesState(
position_constraints=[0.0],
velocity_constraints=[0.0],
acceleration_constraints=[0.0]),
transitions={
'configured': 'FJTA',
'param_error': 'failed'
},
autonomy={
'configured': Autonomy.Off,
'param_error': Autonomy.Off
},
remapping={
'joint_names':
'joint_names',
'joint_tolerances':
'joint_path_tolerances'
})
# x:1386 y:539
OperatableStateMachine.add(
'JV2MoveAction',
JointValuesToMoveActionState(
joint_names=['m1n6s200_joint_1', 'm1n6s200_joint_2'],
joint_values=[0.0, 1.57],
move_group='arm',
action_topic='/move_group',
joint_tolerance=0.02,
constraint_weight=1.0,
allowed_planning_time=4.0,
wait_duration=0.2,
timeout=30.0),
transitions={
'reached': 'SetConfigName_2',
'param_error': 'failed',
'planning_failed': 'failed',
'control_failed': 'CtrlFailed',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Full,
'param_error': Autonomy.Full,
'planning_failed': Autonomy.Full,
'control_failed': Autonomy.Full,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'action_topic': 'action_topic',
'status_text': 'status_text',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:1324 y:272
OperatableStateMachine.add('CtrlFailed',
LogState(text="Control failed",
severity=Logger.REPORT_HINT),
transitions={'done': 'finished'},
autonomy={'done': Autonomy.Full})
# x:1648 y:554
OperatableStateMachine.add('GetHome',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'SayJointNames',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'selected_move_group': 'move_group',
'config_name': 'config_name',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:1489 y:749
OperatableStateMachine.add('SetConfigName_2',
UserDataState(data="Home"),
transitions={'done': 'ConfigNamePrint'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'config_name'})
# x:1644 y:267
OperatableStateMachine.add('MoveGroup',
MoveGroupState(timeout=30.0,
wait_duration=0.02,
move_group=None),
transitions={
'reached': 'finished',
'control_failed': 'CtrlFailed',
'planning_failed': 'failed',
'param_error': 'failed',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Full,
'control_failed': Autonomy.Full,
'planning_failed': Autonomy.High,
'param_error': Autonomy.Full,
'failed': Autonomy.Full
},
remapping={
'move_group':
'move_group',
'joint_names':
'joint_names',
'joint_values':
'joint_values',
'status_text':
'status_text',
'planned_trajectory':
'planned_trajectory',
'executed_trajectory':
'executed_trajectory'
})
# x:1689 y:666
OperatableStateMachine.add('ConfigNamePrint',
LogKeyState(
text="Config {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'GetHome'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'config_name'})
# x:1701 y:450
OperatableStateMachine.add('SayJointNames',
LogKeyState(
text="Joint names {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'SayJointValues'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_names'})
# x:1550 y:367
OperatableStateMachine.add('SayJointValues',
LogKeyState(
text="Values {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'MoveGroup'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_values'})
# x:1113 y:682
OperatableStateMachine.add('PathTolFailed',
LogState(text="Path tolerance failure",
severity=Logger.REPORT_HINT),
transitions={'done': 'JV2MoveAction'},
autonomy={'done': Autonomy.High})
# x:1240 y:661
OperatableStateMachine.add('GoalTolFailure',
LogState(text="Goal tolerance failure",
severity=Logger.REPORT_HINT),
transitions={'done': 'JV2MoveAction'},
autonomy={'done': Autonomy.Off})
return _state_machine
def create(self):
# x:1453 y:629, x:807 y:466
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.action_topic = "/move_group"
_state_machine.userdata.trajectory_action_topic = "/m1n6s200_driver/arm_controller/follow_joint_trajectory"
_state_machine.userdata.move_group_arm = "arm"
_state_machine.userdata.config_vertical = "Vertical"
_state_machine.userdata.config_home = "Home"
_state_machine.userdata.config_retract = "Retract"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add(
'MoveIt',
SetupProxyMoveItClientState(
robot_description="/robot_description",
robot_description_semantic=None,
move_group_capabilities="/move_group",
action_type_and_topics=[[
"MoveGroupAction", ["/move_group"]
]],
enter_wait_duration=0.0),
transitions={
'connected': 'Connected',
'topics_unavailable': 'failed',
'param_error': 'failed'
},
autonomy={
'connected': Autonomy.High,
'topics_unavailable': Autonomy.Full,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'move_groups': 'move_groups'
})
# x:132 y:177
OperatableStateMachine.add('Connected',
LogState(text="Connected",
severity=Logger.REPORT_HINT),
transitions={'done': 'Vertical'},
autonomy={'done': Autonomy.Off})
# x:61 y:638
OperatableStateMachine.add('Vertical',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'GoalTolerances',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Low,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'selected_move_group':
'move_group_arm',
'config_name': 'config_vertical',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:823 y:29
OperatableStateMachine.add(
'Plan',
JointValuesToMoveItPlanState(timeout=5.0,
enter_wait_duration=0.5,
action_topic=None),
transitions={
'planned': 'Execute',
'failed': 'failed',
'topics_unavailable': 'failed',
'param_error': 'failed'
},
autonomy={
'planned': Autonomy.High,
'failed': Autonomy.Full,
'topics_unavailable': Autonomy.Full,
'param_error': Autonomy.Full
},
remapping={
'action_topic': 'action_topic',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values',
'joint_trajectory': 'joint_trajectory'
})
# x:1215 y:121
OperatableStateMachine.add(
'Execute',
TrajectoryToFollowJointTrajectoryActionState(
goal_time_tolerance=3.0,
max_delay=-1.0,
wait_duration=2.0,
timeout=1.0,
action_topic=None),
transitions={
'reached': 'Decision',
'goal_failed': 'GoalFail',
'path_failed': 'PathFail',
'invalid_request': 'failed',
'param_error': 'failed',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Full,
'goal_failed': Autonomy.Low,
'path_failed': Autonomy.Low,
'invalid_request': Autonomy.Full,
'param_error': Autonomy.Full,
'failed': Autonomy.Full
},
remapping={
'trajectory_action_topic': 'trajectory_action_topic',
'joint_trajectory': 'joint_trajectory',
'joint_goal_tolerances': 'joint_goal_tolerances',
'joint_path_tolerances': 'joint_path_tolerances',
'status_text': 'status_text',
'goal_names': 'goal_names',
'goal_values': 'goal_values'
})
# x:342 y:441
OperatableStateMachine.add('GoalTolerances',
SetJointTrajectoryTolerancesState(
position_constraints=[0.05],
velocity_constraints=[0.0],
acceleration_constraints=[0.0]),
transitions={
'configured': 'PathTolerances',
'param_error': 'failed'
},
autonomy={
'configured': Autonomy.Off,
'param_error': Autonomy.Full
},
remapping={
'joint_names':
'joint_names',
'joint_tolerances':
'joint_goal_tolerances'
})
# x:251 y:334
OperatableStateMachine.add('PathTolerances',
SetJointTrajectoryTolerancesState(
position_constraints=[0.08],
velocity_constraints=[0.0],
acceleration_constraints=[0.0]),
transitions={
'configured': 'DumpGoalTolerance',
'param_error': 'failed'
},
autonomy={
'configured': Autonomy.Off,
'param_error': Autonomy.Full
},
remapping={
'joint_names':
'joint_names',
'joint_tolerances':
'joint_path_tolerances'
})
# x:389 y:34
OperatableStateMachine.add(
'DumpGoalTolerance',
LogKeyState(text="Goal tolerance {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'DumpPathTolerance'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_goal_tolerances'})
# x:560 y:50
OperatableStateMachine.add(
'DumpPathTolerance',
LogKeyState(text="Path tolerance {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'Plan'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_path_tolerances'})
# x:889 y:579
OperatableStateMachine.add('Home',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'Plan',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Off,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'selected_move_group':
'move_group_arm',
'config_name': 'config_home',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:1265 y:670
OperatableStateMachine.add(
'Decision',
OperatorDecisionState(outcomes=["home", "vertical", "stop"],
hint=None,
suggestion=None),
transitions={
'home': 'Home',
'vertical': 'Vertical',
'stop': 'finished'
},
autonomy={
'home': Autonomy.Off,
'vertical': Autonomy.Off,
'stop': Autonomy.Full
})
# x:1238 y:387
OperatableStateMachine.add('PathFail',
LogKeyState(
text="Path failure {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'Decision'},
autonomy={'done': Autonomy.High},
remapping={'data': 'status_text'})
# x:1136 y:380
OperatableStateMachine.add('GoalFail',
LogKeyState(
text="Goal failure {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'Decision'},
autonomy={'done': Autonomy.High},
remapping={'data': 'status_text'})
return _state_machine
def create(self):
# x:1066 y:202, x:512 y:391
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.action_topic = "/move_group"
_state_machine.userdata.move_group_arm = "arm"
_state_machine.userdata.config_name = "Home"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:41 y:57
OperatableStateMachine.add(
'SetupMoveItProxy',
SetupProxyMoveItClientState(
robot_description="/robot_description",
robot_description_semantic=None,
move_group_capabilities="/move_group",
action_type_and_topics=None,
enter_wait_duration=0.5),
transitions={
'connected': 'GetHome',
'topics_unavailable': 'failed',
'param_error': 'failed'
},
autonomy={
'connected': Autonomy.Off,
'topics_unavailable': Autonomy.High,
'param_error': Autonomy.High
},
remapping={
'robot_name': 'robot_name',
'move_groups': 'move_groups'
})
# x:469 y:185
OperatableStateMachine.add('PrintConfig',
LogKeyState(
text=" Config {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'MoveHome'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_names'})
# x:698 y:110
OperatableStateMachine.add('MoveHome',
UserDataToMoveActionState(
joint_tolerance=0.05,
constraint_weight=1.0,
allowed_planning_time=2.0,
wait_duration=2.0,
timeout=30.0,
action_topic=None),
transitions={
'reached': 'MoveVertical',
'param_error': 'failed',
'planning_failed': 'failed',
'control_failed': 'failed',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Full,
'param_error': Autonomy.Full,
'planning_failed': Autonomy.Full,
'control_failed': Autonomy.Full,
'failed': Autonomy.Off
},
remapping={
'move_group': 'move_group',
'action_topic': 'action_topic',
'joint_names': 'joint_names',
'joint_values': 'joint_values',
'status_text': 'status_text'
})
# x:453 y:563
OperatableStateMachine.add(
'MoveVertical',
JointValuesToMoveActionState(joint_names=[
'm1n6s200_joint_1', 'm1n6s200_joint_2', 'm1n6s200_joint_3',
'm1n6s200_joint_4', 'm1n6s200_joint_5', 'm1n6s200_joint_6'
],
joint_values=[
0.0, 3.14, 3.14, 0.0, 0.0, 0.0
],
move_group="arm",
action_topic="/move_group",
joint_tolerance=0.05,
constraint_weight=1.0,
allowed_planning_time=2.0,
wait_duration=2.0,
timeout=10.0),
transitions={
'reached': 'GetHome',
'param_error': 'failed',
'planning_failed': 'failed',
'control_failed': 'failed',
'failed': 'failed'
},
autonomy={
'reached': Autonomy.Low,
'param_error': Autonomy.High,
'planning_failed': Autonomy.High,
'control_failed': Autonomy.High,
'failed': Autonomy.High
},
remapping={
'move_group': 'move_group',
'action_topic': 'action_topic',
'status_text': 'status_text',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:87 y:259
OperatableStateMachine.add('GetHome',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'PrintConfig',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.High,
'param_error': Autonomy.High
},
remapping={
'robot_name': 'robot_name',
'selected_move_group':
'move_group_arm',
'config_name': 'config_name',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
return _state_machine
def create(self):
# x:273 y:335, x:947 y:170
_state_machine = OperatableStateMachine(
outcomes=['finished', 'failed'])
_state_machine.userdata.action_topic = "/move_group"
_state_machine.userdata.trajectory_action_topic = "/m1n6s200_driver/arm_controller/follow_joint_trajectory"
_state_machine.userdata.gripper_action_topic = "/m1n6s200_driver/gripper_controller/follow_joint_trajectory"
_state_machine.userdata.move_group_arm = "arm"
_state_machine.userdata.move_group_gripper = "gripper"
_state_machine.userdata.config_name_vertical = "Vertical"
_state_machine.userdata.config_name_home = "Home"
_state_machine.userdata.config_name_retract = "Retract"
_state_machine.userdata.config_name_open = "Open"
_state_machine.userdata.config_name_close = "Close"
# Additional creation code can be added inside the following tags
# [MANUAL_CREATE]
# [/MANUAL_CREATE]
with _state_machine:
# x:30 y:40
OperatableStateMachine.add(
'MoveIt',
SetupProxyMoveItClientState(
robot_description="/robot_description",
robot_description_semantic=None,
move_group_capabilities="/move_group",
action_type_and_topics=[[
"MoveGroupAction", ["/move_group"]
]],
enter_wait_duration=2.0),
transitions={
'connected': 'Connected',
'topics_unavailable': 'failed',
'param_error': 'failed'
},
autonomy={
'connected': Autonomy.High,
'topics_unavailable': Autonomy.Full,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'move_groups': 'move_groups'
})
# x:132 y:177
OperatableStateMachine.add('Connected',
LogState(text="Connected",
severity=Logger.REPORT_HINT),
transitions={'done': 'Decision'},
autonomy={'done': Autonomy.Off})
# x:303 y:531
OperatableStateMachine.add('Vertical',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'GoalTolerances',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Low,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'selected_move_group':
'move_group_arm',
'config_name':
'config_name_vertical',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:823 y:29
OperatableStateMachine.add(
'Plan',
JointValuesToMoveItPlanState(timeout=5.0,
enter_wait_duration=0.5,
action_topic=None),
transitions={
'planned': 'SwitchController',
'failed': 'failed',
'topics_unavailable': 'failed',
'param_error': 'failed'
},
autonomy={
'planned': Autonomy.Low,
'failed': Autonomy.Full,
'topics_unavailable': Autonomy.Full,
'param_error': Autonomy.Full
},
remapping={
'action_topic': 'action_topic',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values',
'joint_trajectory': 'joint_trajectory'
})
# x:1611 y:323
OperatableStateMachine.add(
'Execute',
TrajectoryToFollowJointTrajectoryActionState(
goal_time_tolerance=3.0,
max_delay=-1.0,
wait_duration=2.0,
timeout=1.0,
action_topic=None),
transitions={
'reached': 'Decision',
'goal_failed': 'LogFail',
'path_failed': 'LogFail',
'invalid_request': 'InvalidRequestError',
'param_error': 'failed',
'failed': 'LogFail'
},
autonomy={
'reached': Autonomy.Full,
'goal_failed': Autonomy.Off,
'path_failed': Autonomy.Off,
'invalid_request': Autonomy.Off,
'param_error': Autonomy.Full,
'failed': Autonomy.Off
},
remapping={
'trajectory_action_topic': 'trajectory_action_topic',
'joint_trajectory': 'joint_trajectory',
'joint_goal_tolerances': 'joint_goal_tolerances',
'joint_path_tolerances': 'joint_path_tolerances',
'status_text': 'status_text',
'goal_names': 'goal_names',
'goal_values': 'goal_values'
})
# x:469 y:348
OperatableStateMachine.add('GoalTolerances',
SetJointTrajectoryTolerancesState(
position_constraints=[0.05],
velocity_constraints=[0.0],
acceleration_constraints=[0.0]),
transitions={
'configured': 'PathTolerances',
'param_error': 'failed'
},
autonomy={
'configured': Autonomy.Off,
'param_error': Autonomy.Full
},
remapping={
'joint_names':
'joint_names',
'joint_tolerances':
'joint_goal_tolerances'
})
# x:372 y:200
OperatableStateMachine.add('PathTolerances',
SetJointTrajectoryTolerancesState(
position_constraints=[0.08],
velocity_constraints=[0.0],
acceleration_constraints=[0.0]),
transitions={
'configured': 'DumpGoalTolerance',
'param_error': 'failed'
},
autonomy={
'configured': Autonomy.Off,
'param_error': Autonomy.Full
},
remapping={
'joint_names':
'joint_names',
'joint_tolerances':
'joint_path_tolerances'
})
# x:348 y:16
OperatableStateMachine.add(
'DumpGoalTolerance',
LogKeyState(text="Goal tolerance {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'DumpPathTolerance'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_goal_tolerances'})
# x:530 y:21
OperatableStateMachine.add(
'DumpPathTolerance',
LogKeyState(text="Path tolerance {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'LogActiveMoveGroup'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'joint_path_tolerances'})
# x:912 y:495
OperatableStateMachine.add('Home',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'GoalTolerances',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Low,
'param_error': Autonomy.Full
},
remapping={
'robot_name': 'robot_name',
'selected_move_group':
'move_group_arm',
'config_name': 'config_name_home',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:187 y:726
OperatableStateMachine.add('Decision',
OperatorDecisionState(outcomes=[
"home", "vertical", "stop", "close",
"open"
],
hint=None,
suggestion=None),
transitions={
'home': 'Home',
'vertical': 'Vertical',
'stop': 'finished',
'close': 'Close',
'open': 'Open'
},
autonomy={
'home': Autonomy.Low,
'vertical': Autonomy.Low,
'stop': Autonomy.Full,
'close': Autonomy.Low,
'open': Autonomy.Low
})
# x:748 y:676
OperatableStateMachine.add('Close',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'GoalTolerances',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Low,
'param_error': Autonomy.High
},
remapping={
'robot_name': 'robot_name',
'selected_move_group':
'move_group_gripper',
'config_name': 'config_name_close',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:1083 y:49
OperatableStateMachine.add('SwitchController',
DecisionState(
outcomes=["arm", "gripper"],
conditions=lambda mg: "gripper"
if mg == "gripper" else "arm"),
transitions={
'arm': 'Execute',
'gripper': 'Gripper'
},
autonomy={
'arm': Autonomy.Low,
'gripper': Autonomy.Low
},
remapping={'input_value': 'move_group'})
# x:1244 y:428
OperatableStateMachine.add(
'Gripper',
TrajectoryToFollowJointTrajectoryActionState(
goal_time_tolerance=3.0,
max_delay=-1.0,
wait_duration=2.0,
timeout=1.0,
action_topic=None),
transitions={
'reached': 'Decision',
'goal_failed': 'LogFail',
'path_failed': 'LogFail',
'invalid_request': 'InvalidRequestError',
'param_error': 'failed',
'failed': 'LogFail'
},
autonomy={
'reached': Autonomy.Off,
'goal_failed': Autonomy.Off,
'path_failed': Autonomy.Off,
'invalid_request': Autonomy.Off,
'param_error': Autonomy.High,
'failed': Autonomy.Off
},
remapping={
'trajectory_action_topic': 'gripper_action_topic',
'joint_trajectory': 'joint_trajectory',
'joint_goal_tolerances': 'joint_goal_tolerances',
'joint_path_tolerances': 'joint_path_tolerances',
'status_text': 'status_text',
'goal_names': 'goal_names',
'goal_values': 'goal_values'
})
# x:612 y:757
OperatableStateMachine.add('Open',
GetJointValuesFromSrdfConfigState(),
transitions={
'retrieved': 'GoalTolerances',
'param_error': 'failed'
},
autonomy={
'retrieved': Autonomy.Low,
'param_error': Autonomy.High
},
remapping={
'robot_name': 'robot_name',
'selected_move_group':
'move_group_gripper',
'config_name': 'config_name_open',
'move_group': 'move_group',
'joint_names': 'joint_names',
'joint_values': 'joint_values'
})
# x:745 y:1002
OperatableStateMachine.add('InvalidRequestError',
LogState(text="Invalid request",
severity=Logger.REPORT_HINT),
transitions={'done': 'Decision'},
autonomy={'done': Autonomy.Off})
# x:1315 y:971
OperatableStateMachine.add('LogFail',
LogKeyState(
text="Failed {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'Decision'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'status_text'})
# x:716 y:88
OperatableStateMachine.add('LogActiveMoveGroup',
LogKeyState(
text="Active Move Group is {}",
severity=Logger.REPORT_HINT),
transitions={'done': 'Plan'},
autonomy={'done': Autonomy.Off},
remapping={'data': 'move_group'})
return _state_machine