def __init__(self, log_benchmark_data=False): smach.StateMachine.__init__(self, outcomes=['success', 'failure'], input_keys=[ 'move_arm_to', 'reference_point', 'end_effector_pose', 'benchmark_state', 'logging_status' ]) with self: smach.StateMachine.add('MOVE_ARM_TO_CALIBRATION_CONFIGURATION', gms.move_arm(), transitions={ 'succeeded': 'START_LOGGING', 'failed': 'START_LOGGING' }) smach.StateMachine.add( 'START_LOGGING', start_logging_offline_data(log_benchmark_data), transitions={'success': 'STOP_COMPUTE_TASK_FRAME_TRANSFORM'}) smach.StateMachine.add( 'STOP_COMPUTE_TASK_FRAME_TRANSFORM', gbs.send_and_wait_events_combined( event_in_list=[('/compute_transform/event_in', 'e_stop')], event_out_list=[('/compute_transform/event_out', 'e_stopped', True)], timeout_duration=15), transitions={ 'success': 'PUBLISH_REFERENCE_POINT', 'timeout': 'PUBLISH_REFERENCE_POINT', 'failure': 'PUBLISH_REFERENCE_POINT' }) smach.StateMachine.add( 'PUBLISH_REFERENCE_POINT', cfs.publish_task_data( topic_name='/compute_transform/reference_point'), transitions={ 'success': 'COMPUTE_TASK_FRAME_TRANSFORM', 'failure': 'failure' }, remapping={'task_data': 'reference_point'}) smach.StateMachine.add( 'COMPUTE_TASK_FRAME_TRANSFORM', gbs.send_and_wait_events_combined( event_in_list=[('/compute_transform/event_in', 'e_start')], event_out_list=[('/compute_transform/event_out', 'e_success', True)], timeout_duration=15), transitions={ 'success': 'success', 'timeout': 'failure', 'failure': 'failure' })
def __init__(self, log_benchmark_data=False): smach.StateMachine.__init__(self, outcomes=['success', 'failure'], input_keys=['logging_status']) with self: smach.StateMachine.add( 'STOP_LOGGING', stop_logging_offline_data(log_benchmark_data), transitions={'success': 'STOP_ALL'}) smach.StateMachine.add( 'STOP_ALL', gbs.send_and_wait_events_combined(event_in_list=[ ('/compute_transform/event_in', 'e_stop'), ('/linear_interpolator_demo/event_in', 'e_stop'), ('/linear_interpolator_demo_trajectory_executor/event_in', 'e_stop'), ('/end_effector_pose_logger/event_in', 'e_stop') ]), transitions={ 'success': 'success', 'timeout': 'failure', 'failure': 'failure' })
def __init__(self, log_benchmark_data=False): smach.StateMachine.__init__(self, outcomes=['success'], input_keys=['logging_status']) with self: if log_benchmark_data: smach.StateMachine.add( 'RESET_FBM', cfs.reset_fbm(), transitions={'success': 'STOP_LOGGING_OFFLINE_DATA'}) smach.StateMachine.add( 'STOP_LOGGING_OFFLINE_DATA', gbs.send_and_wait_events_combined( event_in_list=[('/rosbag_recorder/event_in', 'e_stop')]), transitions={ 'success': 'SEND_LOGGING_STATUS', 'timeout': 'SEND_LOGGING_STATUS', 'failure': 'SEND_LOGGING_STATUS' }) else: smach.StateMachine.add( 'RESET_FBM', cfs.reset_fbm(), transitions={'success': 'SEND_LOGGING_STATUS'}) smach.StateMachine.add('SEND_LOGGING_STATUS', rockin_refbox.send_refbox_logging_status(), transitions={'done': 'success'})
def __init__(self, log_benchmark_data=False): smach.StateMachine.__init__( self, outcomes=["success"], input_keys=["logging_status"] ) with self: if log_benchmark_data: smach.StateMachine.add( "RESET_FBM", cfs.reset_fbm(), transitions={"success": "STOP_LOGGING_OFFLINE_DATA"}, ) smach.StateMachine.add( "STOP_LOGGING_OFFLINE_DATA", gbs.send_and_wait_events_combined( event_in_list=[("/rosbag_recorder/event_in", "e_stop")] ), transitions={ "success": "SEND_LOGGING_STATUS", "timeout": "SEND_LOGGING_STATUS", "failure": "SEND_LOGGING_STATUS", }, ) else: smach.StateMachine.add( "RESET_FBM", cfs.reset_fbm(), transitions={"success": "SEND_LOGGING_STATUS"}, ) smach.StateMachine.add( "SEND_LOGGING_STATUS", rockin_refbox.send_refbox_logging_status(), transitions={"done": "success"}, )
def __init__(self, log_benchmark_data=False): smach.StateMachine.__init__( self, outcomes=["success", "failure"], input_keys=["logging_status"] ) with self: smach.StateMachine.add( "STOP_LOGGING", stop_logging_offline_data(log_benchmark_data), transitions={"success": "STOP_ALL"}, ) smach.StateMachine.add( "STOP_ALL", gbs.send_and_wait_events_combined( event_in_list=[ ("/compute_transform/event_in", "e_stop"), ("/linear_interpolator_demo/event_in", "e_stop"), ( "/linear_interpolator_demo_trajectory_executor/event_in", "e_stop", ), ("/end_effector_pose_logger/event_in", "e_stop"), ] ), transitions={ "success": "success", "timeout": "failure", "failure": "failure", }, )
def __init__(self): smach.StateMachine.__init__(self, outcomes=['success', 'failure']) with self: smach.StateMachine.add( 'STOP_ALL', gbs.send_and_wait_events_combined(event_in_list=[ ('/compute_transform/event_in', 'e_stop'), ('/linear_interpolator_demo/event_in', 'e_stop'), ('/linear_interpolator_demo_trajectory_executor/event_in', 'e_stop'), ('/end_effector_pose_logger/event_in', 'e_stop') ]), transitions={ 'success': 'success', 'timeout': 'failure', 'failure': 'failure' })
def __init__(self): smach.StateMachine.__init__( self, outcomes=["success", "failure"], input_keys=[ "reference_point", "start_point", "goal_point", "move_arm_to", "logging_status", ], output_keys=[ "reference_point", "start_point", "goal_point", "move_arm_to", "logging_status", ], ) with self: smach.StateMachine.add( "STOP_ALL", stop_all(), transitions={ "success": "START_END_EFFECTOR_POSE_LOGGER", "failure": "failure", }, ) smach.StateMachine.add( "START_END_EFFECTOR_POSE_LOGGER", gbs.send_and_wait_events_combined( event_in_list=[("/end_effector_pose_logger/event_in", "e_start")] ), transitions={ "success": "INIT_DATA", "timeout": "failure", "failure": "failure", }, ) smach.StateMachine.add( "INIT_DATA", cfs.initialize_fbm(), transitions={"success": "success"} )
def __init__(self): smach.StateMachine.__init__(self, outcomes=["success", "failure"]) with self: smach.StateMachine.add( "STOP_ALL", gbs.send_and_wait_events_combined( event_in_list=[ ("/compute_transform/event_in", "e_stop"), ("/linear_interpolator_demo/event_in", "e_stop"), ( "/linear_interpolator_demo_trajectory_executor/event_in", "e_stop", ), ("/end_effector_pose_logger/event_in", "e_stop"), ] ), transitions={ "success": "success", "timeout": "failure", "failure": "failure", }, )
def __init__(self): smach.StateMachine.__init__(self, outcomes=['success', 'failure'], input_keys=[ 'reference_point', 'start_point', 'goal_point', 'move_arm_to', 'logging_status' ], output_keys=[ 'reference_point', 'start_point', 'goal_point', 'move_arm_to', 'logging_status' ]) with self: smach.StateMachine.add('STOP_ALL', stop_all(), transitions={ 'success': 'START_END_EFFECTOR_POSE_LOGGER', 'failure': 'failure' }) smach.StateMachine.add( 'START_END_EFFECTOR_POSE_LOGGER', gbs.send_and_wait_events_combined( event_in_list=[('/end_effector_pose_logger/event_in', 'e_start')]), transitions={ 'success': 'INIT_DATA', 'timeout': 'failure', 'failure': 'failure' }) smach.StateMachine.add('INIT_DATA', cfs.initialize_fbm(), transitions={'success': 'success'})
def __init__(self, use_mockup=None): smach.StateMachine.__init__(self, outcomes=['success', 'failure']) with self: #Opening the gripper smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'), transitions={'succeeded':'STOP_POSE_SHIFTER'}) # Sending STOP to all components smach.StateMachine.add('STOP_POSE_SHIFTER', gbs.send_event([('/plate_pose_shifter/event_in','e_stop')]), transitions={'success':'SET_NAMED_CONFIG_POSE_SHIFT'}) smach.StateMachine.add('SET_NAMED_CONFIG_POSE_SHIFT', gbs.set_named_config('plate_pose_shifter'), transitions={'success':'SHIFT_PLATE_POSE', 'failure':'failure', 'timeout': 'SET_NAMED_CONFIG_POSE_SHIFT'}) smach.StateMachine.add('SHIFT_PLATE_POSE', gbs.send_and_wait_events_combined( event_in_list=[('/plate_pose_shifter/event_in','e_start')], event_out_list=[('/plate_pose_shifter/event_out','e_success', True)], timeout_duration=5), transitions={'success':'SET_NAMED_CONFIG_PREGRASP', 'timeout':'failure', 'failure':'failure'}) smach.StateMachine.add('SET_NAMED_CONFIG_PREGRASP', gbs.set_named_config('conveyor_belt_plate_pickup'), transitions={'success':'PLAN_ARM_MOTION', 'failure':'failure', 'timeout': 'SET_NAMED_CONFIG_PREGRASP'}) smach.StateMachine.add('PLAN_ARM_MOTION', gbs.send_and_wait_events_combined( event_in_list=[('/pregrasp_planner_pipeline/event_in','e_start')], event_out_list=[('/pregrasp_planner_pipeline/event_out','e_success', True)]), transitions={'success':'STOP_PLAN_ARM_MOTION', 'timeout':'failure', 'failure':'failure'}) smach.StateMachine.add('STOP_PLAN_ARM_MOTION', gbs.send_event([('/pregrasp_planner_pipeline/event_in','e_stop')]), transitions={'success':'MOVE_ARM_TO_OBJECT'}) smach.StateMachine.add('MOVE_ARM_TO_OBJECT', gbs.send_and_wait_events_combined( event_in_list=[('/move_arm_planned/event_in','e_start')], event_out_list=[('/move_arm_planned/event_out','e_success', True)], timeout_duration=10), transitions={'success':'STOP_MOVE_ARM_TO_OBJECT', 'timeout':'failure', 'failure':'failure'}) smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT', gbs.send_event([('/move_arm_planned/event_in','e_stop')]), transitions={'success':'SET_NAMED_CONFIG_APPROACH_PLATE'}) smach.StateMachine.add('SET_NAMED_CONFIG_APPROACH_PLATE', gbs.set_named_config('approach_plate'), transitions={'success':'PLAN_APPROACH_PLATE', 'failure':'failure', 'timeout': 'SET_NAMED_CONFIG_APPROACH_PLATE'}) smach.StateMachine.add('PLAN_APPROACH_PLATE', gbs.send_and_wait_events_combined( event_in_list=[('/poses_to_move_wrtgripper/event_in','e_start')], event_out_list=[('/poses_to_move_wrtgripper/event_out','e_success', True)], timeout_duration=10), transitions={'success':'APPROACH_PLATE', 'timeout':'APPROACH_PLATE', 'failure':'APPROACH_PLATE'}) smach.StateMachine.add('APPROACH_PLATE', gbs.send_and_wait_events_combined( event_in_list=[('/cartesian_controller_demo/event_in','e_start')], event_out_list=[('/cartesian_controller_demo/event_out','e_success', True)], timeout_duration=10), transitions={'success':'CLOSE_GRIPPER', 'timeout':'CLOSE_GRIPPER', 'failure':'CLOSE_GRIPPER'}) smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'), transitions={'succeeded':'VERIFY_GRIPPER_CLOSED'}) smach.StateMachine.add('VERIFY_GRIPPER_CLOSED', gbs.send_and_wait_events_combined( event_in_list=[('/gripper_state_monitor/event_in','e_trigger')], event_out_list=[('/gripper_state_monitor/event_out','e_gripper_closed', True)], timeout_duration=5), transitions={'success':'STOP_APPROACH_PLATE', 'timeout':'CLOSE_GRIPPER', 'failure':'CLOSE_GRIPPER'}) smach.StateMachine.add('STOP_APPROACH_PLATE', gbs.send_event([('/cartesian_controller_demo/event_in','e_stop'), ('/poses_to_move_wrtgripper/event_in','e_stop')]), transitions={'success':'SET_NAMED_CONFIG_RETREAT_PLATE'}) smach.StateMachine.add('SET_NAMED_CONFIG_RETREAT_PLATE', gbs.set_named_config('retreat_plate'), transitions={'success':'PLAN_RETREAT_PLATE', 'failure':'failure', 'timeout': 'SET_NAMED_CONFIG_RETREAT_PLATE'}) smach.StateMachine.add('PLAN_RETREAT_PLATE', gbs.send_and_wait_events_combined( event_in_list=[('/poses_to_move_wrtgripper/event_in','e_start')], event_out_list=[('/poses_to_move_wrtgripper/event_out','e_success', True)], timeout_duration=10), transitions={'success':'RETREAT_PLATE', 'timeout':'RETREAT_PLATE', 'failure':'RETREAT_PLATE'}) smach.StateMachine.add('RETREAT_PLATE', gbs.send_and_wait_events_combined( event_in_list=[('/cartesian_controller_demo/event_in','e_start')], event_out_list=[('/cartesian_controller_demo/event_out','e_success', True)], timeout_duration=10), transitions={'success':'STOP_RETREAT_PLATE', 'timeout':'STOP_RETREAT_PLATE', 'failure':'STOP_RETREAT_PLATE'}) smach.StateMachine.add('STOP_RETREAT_PLATE', gbs.send_event([('/cartesian_controller_demo/event_in','e_stop'), ('/poses_to_move_wrtgripper/event_in','e_stop')]), transitions={'success':'MOVE_TO_HOLD'}) smach.StateMachine.add('MOVE_TO_HOLD', gms.move_arm('look_at_workspace'), transitions={'succeeded': 'success', 'failed': 'failure'})
def __init__(self, use_mockup=None): smach.StateMachine.__init__( self, outcomes=['success', 'failure'], input_keys=['is_object_grasped', 'end_effector_pose'], output_keys=['is_object_grasped', 'end_effector_pose']) with self: smach.StateMachine.add( 'SET_NAMED_CONFIG_PREGRASP', gbs.set_named_config('pregrasp_laying_object'), transitions={ 'success': 'PLAN_ARM_MOTION', 'failure': 'failure', 'timeout': 'SET_NAMED_CONFIG_PREGRASP' }) smach.StateMachine.add( 'PLAN_ARM_MOTION', gbs.send_and_wait_events_combined( event_in_list=[('/pregrasp_planner_pipeline/event_in', 'e_start')], event_out_list=[('/pregrasp_planner_pipeline/event_out', 'e_success', True)]), transitions={ 'success': 'STOP_PLAN_ARM_MOTION', 'timeout': 'failure', 'failure': 'failure' }) smach.StateMachine.add( 'STOP_PLAN_ARM_MOTION', gbs.send_event([('/pregrasp_planner_pipeline/event_in', 'e_stop')]), transitions={'success': 'MOVE_ARM_TO_OBJECT'}) smach.StateMachine.add( 'MOVE_ARM_TO_OBJECT', gbs.send_and_wait_events_combined( event_in_list=[('/move_arm_planned/event_in', 'e_start')], event_out_list=[('/move_arm_planned/event_out', 'e_success', True)], timeout_duration=15), transitions={ 'success': 'STOP_MOVE_ARM_TO_OBJECT', 'timeout': 'failure', 'failure': 'failure' }) smach.StateMachine.add( 'STOP_MOVE_ARM_TO_OBJECT', gbs.send_event([('/move_arm_planned/event_in', 'e_stop')]), transitions={'success': 'SAVE_GRIPPER_POSE'}) smach.StateMachine.add('SAVE_GRIPPER_POSE', mfs.save_gripper_pose(), transitions={'success': 'CLOSE_GRIPPER'}) smach.StateMachine.add( 'CLOSE_GRIPPER', gms.control_gripper('close'), transitions={'succeeded': 'VERIFY_GRIPPER_CLOSED'}) smach.StateMachine.add('VERIFY_GRIPPER_CLOSED', gbs.send_and_wait_events_combined( event_in_list=[ ('/gripper_state_monitor/event_in', 'e_trigger') ], event_out_list=[ ('/gripper_state_monitor/event_out', 'e_gripper_closed', True) ], timeout_duration=5), transitions={ 'success': 'SET_NAMED_CONFIG_LIFT_OBJECT', 'timeout': 'CLOSE_GRIPPER', 'failure': 'CLOSE_GRIPPER' }) smach.StateMachine.add('SET_NAMED_CONFIG_LIFT_OBJECT', gbs.set_named_config('lift_object'), transitions={ 'success': 'PLAN_LIFT_OBJECT', 'failure': 'failure', 'timeout': 'SET_NAMED_CONFIG_LIFT_OBJECT' }) smach.StateMachine.add('PLAN_LIFT_OBJECT', gbs.send_and_wait_events_combined( event_in_list=[ ('/poses_to_move_wrtbase/event_in', 'e_start') ], event_out_list=[ ('/poses_to_move_wrtbase/event_out', 'e_success', True) ], timeout_duration=3), transitions={ 'success': 'LIFT_OBJECT', 'timeout': 'LIFT_OBJECT', 'failure': 'LIFT_OBJECT' }) smach.StateMachine.add( 'LIFT_OBJECT', gbs.send_and_wait_events_combined( event_in_list=[('/cartesian_controller_demo/event_in', 'e_start')], event_out_list=[('/cartesian_controller_demo/event_out', 'e_success', True)], timeout_duration=10), transitions={ 'success': 'VERIFY_GRASPED', 'timeout': 'VERIFY_GRASPED', 'failure': 'VERIFY_GRASPED' }) smach.StateMachine.add( 'VERIFY_GRASPED', gbs.send_and_wait_events_combined( event_in_list=[ ('/gripper_controller/grasp_monitor/event_in', 'e_trigger') ], event_out_list=[ ('/gripper_controller/grasp_monitor/event_out', 'e_object_grasped', True) ], timeout_duration=10), transitions={ 'success': 'SET_OBJECT_GRASPED', 'timeout': 'SET_OBJECT_NOT_GRASPED', 'failure': 'SET_OBJECT_NOT_GRASPED' }) smach.StateMachine.add('SET_OBJECT_GRASPED', mfs.set_is_object_grasped(True), transitions={'success': 'success'}) smach.StateMachine.add('SET_OBJECT_NOT_GRASPED', mfs.set_is_object_grasped(False), transitions={'success': 'success'})
def __init__(self, use_mockup=None): smach.StateMachine.__init__(self, outcomes=["success", "failure"]) with self: # Opening the gripper smach.StateMachine.add( "OPEN_GRIPPER", gms.control_gripper("open"), transitions={"succeeded": "STOP_POSE_SHIFTER"}, ) # Sending STOP to all components smach.StateMachine.add( "STOP_POSE_SHIFTER", gbs.send_event([("/plate_pose_shifter/event_in", "e_stop")]), transitions={"success": "SET_NAMED_CONFIG_POSE_SHIFT"}, ) smach.StateMachine.add( "SET_NAMED_CONFIG_POSE_SHIFT", gbs.set_named_config("plate_pose_shifter"), transitions={ "success": "SHIFT_PLATE_POSE", "failure": "failure", "timeout": "SET_NAMED_CONFIG_POSE_SHIFT", }, ) smach.StateMachine.add( "SHIFT_PLATE_POSE", gbs.send_and_wait_events_combined( event_in_list=[("/plate_pose_shifter/event_in", "e_start") ], event_out_list=[("/plate_pose_shifter/event_out", "e_success", True)], timeout_duration=5, ), transitions={ "success": "SET_NAMED_CONFIG_PREGRASP", "timeout": "failure", "failure": "failure", }, ) smach.StateMachine.add( "SET_NAMED_CONFIG_PREGRASP", gbs.set_named_config("conveyor_belt_plate_pickup"), transitions={ "success": "PLAN_ARM_MOTION", "failure": "failure", "timeout": "SET_NAMED_CONFIG_PREGRASP", }, ) smach.StateMachine.add( "PLAN_ARM_MOTION", gbs.send_and_wait_events_combined( event_in_list=[("/pregrasp_planner_pipeline/event_in", "e_start")], event_out_list=[("/pregrasp_planner_pipeline/event_out", "e_success", True)], ), transitions={ "success": "STOP_PLAN_ARM_MOTION", "timeout": "failure", "failure": "failure", }, ) smach.StateMachine.add( "STOP_PLAN_ARM_MOTION", gbs.send_event([("/pregrasp_planner_pipeline/event_in", "e_stop")]), transitions={"success": "MOVE_ARM_TO_OBJECT"}, ) smach.StateMachine.add( "MOVE_ARM_TO_OBJECT", gbs.send_and_wait_events_combined( event_in_list=[("/move_arm_planned/event_in", "e_start")], event_out_list=[("/move_arm_planned/event_out", "e_success", True)], timeout_duration=10, ), transitions={ "success": "STOP_MOVE_ARM_TO_OBJECT", "timeout": "failure", "failure": "failure", }, ) smach.StateMachine.add( "STOP_MOVE_ARM_TO_OBJECT", gbs.send_event([("/move_arm_planned/event_in", "e_stop")]), transitions={"success": "SET_NAMED_CONFIG_APPROACH_PLATE"}, ) smach.StateMachine.add( "SET_NAMED_CONFIG_APPROACH_PLATE", gbs.set_named_config("approach_plate"), transitions={ "success": "PLAN_APPROACH_PLATE", "failure": "failure", "timeout": "SET_NAMED_CONFIG_APPROACH_PLATE", }, ) smach.StateMachine.add( "PLAN_APPROACH_PLATE", gbs.send_and_wait_events_combined( event_in_list=[("/poses_to_move_wrtgripper/event_in", "e_start")], event_out_list=[("/poses_to_move_wrtgripper/event_out", "e_success", True)], timeout_duration=10, ), transitions={ "success": "APPROACH_PLATE", "timeout": "APPROACH_PLATE", "failure": "APPROACH_PLATE", }, ) smach.StateMachine.add( "APPROACH_PLATE", gbs.send_and_wait_events_combined( event_in_list=[("/cartesian_controller_demo/event_in", "e_start")], event_out_list=[("/cartesian_controller_demo/event_out", "e_success", True)], timeout_duration=10, ), transitions={ "success": "CLOSE_GRIPPER", "timeout": "CLOSE_GRIPPER", "failure": "CLOSE_GRIPPER", }, ) smach.StateMachine.add( "CLOSE_GRIPPER", gms.control_gripper("close"), transitions={"succeeded": "VERIFY_GRIPPER_CLOSED"}, ) smach.StateMachine.add( "VERIFY_GRIPPER_CLOSED", gbs.send_and_wait_events_combined( event_in_list=[("/gripper_state_monitor/event_in", "e_trigger")], event_out_list=[("/gripper_state_monitor/event_out", "e_gripper_closed", True)], timeout_duration=5, ), transitions={ "success": "STOP_APPROACH_PLATE", "timeout": "CLOSE_GRIPPER", "failure": "CLOSE_GRIPPER", }, ) smach.StateMachine.add( "STOP_APPROACH_PLATE", gbs.send_event([ ("/cartesian_controller_demo/event_in", "e_stop"), ("/poses_to_move_wrtgripper/event_in", "e_stop"), ]), transitions={"success": "SET_NAMED_CONFIG_RETREAT_PLATE"}, ) smach.StateMachine.add( "SET_NAMED_CONFIG_RETREAT_PLATE", gbs.set_named_config("retreat_plate"), transitions={ "success": "PLAN_RETREAT_PLATE", "failure": "failure", "timeout": "SET_NAMED_CONFIG_RETREAT_PLATE", }, ) smach.StateMachine.add( "PLAN_RETREAT_PLATE", gbs.send_and_wait_events_combined( event_in_list=[("/poses_to_move_wrtgripper/event_in", "e_start")], event_out_list=[("/poses_to_move_wrtgripper/event_out", "e_success", True)], timeout_duration=10, ), transitions={ "success": "RETREAT_PLATE", "timeout": "RETREAT_PLATE", "failure": "RETREAT_PLATE", }, ) smach.StateMachine.add( "RETREAT_PLATE", gbs.send_and_wait_events_combined( event_in_list=[("/cartesian_controller_demo/event_in", "e_start")], event_out_list=[("/cartesian_controller_demo/event_out", "e_success", True)], timeout_duration=10, ), transitions={ "success": "STOP_RETREAT_PLATE", "timeout": "STOP_RETREAT_PLATE", "failure": "STOP_RETREAT_PLATE", }, ) smach.StateMachine.add( "STOP_RETREAT_PLATE", gbs.send_event([ ("/cartesian_controller_demo/event_in", "e_stop"), ("/poses_to_move_wrtgripper/event_in", "e_stop"), ]), transitions={"success": "MOVE_TO_HOLD"}, ) smach.StateMachine.add( "MOVE_TO_HOLD", gms.move_arm("look_at_workspace"), transitions={ "succeeded": "success", "failed": "failure" }, )
def __init__(self): smach.StateMachine.__init__(self, outcomes=['success', 'failure'], input_keys=['start', 'goal']) with self: smach.StateMachine.add( 'PUBLISH_START_POINT', cfs.publish_task_data( topic_name='/path_generator/start_point'), transitions={ 'success': 'PUBLISH_GOAL_POINT', 'failure': 'failure' }, remapping={'task_data': 'start'}) smach.StateMachine.add( 'PUBLISH_GOAL_POINT', cfs.publish_task_data(topic_name='/path_generator/end_point'), transitions={ 'success': 'PLAN_PATH', 'failure': 'failure' }, remapping={'task_data': 'goal'}) smach.StateMachine.add( 'PLAN_PATH', gbs.send_and_wait_events_combined( event_in_list=[('/linear_interpolator_demo/event_in', 'e_start')], event_out_list=[('/linear_interpolator_demo/event_out', 'e_success', True)], timeout_duration=30), transitions={ 'success': 'EXECUTE_PLAN', 'timeout': 'failure', 'failure': 'failure' }) smach.StateMachine.add( 'EXECUTE_PLAN', gbs.send_and_wait_events_combined( event_in_list= [('/linear_interpolator_demo_trajectory_executor/event_in', 'e_start')], event_out_list= [('/linear_interpolator_demo_trajectory_executor/event_out', 'e_success', True)], timeout_duration=60), transitions={ 'success': 'STOP_ALL', 'timeout': 'failure', 'failure': 'failure' }) smach.StateMachine.add( 'STOP_ALL', gbs.send_and_wait_events_combined(event_in_list=[ ('/linear_interpolator_demo/event_in', 'e_stop'), ('/linear_interpolator_demo_trajectory_executor/event_in', 'e_stop') ], timeout_duration=15), transitions={ 'success': 'success', 'timeout': 'failure', 'failure': 'failure' })
def __init__(self, log_benchmark_data=False): smach.StateMachine.__init__( self, outcomes=["success", "failure"], input_keys=[ "move_arm_to", "reference_point", "end_effector_pose", "benchmark_state", "logging_status", ], ) with self: smach.StateMachine.add( "MOVE_ARM_TO_CALIBRATION_CONFIGURATION", gms.move_arm(), transitions={"succeeded": "START_LOGGING", "failed": "START_LOGGING"}, ) smach.StateMachine.add( "START_LOGGING", start_logging_offline_data(log_benchmark_data), transitions={"success": "STOP_COMPUTE_TASK_FRAME_TRANSFORM"}, ) smach.StateMachine.add( "STOP_COMPUTE_TASK_FRAME_TRANSFORM", gbs.send_and_wait_events_combined( event_in_list=[("/compute_transform/event_in", "e_stop")], event_out_list=[ ("/compute_transform/event_out", "e_stopped", True) ], timeout_duration=15, ), transitions={ "success": "PUBLISH_REFERENCE_POINT", "timeout": "PUBLISH_REFERENCE_POINT", "failure": "PUBLISH_REFERENCE_POINT", }, ) smach.StateMachine.add( "PUBLISH_REFERENCE_POINT", cfs.publish_task_data(topic_name="/compute_transform/reference_point"), transitions={ "success": "COMPUTE_TASK_FRAME_TRANSFORM", "failure": "failure", }, remapping={"task_data": "reference_point"}, ) smach.StateMachine.add( "COMPUTE_TASK_FRAME_TRANSFORM", gbs.send_and_wait_events_combined( event_in_list=[("/compute_transform/event_in", "e_start")], event_out_list=[ ("/compute_transform/event_out", "e_success", True) ], timeout_duration=15, ), transitions={ "success": "success", "timeout": "failure", "failure": "failure", }, )
def __init__(self): smach.StateMachine.__init__( self, outcomes=["success", "failure"], input_keys=["start", "goal"] ) with self: smach.StateMachine.add( "PUBLISH_START_POINT", cfs.publish_task_data(topic_name="/path_generator/start_point"), transitions={"success": "PUBLISH_GOAL_POINT", "failure": "failure"}, remapping={"task_data": "start"}, ) smach.StateMachine.add( "PUBLISH_GOAL_POINT", cfs.publish_task_data(topic_name="/path_generator/end_point"), transitions={"success": "PLAN_PATH", "failure": "failure"}, remapping={"task_data": "goal"}, ) smach.StateMachine.add( "PLAN_PATH", gbs.send_and_wait_events_combined( event_in_list=[("/linear_interpolator_demo/event_in", "e_start")], event_out_list=[ ("/linear_interpolator_demo/event_out", "e_success", True) ], timeout_duration=30, ), transitions={ "success": "EXECUTE_PLAN", "timeout": "failure", "failure": "failure", }, ) smach.StateMachine.add( "EXECUTE_PLAN", gbs.send_and_wait_events_combined( event_in_list=[ ( "/linear_interpolator_demo_trajectory_executor/event_in", "e_start", ) ], event_out_list=[ ( "/linear_interpolator_demo_trajectory_executor/event_out", "e_success", True, ) ], timeout_duration=60, ), transitions={ "success": "STOP_ALL", "timeout": "failure", "failure": "failure", }, ) smach.StateMachine.add( "STOP_ALL", gbs.send_and_wait_events_combined( event_in_list=[ ("/linear_interpolator_demo/event_in", "e_stop"), ( "/linear_interpolator_demo_trajectory_executor/event_in", "e_stop", ), ], timeout_duration=15, ), transitions={ "success": "success", "timeout": "failure", "failure": "failure", }, )
def __init__(self, use_mockup=None): smach.StateMachine.__init__( self, outcomes=["success", "failure"], input_keys=["is_object_grasped", "end_effector_pose"], output_keys=["is_object_grasped", "end_effector_pose"], ) with self: smach.StateMachine.add( "SET_NAMED_CONFIG_PREGRASP", gbs.set_named_config("pregrasp_laying_object"), transitions={ "success": "PLAN_ARM_MOTION", "failure": "failure", "timeout": "SET_NAMED_CONFIG_PREGRASP", }, ) smach.StateMachine.add( "PLAN_ARM_MOTION", gbs.send_and_wait_events_combined( event_in_list=[("/pregrasp_planner_pipeline/event_in", "e_start")], event_out_list=[("/pregrasp_planner_pipeline/event_out", "e_success", True)], ), transitions={ "success": "STOP_PLAN_ARM_MOTION", "timeout": "failure", "failure": "failure", }, ) smach.StateMachine.add( "STOP_PLAN_ARM_MOTION", gbs.send_event([("/pregrasp_planner_pipeline/event_in", "e_stop")]), transitions={"success": "MOVE_ARM_TO_OBJECT"}, ) smach.StateMachine.add( "MOVE_ARM_TO_OBJECT", gbs.send_and_wait_events_combined( event_in_list=[("/move_arm_planned/event_in", "e_start")], event_out_list=[("/move_arm_planned/event_out", "e_success", True)], timeout_duration=15, ), transitions={ "success": "STOP_MOVE_ARM_TO_OBJECT", "timeout": "failure", "failure": "failure", }, ) smach.StateMachine.add( "STOP_MOVE_ARM_TO_OBJECT", gbs.send_event([("/move_arm_planned/event_in", "e_stop")]), transitions={"success": "SAVE_GRIPPER_POSE"}, ) smach.StateMachine.add( "SAVE_GRIPPER_POSE", mfs.save_gripper_pose(), transitions={"success": "CLOSE_GRIPPER"}, ) smach.StateMachine.add( "CLOSE_GRIPPER", gms.control_gripper("close"), transitions={"succeeded": "VERIFY_GRIPPER_CLOSED"}, ) smach.StateMachine.add( "VERIFY_GRIPPER_CLOSED", gbs.send_and_wait_events_combined( event_in_list=[("/gripper_state_monitor/event_in", "e_trigger")], event_out_list=[("/gripper_state_monitor/event_out", "e_gripper_closed", True)], timeout_duration=5, ), transitions={ "success": "SET_NAMED_CONFIG_LIFT_OBJECT", "timeout": "CLOSE_GRIPPER", "failure": "CLOSE_GRIPPER", }, ) smach.StateMachine.add( "SET_NAMED_CONFIG_LIFT_OBJECT", gbs.set_named_config("lift_object"), transitions={ "success": "PLAN_LIFT_OBJECT", "failure": "failure", "timeout": "SET_NAMED_CONFIG_LIFT_OBJECT", }, ) smach.StateMachine.add( "PLAN_LIFT_OBJECT", gbs.send_and_wait_events_combined( event_in_list=[("/poses_to_move_wrtbase/event_in", "e_start")], event_out_list=[("/poses_to_move_wrtbase/event_out", "e_success", True)], timeout_duration=3, ), transitions={ "success": "LIFT_OBJECT", "timeout": "LIFT_OBJECT", "failure": "LIFT_OBJECT", }, ) smach.StateMachine.add( "LIFT_OBJECT", gbs.send_and_wait_events_combined( event_in_list=[("/cartesian_controller_demo/event_in", "e_start")], event_out_list=[("/cartesian_controller_demo/event_out", "e_success", True)], timeout_duration=10, ), transitions={ "success": "VERIFY_GRASPED", "timeout": "VERIFY_GRASPED", "failure": "VERIFY_GRASPED", }, ) smach.StateMachine.add( "VERIFY_GRASPED", gbs.send_and_wait_events_combined( event_in_list=[ ("/gripper_controller/grasp_monitor/event_in", "e_trigger") ], event_out_list=[( "/gripper_controller/grasp_monitor/event_out", "e_object_grasped", True, )], timeout_duration=10, ), transitions={ "success": "SET_OBJECT_GRASPED", "timeout": "SET_OBJECT_NOT_GRASPED", "failure": "SET_OBJECT_NOT_GRASPED", }, ) smach.StateMachine.add( "SET_OBJECT_GRASPED", mfs.set_is_object_grasped(True), transitions={"success": "success"}, ) smach.StateMachine.add( "SET_OBJECT_NOT_GRASPED", mfs.set_is_object_grasped(False), transitions={"success": "success"}, )