def main():
rospy.init_node('perceive_location_server')
sleep_time = rospy.get_param('~sleep_time', 1.0)
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['perceive_location_goal'],
output_keys=['perceive_location_feedback', 'perceive_location_result'])
# Open the container
with sm:
# approach to platform
smach.StateMachine.add(
'GET_GOAL',
getGoal(),
transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME'})
#transitions={'succeeded':'START_OBJECT_LIST_MERGER'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
'PUBLISH_REFERENCE_FRAME',
gbs.send_event([('/static_transform_publisher_node/event_in',
'e_start')]),
transitions={'success': 'SUBSCRIBE_TO_POINT_CLOUD'})
smach.StateMachine.add(
'SUBSCRIBE_TO_POINT_CLOUD',
gbs.send_event([('/mcr_perception/mux_pointcloud/select',
'/arm_cam3d/depth_registered/points')]),
transitions={'success': 'START_OBJECT_LIST_MERGER'})
smach.StateMachine.add(
'START_OBJECT_LIST_MERGER',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/object_list_merger/event_in', 'e_start'),
('/mcr_perception/object_selector/event_in', 'e_start')
],
event_out_list=[
('/mcr_perception/object_list_merger/event_out',
'e_started', True)
],
timeout_duration=5),
transitions={
'success': 'LOOK_AT_WORKSPACE',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# send arm to a position in which the depth camera can see the platformsmach.StateMachine.add('LOOK_AT_WORKSPACE_LEFT', gms.move_arm('look_at_workspace_LEFT'),
smach.StateMachine.add('LOOK_AT_WORKSPACE',
gms.move_arm('look_at_workspace_from_far'),
transitions={
'succeeded': 'START_SEGMENTATION',
'failed': 'LOOK_AT_WORKSPACE'
})
# this is new scene segmentation pipeline combined with object detection
smach.StateMachine.add(
'START_SEGMENTATION',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/scene_segmentation/event_in',
'e_start')],
event_out_list=[
('/mcr_perception/scene_segmentation/event_out',
'e_started', True)
],
timeout_duration=5),
transitions={
'success': 'WAIT_LEFT',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
smach.StateMachine.add('WAIT_LEFT',
wait_for(sleep_time=sleep_time),
transitions={'success': 'ADD_POINT_CLOUD'})
smach.StateMachine.add(
'ADD_POINT_CLOUD',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/scene_segmentation/event_in',
'e_add_cloud_start')],
event_out_list=[
('/mcr_perception/scene_segmentation/event_out',
'e_add_cloud_stopped', True)
],
timeout_duration=5),
transitions={
'success': 'RECOGNIZE_OBJECTS',
'timeout': 'ADD_POINT_CLOUD',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# in the new scene_segmentation pipeline, object detector can be called by string msg e_segment
smach.StateMachine.add(
'RECOGNIZE_OBJECTS',
gbs.send_and_wait_events_combined(
event_in_list=
[('/mcr_perception/multimodal_object_identification/input/event_in',
'e_trigger'),
('/mcr_perception/scene_segmentation/event_in', 'e_segment')],
event_out_list=
[('/mcr_perception/scene_segmentation/event_out', 'e_done',
True),
('/mcr_perception/multimodal_object_identification/output/event_out',
'e_done', True)],
timeout_duration=7),
transitions={
'success': 'STOP_PUBLISH_ACCUMULATED_PC',
'timeout': 'STOP_PUBLISH_ACCUMULATED_PC',
'failure': 'STOP_PUBLISH_ACCUMULATED_PC'
})
smach.StateMachine.add(
'STOP_PUBLISH_ACCUMULATED_PC',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/scene_segmentation/event_in',
'e_stop')],
event_out_list=[
('/mcr_perception/scene_segmentation/event_out',
'e_stopped', True)
],
timeout_duration=5),
transitions={
'success': 'STOP_COMPONENTS',
'timeout': 'STOP_COMPONENTS',
'failure': 'STOP_COMPONENTS'
})
smach.StateMachine.add(
'STOP_COMPONENTS',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/object_list_merger/event_in', 'e_stop'),
('/mcr_perception/scene_segmentation/event_in', 'e_stop')
],
event_out_list=[
('/mcr_perception/object_list_merger/event_out',
'e_stopped', True),
('/mcr_perception/scene_segmentation/event_out',
'e_stopped', True)
],
timeout_duration=5),
transitions={
'success': 'UNSUBSCRIBE_FROM_POINT_CLOUD',
'timeout': 'UNSUBSCRIBE_FROM_POINT_CLOUD',
'failure': 'UNSUBSCRIBE_FROM_POINT_CLOUD'
})
smach.StateMachine.add(
'UNSUBSCRIBE_FROM_POINT_CLOUD',
gbs.send_event([('/mcr_perception/mux_pointcloud/select',
'/empty_topic')]),
transitions={'success': 'PUBLISH_MERGED_OBJECT_LIST'})
smach.StateMachine.add(
'PUBLISH_MERGED_OBJECT_LIST',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/object_list_merger/event_in',
'e_trigger')],
event_out_list=[
('/mcr_perception/object_list_merger/event_out', 'e_done',
True)
],
timeout_duration=5),
transitions={
'success': 'SET_ACTION_LIB_SUCCESS',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_SUCCESS'
})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('perceive_location_smach_viewer', sm,
'/PERCEIVE_LOCATION_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='perceive_location_server',
action_spec=PerceiveLocationAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='perceive_location_goal',
feedback_key='perceive_location_feedback',
result_key='perceive_location_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("move_base_safe_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED", "OVERALL_PREEMPTED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
smach.StateMachine.add(
"CHECK_IF_BARRIER_TAPE_ENABLED",
CheckDontBeSafe(),
transitions={
"safe": "START_BARRIER_TAPE_DETECTION",
"unsafe": "SETUP_MOVE_BASE",
},
)
smach.StateMachine.add(
"START_BARRIER_TAPE_DETECTION",
gbs.send_event([(
"/mir_perception/barrier_tape_detection/event_in",
"e_start",
)]),
transitions={"success": "MOVE_ARM_TO_DETECT_BARRIER_TAPE"},
)
smach.StateMachine.add(
"MOVE_ARM_TO_DETECT_BARRIER_TAPE",
gms.move_arm("barrier_tape", blocking=False),
transitions={
"succeeded": "SETUP_MOVE_BASE",
"failed": "MOVE_ARM_TO_DETECT_BARRIER_TAPE"
},
)
# get pose from action lib as string, convert to pose stamped and publish
smach.StateMachine.add(
"SETUP_MOVE_BASE",
SetupMoveBase("/move_base_wrapper/pose_in"),
transitions={
"succeeded": "SET_DIRECT_BASE_CONTROLLER_PARAMETERS",
"failed": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE",
"preempted": "OVERALL_PREEMPTED",
},
)
smach.StateMachine.add(
"SET_DIRECT_BASE_CONTROLLER_PARAMETERS",
gbs.set_named_config("dbc_move_base"),
transitions={
"success": "START_MOVE_BASE",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"START_MOVE_BASE",
StartMoveBase(
event_in_topic="/move_base_wrapper/event_in",
event_out_topic="/move_base_wrapper/event_out",
timeout_duration=50,
),
transitions={
"success": "PREPARE_ARM_FOR_NEXT_ACTION",
"timeout": "RESET_BARRIER_TAPE",
"failure": "RESET_BARRIER_TAPE",
"stopped": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE",
"preempted": "OVERALL_PREEMPTED",
},
)
smach.StateMachine.add(
"RESET_BARRIER_TAPE",
gbs.send_event([(
"/mir_perception/barrier_tape_detection/event_in",
"e_reset",
), (
"/move_base_wrapper/event_in",
"e_stop",
)]),
transitions={"success": "SETUP_MOVE_BASE_AGAIN"},
)
smach.StateMachine.add(
"SETUP_MOVE_BASE_AGAIN",
SetupMoveBase("/move_base_wrapper/pose_in"),
transitions={
"succeeded": "START_MOVE_BASE_AGAIN",
"failed": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE",
"preempted": "OVERALL_PREEMPTED",
},
)
smach.StateMachine.add(
"START_MOVE_BASE_AGAIN",
gbs.send_and_wait_events_combined(
event_in_list=[("/move_base_wrapper/event_in", "e_start")],
event_out_list=[("/move_base_wrapper/event_out", "e_success",
True)],
timeout_duration=50,
),
transitions={
"success": "PREPARE_ARM_FOR_NEXT_ACTION",
"timeout": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE",
"failure": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE",
},
)
# send arm to a position depending on next action
smach.StateMachine.add(
"PREPARE_ARM_FOR_NEXT_ACTION",
PrepareArmForNextAction(),
transitions={
"succeeded": "MOVE_ARM_FOR_NEXT_ACTION",
"skipped": "REACH_DESTINATION_PRECISELY",
"failed": "PREPARE_ARM_FOR_NEXT_ACTION",
},
)
smach.StateMachine.add(
"MOVE_ARM_FOR_NEXT_ACTION",
gms.move_arm(blocking=False),
transitions={
"succeeded": "REACH_DESTINATION_PRECISELY",
"failed": "REACH_DESTINATION_PRECISELY"
},
)
# call direct base controller to fine adjust the base to the final desired pose
# (navigation tolerance is set to a wide tolerance)
smach.StateMachine.add(
"REACH_DESTINATION_PRECISELY",
gbs.send_and_wait_events_combined(
event_in_list=[
(
"/mcr_navigation/direct_base_controller/coordinator/event_in",
"e_start",
),
],
event_out_list=[(
"/mcr_navigation/direct_base_controller/coordinator/event_out",
"e_success",
True,
)],
timeout_duration=5,
), # this is a tradeoff between speed and accuracy, set a higher value for accuracy increase
transitions={
"success": "STOP_CONTROLLER_WITH_SUCCESS",
"timeout": "STOP_CONTROLLER_WITH_SUCCESS",
"failure": "STOP_CONTROLLER_WITH_FAILURE",
},
)
# stop controller with success
smach.StateMachine.add(
"STOP_CONTROLLER_WITH_SUCCESS",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mcr_navigation/direct_base_controller/coordinator/event_in",
"e_stop",
)],
event_out_list=[(
"/mcr_navigation/direct_base_controller/coordinator/event_out",
"e_stopped",
True,
)],
timeout_duration=1,
),
transitions={
"success": "STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS",
"timeout": "STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS",
"failure": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE",
},
)
# stop controller with failure
smach.StateMachine.add(
"STOP_CONTROLLER_WITH_FAILURE",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mcr_navigation/direct_base_controller/coordinator/event_in",
"e_stop",
)],
event_out_list=[(
"/mcr_navigation/direct_base_controller/coordinator/event_out",
"e_stopped",
True,
)],
timeout_duration=1,
),
transitions={
"success": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE",
"timeout": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE",
"failure": "STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE",
gbs.send_event([("/mir_perception/barrier_tape_detection/event_in",
"e_stop"),
(
"/move_base_wrapper/event_in",
"e_stop",
)]),
transitions={"success": "OVERALL_FAILED"},
)
smach.StateMachine.add(
"STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS",
gbs.send_event([("/mir_perception/barrier_tape_detection/event_in",
"e_stop")]),
transitions={"success": "ALIGN_WITH_WORKSPACE"},
)
smach.StateMachine.add(
"ALIGN_WITH_WORKSPACE",
AlignWithWorkspace(),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "OVERALL_SUCCESS",
},
)
state_publisher = rospy.Publisher('~current_state', String, queue_size=1)
sm.register_transition_cb(transition_cb, [state_publisher])
sm.register_start_cb(start_cb, [state_publisher])
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = smach_ros.IntrospectionServer("move_base_safe_smach_viewer", sm,
"/MOVE_BASE_SAFE_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="move_base_safe_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["OVERALL_PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('perceive_location_server')
sleep_time = rospy.get_param('~sleep_time', 1.0)
# Construct state machine
sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['goal'],
output_keys=['feedback', 'result'])
# Open the container
sm.userdata.arm_pose_list = [
'look_at_workspace_from_near', 'look_at_workspace_from_near_left',
'look_at_workspace_from_near_right'
]
sm.userdata.arm_pose_index = 0
base_x_offset = rospy.get_param('~base_x_offset', 0.0)
base_y_offset = rospy.get_param('~base_y_offset', 0.25)
base_theta_offset = rospy.get_param('~base_theta_offset', 0.0)
sm.userdata.base_pose_list = [{
'x': 0.0,
'y': 0.0,
'theta': 0.0
}, {
'x': base_x_offset,
'y': base_y_offset,
'theta': base_theta_offset
}, {
'x': base_x_offset,
'y': -base_y_offset,
'theta': -base_theta_offset
}]
sm.userdata.base_pose_index = 0
with sm:
# approach to platform
smach.StateMachine.add(
'SETUP',
Setup(),
transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME'})
# publish a static frame which will be used as reference for perceived objs
smach.StateMachine.add(
'PUBLISH_REFERENCE_FRAME',
gbs.send_event([('/static_transform_publisher_node/event_in',
'e_start')]),
transitions={'success': 'SET_DBC_PARAMS'})
# publish a static frame which will be used as reference for perceived objs
smach.StateMachine.add('SET_DBC_PARAMS',
gbs.set_named_config('dbc_pick_object'),
transitions={
'success': 'START_OBJECT_LIST_MERGER',
'timeout': 'OVERALL_FAILED',
'failure': 'OVERALL_FAILED'
})
smach.StateMachine.add(
'START_OBJECT_LIST_MERGER',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/object_list_merger/event_in', 'e_start'),
('/mcr_perception/object_selector/event_in', 'e_start')
],
event_out_list=[
('/mcr_perception/object_list_merger/event_out',
'e_started', True)
],
timeout_duration=5),
transitions={
'success': 'GET_MOTION_TYPE',
'timeout': 'OVERALL_FAILED',
'failure': 'OVERALL_FAILED'
})
smach.StateMachine.add('GET_MOTION_TYPE',
GetMotionType(),
transitions={
'base_motion': 'SET_APPROPRIATE_BASE_POSE',
'arm_motion': 'SET_APPROPRIATE_ARM_POSE'
})
smach.StateMachine.add('SET_APPROPRIATE_BASE_POSE',
SetupMoveBaseWithDBC(),
transitions={
'pose_set': 'MOVE_BASE_WITH_DBC',
'tried_all': 'POPULATE_RESULT_WITH_OBJECTS'
})
smach.StateMachine.add(
'MOVE_BASE_WITH_DBC',
gbs.send_and_wait_events_combined(
event_in_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_start')],
event_out_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_success', True)],
timeout_duration=10),
transitions={
'success': 'MOVE_ARM',
'timeout': 'STOP_DBC',
'failure': 'STOP_DBC'
})
smach.StateMachine.add('SET_APPROPRIATE_ARM_POSE',
SetupMoveArm(),
transitions={
'pose_set': 'MOVE_ARM',
'tried_all': 'POPULATE_RESULT_WITH_OBJECTS'
})
# move arm to appropriate position
smach.StateMachine.add('MOVE_ARM',
gms.move_arm_and_gripper('open'),
transitions={
'succeeded': 'START_OBJECT_RECOGNITION',
'failed': 'MOVE_ARM'
})
# New perception pipeline state machine
smach.StateMachine.add(
'START_OBJECT_RECOGNITION',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mir_perception/multimodal_object_recognition/event_in',
'e_start')
],
event_out_list=[
('/mir_perception/multimodal_object_recognition/event_out',
'e_done', True)
],
timeout_duration=10),
transitions={
'success': 'STOP_RECOGNITION',
'timeout': 'OVERALL_FAILED',
'failure': 'OVERALL_FAILED'
})
smach.StateMachine.add(
'STOP_RECOGNITION',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mir_perception/multimodal_object_recognition/event_in',
'e_stop')
],
event_out_list=[
('/mir_perception/multimodal_object_recognition/event_out',
'e_stopped', True)
],
timeout_duration=5),
transitions={
'success': 'STOP_OBJECT_LIST_MERGER',
'timeout': 'STOP_OBJECT_LIST_MERGER',
'failure': 'STOP_OBJECT_LIST_MERGER'
})
smach.StateMachine.add(
'STOP_OBJECT_LIST_MERGER',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/object_list_merger/event_in',
'e_stop')],
event_out_list=[
('/mcr_perception/object_list_merger/event_out',
'e_stopped', True)
],
timeout_duration=5),
transitions={
'success': 'PUBLISH_MERGED_OBJECT_LIST',
'timeout': 'OVERALL_FAILED',
'failure': 'OVERALL_FAILED'
})
smach.StateMachine.add(
'PUBLISH_MERGED_OBJECT_LIST',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/object_list_merger/event_in',
'e_trigger')],
event_out_list=[
('/mcr_perception/object_list_merger/event_out', 'e_done',
True)
],
timeout_duration=5),
transitions={
'success': 'CHECK_IF_OBJECTS_FOUND',
'timeout': 'CHECK_IF_OBJECTS_FOUND',
'failure': 'CHECK_IF_OBJECTS_FOUND'
})
smach.StateMachine.add(
'CHECK_IF_OBJECTS_FOUND',
gbs.send_and_wait_events_combined(
event_in_list=[(
'/mcr_perception/object_list_merger/object_found_event_in',
'e_trigger')],
event_out_list=[(
'/mcr_perception/object_list_merger/object_found_event_out',
'e_objects_found', True)],
timeout_duration=10.0),
transitions={
'success': 'POPULATE_RESULT_WITH_OBJECTS',
'timeout': 'OVERALL_FAILED',
'failure': 'GET_MOTION_TYPE'
})
# populate action server result with perceived objects
smach.StateMachine.add('POPULATE_RESULT_WITH_OBJECTS',
PopulateResultWithObjects(),
transitions={'succeeded': 'OVERALL_SUCCESS'})
smach.StateMachine.add(
'STOP_DBC',
gbs.send_and_wait_events_combined(
event_in_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_stop')],
event_out_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_stopped', True)],
timeout_duration=10),
transitions={
'success': 'OVERALL_FAILED',
'timeout': 'OVERALL_FAILED',
'failure': 'OVERALL_FAILED'
})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = IntrospectionServer('perceive_location_smach_viewer', sm,
'/PERCEIVE_LOCATION_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='perceive_location_server',
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='goal',
feedback_key='feedback',
result_key='result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('perceive_location_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['perceive_location_goal'],
output_keys=['perceive_location_feedback', 'perceive_location_result'])
# Open the container
with sm:
# approach to platform
smach.StateMachine.add(
'GET_GOAL',
getGoal(),
transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME'})
#transitions={'succeeded':'START_OBJECT_LIST_MERGER'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
'PUBLISH_REFERENCE_FRAME',
gbs.send_event([('/static_transform_publisher_node/event_in',
'e_start')]),
transitions={'success': 'START_OBJECT_LIST_MERGER'})
smach.StateMachine.add(
'START_OBJECT_LIST_MERGER',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/object_list_merger/event_in',
'e_start')],
event_out_list=[
('/mcr_perception/object_list_merger/event_out',
'e_started', True)
],
timeout_duration=5),
transitions={
'success': 'LOOK_AT_WORKSPACE_LEFT',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# send arm to a position in which the depth camera can see the platformsmach.StateMachine.add('LOOK_AT_WORKSPACE_LEFT', gms.move_arm('look_at_workspace_LEFT'),
smach.StateMachine.add('LOOK_AT_WORKSPACE_LEFT',
gms.move_arm('look_at_workspace_left'),
transitions={
'succeeded': 'SUBSCRIBE_TO_POINT_CLOUD',
'failed': 'LOOK_AT_WORKSPACE_LEFT'
})
smach.StateMachine.add(
'SUBSCRIBE_TO_POINT_CLOUD',
gbs.send_event([('/mcr_perception/mux_pointcloud/select',
'/arm_cam3d/depth_registered/points')]),
transitions={'success': 'RECOGNIZE_OBJECTS_LEFT'})
# trigger perception pipeline
smach.StateMachine.add(
'RECOGNIZE_OBJECTS_LEFT',
gps.find_objects(retries=1),
transitions={
'objects_found': 'LOOK_AT_WORKSPACE_STRAIGHT',
'no_objects_found': 'LOOK_AT_WORKSPACE_STRAIGHT'
},
#transitions={'objects_found': 'SUBSCRIBE_TO_POINT_CLOUD_2',
# 'no_objects_found':'SUBSCRIBE_TO_POINT_CLOUD_2'},
remapping={'found_objects': 'recognized_objects'})
smach.StateMachine.add(
'SUBSCRIBE_TO_POINT_CLOUD_2',
gbs.send_event([('/mcr_perception/mux_pointcloud/select',
'/arm_cam3d/depth_registered/points')]),
transitions={'success': 'LOOK_AT_WORKSPACE_STRAIGHT'})
# send arm to a position in which the depth camera can see the platform
smach.StateMachine.add('LOOK_AT_WORKSPACE_STRAIGHT',
gms.move_arm('look_at_workspace'),
transitions={
'succeeded': 'RECOGNIZE_OBJECTS_STRAIGHT',
'failed': 'LOOK_AT_WORKSPACE_STRAIGHT'
})
# trigger perception pipeline
smach.StateMachine.add(
'RECOGNIZE_OBJECTS_STRAIGHT',
gps.find_objects(retries=1),
transitions={
'objects_found': 'LOOK_AT_WORKSPACE_RIGHT',
'no_objects_found': 'LOOK_AT_WORKSPACE_RIGHT'
},
#transitions={'objects_found': 'SUBSCRIBE_TO_POINT_CLOUD_3',
# 'no_objects_found':'SUBSCRIBE_TO_POINT_CLOUD_3'},
remapping={'found_objects': 'recognized_objects'})
smach.StateMachine.add(
'SUBSCRIBE_TO_POINT_CLOUD_3',
gbs.send_event([('/mcr_perception/mux_pointcloud/select',
'/arm_cam3d/depth_registered/points')]),
transitions={'success': 'LOOK_AT_WORKSPACE_RIGHT'})
# send arm to a position in which the depth camera can see the platform
smach.StateMachine.add('LOOK_AT_WORKSPACE_RIGHT',
gms.move_arm('look_at_workspace_right'),
transitions={
'succeeded': 'RECOGNIZE_OBJECTS_RIGHT',
'failed': 'LOOK_AT_WORKSPACE_RIGHT'
})
# trigger perception pipeline
smach.StateMachine.add(
'RECOGNIZE_OBJECTS_RIGHT',
gps.find_objects(retries=1),
transitions={
'objects_found': 'STOP_OBJECT_LIST_MERGER',
'no_objects_found': 'STOP_OBJECT_LIST_MERGER'
},
remapping={'found_objects': 'recognized_objects'})
smach.StateMachine.add(
'STOP_OBJECT_LIST_MERGER',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/object_list_merger/event_in',
'e_stop')],
event_out_list=[
('/mcr_perception/object_list_merger/event_out',
'e_stopped', True)
],
timeout_duration=5),
transitions={
'success': 'UNSUBSCRIBE_FROM_POINT_CLOUD',
'timeout': 'UNSUBSCRIBE_FROM_POINT_CLOUD',
'failure': 'UNSUBSCRIBE_FROM_POINT_CLOUD'
})
smach.StateMachine.add(
'UNSUBSCRIBE_FROM_POINT_CLOUD',
gbs.send_event([('/mcr_perception/mux_pointcloud/select',
'/emtpty_topic')]),
transitions={'success': 'PUBLISH_MERGED_OBJECT_LIST'})
smach.StateMachine.add(
'PUBLISH_MERGED_OBJECT_LIST',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/object_list_merger/event_in',
'e_trigger')],
event_out_list=[
('/mcr_perception/object_list_merger/event_out', 'e_done',
True)
],
timeout_duration=5),
transitions={
'success': 'SET_ACTION_LIB_SUCCESS',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('perceive_location_smach_viewer', sm,
'/PERCEIVE_LOCATION_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='perceive_location_server',
action_spec=PerceiveLocationAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='perceive_location_goal',
feedback_key='perceive_location_feedback',
result_key='perceive_location_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('compute_base_shift_server')
# Construct state machine
sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['compute_base_shift_goal'],
output_keys=[
'compute_base_shift_feedback',
'compute_base_shift_result'
])
sm.userdata.next_arm_pose_index = 0
sm.userdata.move_arm_to = None
with sm:
#add states to the container
# remove accumulated base poses and start non loop dependant components
smach.StateMachine.add(
'CLEAR_BASE_POSES',
gbs.send_event([
('/move_base_relative/pose_selector/event_in', 'e_forget'),
('/move_base_relative/random_obj_selector_transformer_republisher/event_in',
'e_start'),
('/move_base_relative/modified_pose_transformer/event_in',
'e_start')
]),
transitions={'success': 'SELECT_NEXT_LOOK_POSE'})
# select a look_at_workspace pose
smach.StateMachine.add(
'SELECT_NEXT_LOOK_POSE',
gms.select_arm_pose(['look_at_workspace']),
transitions={
'succeeded': 'LOOK_AROUND', # next pose selected
'completed': 'STOP_REMAINING_COMPONENTS_WITH_SUCCESS',
'failed': 'STOP_REMAINING_COMPONENTS_WITH_SUCCESS'
}
) # we've run out of poses to select, which means we've gone through the list
# move arm to selected pose
smach.StateMachine.add('LOOK_AROUND',
gms.move_arm(),
transitions={
'succeeded': 'RECOGNIZE_OBJECTS',
'failed': 'LOOK_AROUND'
})
smach.StateMachine.add(
'RECOGNIZE_OBJECTS',
gps.find_objects(retries=1),
transitions={
'objects_found': 'SELECT_OBJECT',
'no_objects_found': 'SELECT_NEXT_LOOK_POSE'
},
remapping={'found_objects': 'recognized_objects'})
# here the object selector is set up to select objects randomly
# it will publish object name (goes to pose selector) and object pose (goes to compute base shift pipeline)
# relative displacement calculator pose republisher in odom frame is also triggered here, before doing computations
smach.StateMachine.add(
'SELECT_OBJECT',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/random_object_selector/event_in',
'e_trigger')
],
event_out_list=[
('/mcr_perception/random_object_selector/event_out',
'e_selected', True)
],
timeout_duration=10),
transitions={
'success': 'SELECT_OBJECT',
'timeout': 'SELECT_NEXT_LOOK_POSE',
'failure': 'SELECT_NEXT_LOOK_POSE'
}
) # this means we're done computing poses for all recognized objects from this pose
# stop relative displacement calculator pose republisher and after that go to success
smach.StateMachine.add(
'STOP_REMAINING_COMPONENTS_WITH_SUCCESS',
gbs.send_event([
('/move_base_relative/random_obj_selector_transformer_republisher/event_in',
'e_stop'),
('/move_base_relative/modified_pose_transformer/event_in',
'e_stop')
]),
transitions={'success': 'SET_ACTION_LIB_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# Construct action server wrapper
asw = ActionServerWrapper(server_name='compute_base_shift_server',
action_spec=ComputeBaseShiftAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='compute_base_shift_goal',
feedback_key='compute_base_shift_feedback',
result_key='compute_base_shift_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("insert_cavity_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
# publish object as string to mcr_perception_selectors -> cavity,
# this component then publishes pose in base_link reference frame
smach.StateMachine.add(
"SELECT_OBJECT",
SelectObject("/mcr_perception/cavity_pose_selector/object_name"),
transitions={"succeeded": "CHECK_IF_OBJECT_IS_AVAILABLE"},
)
smach.StateMachine.add(
"CHECK_IF_OBJECT_IS_AVAILABLE",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mcr_perception/cavity_pose_selector/event_in",
"e_trigger",
)],
event_out_list=[(
"/mcr_perception/cavity_pose_selector/event_out",
"e_success",
True,
)],
timeout_duration=10,
),
transitions={
"success": "SET_DBC_PARAMS",
"timeout": "UNSTAGE_OBJECT",
"failure": "UNSTAGE_OBJECT",
},
)
smach.StateMachine.add(
"SET_DBC_PARAMS",
gbs.set_named_config("dbc_pick_object"),
transitions={
"success": "MOVE_ROBOT_AND_TRY_PLACING",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"MOVE_ROBOT_AND_TRY_PLACING",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_try")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "STOP_POSE_SELECTOR",
"timeout": "STOP_POSE_SELECTOR",
"failure": "STOP_POSE_SELECTOR",
},
)
smach.StateMachine.add(
"STOP_POSE_SELECTOR",
gbs.send_event([("/mcr_perception/cavity_pose_selector/event_in",
"e_stop")]),
transitions={"success": "PERCEIVE_CAVITY"},
)
# perceive cavity again after moving in front of the desired cavity
smach.StateMachine.add(
"PERCEIVE_CAVITY",
gas.perceive_cavity(),
transitions={
"success": "SELECT_OBJECT_AGAIN",
"failed": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"SELECT_OBJECT_AGAIN",
SelectObject("/mcr_perception/cavity_pose_selector/object_name"),
transitions={"succeeded": "CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN"},
)
smach.StateMachine.add(
"CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mcr_perception/cavity_pose_selector/event_in",
"e_trigger",
)],
event_out_list=[(
"/mcr_perception/cavity_pose_selector/event_out",
"e_success",
True,
)],
timeout_duration=10,
),
transitions={
"success": "UNSTAGE_OBJECT",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"UNSTAGE_OBJECT",
gas.unstage_object(),
transitions={
"success": "TRY_INSERTING",
"failed": "OVERALL_FAILED",
},
)
# execute robot motion
smach.StateMachine.add(
"TRY_INSERTING",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start_arm_only")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=20,
),
transitions={
"success": "OPEN_GRIPPER",
"timeout": "STAGE_OBJECT",
"failure": "STAGE_OBJECT",
},
)
smach.StateMachine.add(
"STAGE_OBJECT",
gas.stage_object(),
transitions={
"success": "OVERALL_FAILED",
"failed": "OVERALL_FAILED",
},
)
# close gripper
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "WIGGLE_ARM"},
)
# wiggling the arm for precision placement
smach.StateMachine.add(
"WIGGLE_ARM",
ppt_wiggle_arm(wiggle_offset=-0.12, joint=0),
transitions={
"succeeded": "MOVE_ARM_TO_HOLD",
"failed": "MOVE_ARM_TO_HOLD",
},
)
# move arm to HOLD position
smach.StateMachine.add(
"MOVE_ARM_TO_HOLD",
gms.move_arm("look_at_turntable"),
transitions={
"succeeded": "STOP_POSE_SELECTOR_FINAL",
"failed": "MOVE_ARM_TO_HOLD",
},
)
# sending e_stop to pose selector
smach.StateMachine.add(
"STOP_POSE_SELECTOR_FINAL",
gbs.send_event([("/mcr_perception/cavity_pose_selector/event_in",
"e_stop")]),
transitions={"success": "OVERALL_SUCCESS"},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer("insert_cavity_smach_viewer", sm,
"/INSERT_CAVITY_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="insert_cavity_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main(mokeup=False):
# Open the container
rospy.init_node('pick_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['pick_object_goal'],
output_keys=['pick_object_feedback', 'pick_object_result'])
with sm:
if not mokeup:
# publish object as string to mcr_perception_selectors -> object_selector, this component then publishes
# pose in base_link reference frame when e_trigger is sent (next state)
smach.StateMachine.add(
'SELECT_OBJECT',
select_object(
'/mcr_perception/object_selector/input/object_name'),
transitions={'success': 'OPEN_GRIPPER'})
# open gripper
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
'GENERATE_OBJECT_POSE',
send_event('/mcr_perception/object_selector/event_in',
'e_trigger'),
transitions={'success': 'CHECK_IF_OBJECT_IS_AVAILABLE'})
# waits for object selector response, if failure this means that the string published previously was not found in object list
smach.StateMachine.add(
'CHECK_IF_OBJECT_IS_AVAILABLE',
wait_for_event('/mcr_perception/object_selector/event_out',
1.0),
transitions={
'success': 'RECONFIGURE_PREGRASP_PARAMS',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# reconfigure pregrasp planner for pick
smach.StateMachine.add(
'RECONFIGURE_PREGRASP_PARAMS',
gbs.set_named_config('pick_normal_pregrasp'),
transitions={
'success': 'PLAN_ARM_MOTION',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'RECONFIGURE_PREGRASP_PARAMS'
})
# based on a published pose, calls pregrasp planner to generate a graspable pose and wait for the result of the pregrasp planner
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)],
timeout_duration=10),
transitions={
'success': 'STOP_PLAN_ARM_MOTION',
'timeout': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE',
'failure': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE'
})
# pregrasp planner was successful, so lets stop it since its work is done
smach.StateMachine.add('STOP_PLAN_ARM_MOTION',
send_event(
'/pregrasp_planner_pipeline/event_in',
'e_stop'),
transitions={'success': 'MOVE_ARM_TO_OBJECT'})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add(
'STOP_PLAN_ARM_MOTION_WITH_FAILURE',
send_event('/pregrasp_planner_pipeline/event_in', 'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
# move arm to pregrasp planned pose and wait for its response
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=7),
transitions={
'success': 'STOP_MOVE_ARM_TO_OBJECT',
'timeout': 'STOP_MOVE_ARM_TO_OBJECT',
'failure': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE'
})
# send stop event_in to arm motion component and return failure
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE',
send_event('/move_arm_planned/event_in', 'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
# send stop event_in to arm motion component
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT',
send_event('/move_arm_planned/event_in',
'e_stop'),
transitions={'success': 'CLOSE_GRIPPER'})
# close gripper
smach.StateMachine.add('CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# check if gripper changed in position, if not then close again
# 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':'MOVE_ARM_TO_HOLD',
# 'timeout':'CLOSE_GRIPPER',
# 'failure':'CLOSE_GRIPPER'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_TO_HOLD'
})
# optocoupler check if robot has a object in the gripper, if not then pick failed
# smach.StateMachine.add('VERIFY_OBJECT_GRASPED_OPTOCOUPLER', gbs.send_and_wait_events_combined(
# event_in_list=[('/optocoupler_gripper_status/event_in','e_trigger')],
# event_out_list=[('/optocoupler_gripper_status/event_out','e_grasped', True)],
# timeout_duration=5),
# transitions={'success':'SET_ACTION_LIB_SUCCESS',
# 'timeout':'SET_ACTION_LIB_FAILURE',
# 'failure':'SET_ACTION_LIB_FAILURE'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('pick_object_smach_viewer', sm,
'/PICK_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='pick_object_server',
action_spec=PickObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='pick_object_goal',
feedback_key='pick_object_feedback',
result_key='pick_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
# Open the container
rospy.init_node('insert_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['goal'],
output_keys = ['feedback', 'result'])
with sm:
smach.StateMachine.add('SELECT_OBJECT', SelectObject('/mcr_perception/object_selector/input/object_name'),
transitions={'succeeded':'GENERATE_OBJECT_POSE'})
# generates a pose of object
smach.StateMachine.add('GENERATE_OBJECT_POSE', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/object_selector/event_in','e_trigger')],
event_out_list=[('/mcr_perception/object_selector/event_out','e_selected', True)],
timeout_duration=10),
transitions={'success':'SET_DBC_PARAMS',
'timeout':'OVERALL_FAILED',
'failure':'OVERALL_FAILED'})
smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_pick_object'),
transitions={'success':'MOVE_ROBOT_AND_TRY_PLACING',
'timeout':'OVERALL_FAILED',
'failure':'OVERALL_FAILED'})
smach.StateMachine.add('MOVE_ROBOT_AND_TRY_PLACING', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in','e_try')],
event_out_list=[('/wbc/event_out','e_success', True)],
timeout_duration=50),
transitions={'success':'UNSTAGE_OBJECT',
'timeout':'OVERALL_FAILED',
'failure':'OVERALL_FAILED'})
smach.StateMachine.add('UNSTAGE_OBJECT', gas.unstage_object(),
transitions={'success': 'RE_GENERATE_OBJECT_POSE',
'failed' : 'OVERALL_FAILED'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('RE_GENERATE_OBJECT_POSE', gbs.send_event(
[('/mcr_perception/object_selector/event_in', 'e_re_trigger')]),
transitions={'success':'MOVE_ROBOT_AND_PLACE'})
# execute robot motion
smach.StateMachine.add('MOVE_ROBOT_AND_PLACE', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in','e_start')],
event_out_list=[('/wbc/event_out','e_success', True)],
timeout_duration=20),
transitions={'success':'OPEN_GRIPPER',
'timeout':'MOVE_ARM_TO_DEFAULT_PLACE',
'failure':'MOVE_ARM_TO_DEFAULT_PLACE'})
smach.StateMachine.add('MOVE_ARM_TO_DEFAULT_PLACE', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'MOVE_ARM_CARTESIAN',
'failed':'MOVE_ARM_TO_DEFAULT_PLACE'})
smach.StateMachine.add('MOVE_ARM_CARTESIAN', gbs.send_and_wait_events_combined(
event_in_list=[('/insert_object_workaroud/event_in','e_start')],
event_out_list=[('/insert_object_workaroud/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'OPEN_GRIPPER',
'timeout':'STOP_ARM_CARTESIAN_MOTION',
'failure':'OPEN_GRIPPER'})
smach.StateMachine.add('STOP_ARM_CARTESIAN_MOTION', gbs.send_event(
[('/insert_object_workaroud/event_in', 'e_stop')]),
transitions={'success':'OPEN_GRIPPER'})
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open_narrow'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'OVERALL_SUCCESS',
'failed':'MOVE_ARM_TO_HOLD'})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = smach_ros.IntrospectionServer('insert_object_smach_viewer', sm, '/INSERT_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'insert_object_server',
action_spec = GenericExecuteAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'goal',
feedback_key = 'feedback',
result_key = 'result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'failed', 'timeout'],
input_keys=[],
output_keys=[])
with self:
# Move robot
#smach.StateMachine.add('SHIFT_BASE_1', ShiftBase([0.0, 0.05, 0.0]),
#transitions={'succeeded': 'failed',
# 'timeout': 'failed',
# 'failed': 'failed'})
# start point could accumulator
smach.StateMachine.add(
'START_ACCUMULATOR',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/cloud_accumulator/event_in',
'e_start')
],
event_out_list=[
('/mcr_perception/cloud_accumulator/event_out',
'e_started', True)
],
timeout_duration=5),
transitions={
'success': 'ADD_POINT_COULD_1',
'timeout': 'timeout',
'failure': 'failed'
})
# add point could
smach.StateMachine.add(
'ADD_POINT_COULD_1',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/cloud_accumulator/event_in',
'e_add_cloud_start')
],
event_out_list=[
('/mcr_perception/cloud_accumulator/event_out',
'e_add_cloud_stopped', True)
],
timeout_duration=5),
transitions={
'success': 'SHIFT_BASE',
'timeout': 'ADD_POINT_COULD_1',
'failure': 'failed'
})
# Move robot
smach.StateMachine.add('SHIFT_BASE',
ShiftBase([0.0, 0.30, 0.0]),
transitions={
'succeeded': 'ADD_POINT_COULD_2',
'timeout': 'ADD_POINT_COULD_2',
'failed': 'ADD_POINT_COULD_2'
})
# add point could
smach.StateMachine.add(
'ADD_POINT_COULD_2',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/cloud_accumulator/event_in',
'e_add_cloud_start')
],
event_out_list=[
('/mcr_perception/cloud_accumulator/event_out',
'e_add_cloud_stopped', True)
],
timeout_duration=5),
transitions={
'success': 'SHIFT_BASE_BACK',
'timeout': 'ADD_POINT_COULD_2',
'failure': 'failed'
})
# Move robot
smach.StateMachine.add('SHIFT_BASE_BACK',
ShiftBase([0.0, -0.30, 0.0]),
transitions={
'succeeded': 'succeeded',
'timeout': 'succeeded',
'failed': 'succeeded'
})
def main():
# Open the container
rospy.init_node("pick_object_wbc_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
# read large object list
sm.userdata.large_objects = rospy.get_param("~large_objects", ["S40_40_B", "S40_40_G", "M30", "BEARING_BOX", "MOTOR"])
sm.userdata.reperceive = rospy.get_param("~reperceive", True)
with sm:
smach.StateMachine.add(
"SELECT_OBJECT",
SelectObject("/mcr_perception/object_selector/input/object_name"),
transitions={"succeeded": "GENERATE_OBJECT_POSE"},
)
# generates a pose of object
smach.StateMachine.add(
"GENERATE_OBJECT_POSE",
gbs.send_and_wait_events_combined(
event_in_list=[
("/mcr_perception/object_selector/event_in", "e_trigger")
],
event_out_list=[
("/mcr_perception/object_selector/event_out", "e_selected", True,)
],
timeout_duration=10,
),
transitions={
"success": "SET_DBC_PARAMS",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"SET_DBC_PARAMS",
gbs.set_named_config("dbc_pick_object"),
transitions={
"success": "MOVE_ROBOT_AND_TRY_PICKING",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
# whole body control command. It moves direct base controller and
# checks if an IK soln exists for the arm.
smach.StateMachine.add(
"MOVE_ROBOT_AND_TRY_PICKING",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_try")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "CHECK_IF_REPERCEIVE",
"timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
"failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"CHECK_IF_REPERCEIVE",
ShouldReperceive(),
transitions={
"yes": "OPEN_GRIPPER_FOR_REPERCEIVE",
"no": "GENERATE_OBJECT_POSE_AGAIN",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER_FOR_REPERCEIVE",
gms.control_gripper("open"),
transitions={"succeeded": "MOVE_ARM"},
)
# move arm to appropriate position
smach.StateMachine.add(
"MOVE_ARM",
gms.move_arm("look_at_workspace_from_near"),
transitions={
"succeeded": "START_OBJECT_LIST_MERGER",
"failed": "MOVE_ARM",
},
)
smach.StateMachine.add(
"START_OBJECT_LIST_MERGER",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_list_merger/event_in", "e_start")],
event_out_list=[("/mcr_perception/object_list_merger/event_out", "e_started", True)],
timeout_duration=5,
),
transitions={
"success": "WAIT_FOR_ARM_TO_STABILIZE",
"timeout": "TRY_PICKING",
"failure": "TRY_PICKING",
},
)
smach.StateMachine.add(
"WAIT_FOR_ARM_TO_STABILIZE",
mir_gbs.wait_for(0.5),
transitions={
"succeeded": "START_OBJECT_RECOGNITION",
},
)
# New perception pipeline state machine
smach.StateMachine.add(
"START_OBJECT_RECOGNITION",
gbs.send_and_wait_events_combined(
event_in_list=[("/mir_perception/multimodal_object_recognition/event_in", "e_start")],
event_out_list=[("/mir_perception/multimodal_object_recognition/event_out", "e_done", True)],
timeout_duration=10,
),
transitions={
"success": "STOP_RECOGNITION",
"timeout": "TRY_PICKING",
"failure": "TRY_PICKING",
},
)
smach.StateMachine.add(
"STOP_RECOGNITION",
gbs.send_and_wait_events_combined(
event_in_list=[("/mir_perception/multimodal_object_recognition/event_in", "e_stop")],
event_out_list=[("/mir_perception/multimodal_object_recognition/event_out", "e_stopped", True)],
timeout_duration=5,
),
transitions={
"success": "STOP_OBJECT_LIST_MERGER",
"timeout": "TRY_PICKING",
"failure": "TRY_PICKING",
},
)
smach.StateMachine.add(
"STOP_OBJECT_LIST_MERGER",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_list_merger/event_in", "e_stop")],
event_out_list=[("/mcr_perception/object_list_merger/event_out", "e_stopped", True)],
timeout_duration=5,
),
transitions={
"success": "PUBLISH_MERGED_OBJECT_LIST",
"timeout": "TRY_PICKING",
"failure": "TRY_PICKING",
},
)
smach.StateMachine.add(
"PUBLISH_MERGED_OBJECT_LIST",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_list_merger/event_in", "e_trigger_local")],
event_out_list=[("/mcr_perception/object_list_merger/event_out", "e_done", True)],
timeout_duration=5,
),
transitions={
"success": "SELECT_OBJECT_AGAIN",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"SELECT_OBJECT_AGAIN",
SelectObject("/mcr_perception/local_object_selector/input/object_name"),
transitions={"succeeded": "GENERATE_UPDATED_OBJECT_POSE"},
)
# generates a pose of object
smach.StateMachine.add(
"GENERATE_UPDATED_OBJECT_POSE",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/local_object_selector/event_in", "e_trigger")],
event_out_list=[("/mcr_perception/local_object_selector/event_out", "e_selected", True)],
timeout_duration=10,
),
transitions={
"success": "CHECK_IF_OBJECT_LARGE_LOCAL",
"timeout": "GENERATE_OBJECT_POSE_AGAIN",
"failure": "GENERATE_OBJECT_POSE_AGAIN",
},
)
# generates a pose of object
smach.StateMachine.add(
"GENERATE_OBJECT_POSE_AGAIN",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_selector/event_in", "e_re_trigger")],
event_out_list=[("/mcr_perception/object_selector/event_out", "e_selected", True)],
timeout_duration=10,
),
transitions={
"success": "CHECK_IF_OBJECT_LARGE",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"CHECK_IF_OBJECT_LARGE_LOCAL",
IsObjectLarge(),
transitions={
"large": "OPEN_GRIPPER_WIDE_LOCAL",
"small": "OPEN_GRIPPER_NARROW_LOCAL",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER_WIDE_LOCAL",
gms.control_gripper("open"),
transitions={"succeeded": "TRY_PICKING"},
)
smach.StateMachine.add(
"OPEN_GRIPPER_NARROW_LOCAL",
gms.control_gripper("open_narrow"),
transitions={"succeeded": "TRY_PICKING"},
)
# move only arm for wbc
smach.StateMachine.add(
"TRY_PICKING",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start_arm_only")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=20,
),
transitions={
"success": "CLOSE_GRIPPER",
"timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
"failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"CHECK_IF_OBJECT_LARGE",
IsObjectLarge(),
transitions={
"large": "OPEN_GRIPPER_WIDE",
"small": "OPEN_GRIPPER_NARROW",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER_WIDE",
gms.control_gripper("open"),
transitions={"succeeded": "MOVE_ROBOT_AND_PICK"},
)
smach.StateMachine.add(
"OPEN_GRIPPER_NARROW",
gms.control_gripper("open_narrow"),
transitions={"succeeded": "MOVE_ROBOT_AND_PICK"},
)
# whole body control command. It moves direct base controller and
# calls pre-grasp planner, and (optionally) moves arm to object pose
smach.StateMachine.add(
"MOVE_ROBOT_AND_PICK",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "CLOSE_GRIPPER",
"timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
"failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
gbs.send_event(
[
("/waypoint_trajectory_generation/event_in", "e_stop"),
("/wbc/event_in", "e_stop"),
]
),
transitions={"success": "OVERALL_FAILED"},
)
smach.StateMachine.add(
"CLOSE_GRIPPER",
gms.control_gripper("close"),
transitions={"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE"},
)
# move arm to stage_intemediate position
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "VERIFY_OBJECT_GRASPED",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE",
},
)
smach.StateMachine.add(
"VERIFY_OBJECT_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=5,
),
transitions={
"success": "OVERALL_SUCCESS",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
sm.register_transition_cb(transition_cb)
sm.register_start_cb(start_cb)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer(
"pick_object_smach_viewer", sm, "/PICK_OBJECT_SMACH_VIEWER"
)
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="wbc_pick_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main(mokeup=False):
# Open the container
rospy.init_node('pick_object_wbc_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['pick_object_wbc_goal'],
output_keys = ['pick_object_wbc_feedback', 'pick_object_wbc_result'])
with sm:
if not mokeup:
# open gripper
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'),
transitions={'succeeded': 'SETUP_MOVE_ARM'})
#add states to the container
smach.StateMachine.add('SETUP_MOVE_ARM', SetupMoveArm(),
transitions={'success': 'MOVE_ARM'})
smach.StateMachine.add('MOVE_ARM', gms.move_arm(),
transitions={'succeeded': 'WAIT_FOR_EVENT_FROM_USER',
'failed': 'MOVE_ARM'})
smach.StateMachine.add('WAIT_FOR_EVENT_FROM_USER', WaitForEventFromUser(),
transitions={'success' : 'PLAN_WHOLE_BODY_MOTION',
'failure' : 'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE'})
smach.StateMachine.add('PLAN_WHOLE_BODY_MOTION', send_event('/whole_body_motion_calculator_pipeline/event_in','e_start'),
transitions={'success':'WAIT_PLAN_WHOLE_BODY_MOTION'})
# wait for the result of the pregrasp planner
smach.StateMachine.add('WAIT_PLAN_WHOLE_BODY_MOTION', wait_for_event('/whole_body_motion_calculator_pipeline/event_out', 15.0),
transitions={'success':'STOP_PLAN_WHOLE_BODY_MOTION',
'timeout': 'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE',
'failure':'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE'})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add('STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE', send_event('/whole_body_motion_calculator_pipeline/event_in','e_stop'),
transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'}) # go to select pose input and plan arm motion
smach.StateMachine.add('STOP_PLAN_WHOLE_BODY_MOTION', send_event('/whole_body_motion_calculator_pipeline/event_in','e_stop'),
transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'}) # go to select pose input and plan arm motion
########################################## PREGRASP PIPELINE 2 ##################################################
# based on a published pose, calls pregrasp planner to generate a graspable pose
smach.StateMachine.add('SELECT_PREGRASP_PLANNER_INPUT_2', send_event('/pregrasp_planner_pipeline/event_in','e_start'),
transitions={'success':'WAIT_PLAN_ARM_MOTION_2'})
# wait for the result of the pregrasp planner
smach.StateMachine.add('WAIT_PLAN_ARM_MOTION_2', wait_for_event('/pregrasp_planner_pipeline/event_out', 15.0),
transitions={'success':'STOP_PLAN_ARM_MOTION_2',
'timeout': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
'failure':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'})
smach.StateMachine.add('STOP_PLAN_ARM_MOTION_2', send_event('/pregrasp_planner_pipeline/event_in','e_stop'),
transitions={'success':'MOVE_ARM_TO_OBJECT_2'})
# execute robot motion
smach.StateMachine.add('MOVE_ARM_TO_OBJECT_2', gbs.send_and_wait_events_combined(
event_in_list=[('/waypoint_trajectory_generation/event_in','e_start')],
event_out_list=[('/waypoint_trajectory_generation/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'STOP_MOVE_ARM_TO_OBJECT_2',
'timeout':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
'failure':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'})
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT_2', send_event('/waypoint_trajectory_generation/event_out','e_stop'),
transitions={'success':'SET_ACTION_LIB_SUCCESS'})
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2', send_event('/waypoint_trajectory_generation/event_out','e_stop'),
transitions={'success':'SET_ACTION_LIB_FAILURE'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True),
transitions={'succeeded':'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False),
transitions={'succeeded':'MOVE_ARM_TO_HOLD_FAILURE'})
smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILURE', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'OVERALL_FAILED',
'failed':'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('calibrate_pick_object_smach_viewer', sm, '/CALIBRATE_PICK_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'calibrate_pick_server',
action_spec = PickObjectWBCAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'pick_object_wbc_goal',
feedback_key = 'pick_object_wbc_feedback',
result_key = 'pick_object_wbc_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main(mokeup=False):
# Open the container
rospy.init_node('insert_object_in_cavity_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['insert_object_goal'],
output_keys=['insert_object_feedback', 'insert_object_result'])
with sm:
# publish object as string to mcr_perception_selectors -> cavity, this component then publishes
# pose in base_link reference frame
smach.StateMachine.add(
'SELECT_OBJECT',
select_object('/mcr_perception/cavity_pose_selector/object_name'),
transitions={'success': 'CHECK_IF_OBJECT_IS_AVAILABLE'})
smach.StateMachine.add(
'CHECK_IF_OBJECT_IS_AVAILABLE',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/cavity_pose_selector/event_in',
'e_trigger')
],
event_out_list=[
('/mcr_perception/cavity_pose_selector/event_out',
'e_success', True)
],
timeout_duration=10),
transitions={
'success': 'PLAN_WHOLE_BODY_MOTION',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
smach.StateMachine.add(
'PLAN_WHOLE_BODY_MOTION',
send_event('/whole_body_motion_calculator_pipeline/event_in',
'e_start'),
transitions={'success': 'WAIT_PLAN_WHOLE_BODY_MOTION'})
# wait for the result of the pregrasp planner
smach.StateMachine.add(
'WAIT_PLAN_WHOLE_BODY_MOTION',
wait_for_event('/whole_body_motion_calculator_pipeline/event_out',
15.0),
transitions={
'success': 'CALCULATE_BASE_MOTION',
'timeout': 'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE',
'failure': 'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE'
})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add(
'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE',
send_event('/whole_body_motion_calculator_pipeline/event_in',
'e_stop'),
transitions={'success': 'SELECT_PREGRASP_PLANNER_INPUT_2'
}) # go to select pose input and plan arm motion
smach.StateMachine.add('CALCULATE_BASE_MOTION',
gbs.send_and_wait_events_combined(
event_in_list=[
('/base_motion_calculator/event_in',
'e_start')
],
event_out_list=[
('/base_motion_calculator/event_out',
'e_success', True)
],
timeout_duration=5),
transitions={
'success': 'STOP_PLAN_WHOLE_BODY_MOTION',
'timeout': 'STOP_MOVE_BASE_TO_OBJECT',
'failure': 'STOP_MOVE_BASE_TO_OBJECT'
})
# wbc pipeline was successful, so lets stop it since its work is done
smach.StateMachine.add(
'STOP_PLAN_WHOLE_BODY_MOTION',
send_event('/whole_body_motion_calculator_pipeline/event_in',
'e_stop'),
transitions={'success': 'UNSTAGE_OBJECT'})
smach.StateMachine.add('UNSTAGE_OBJECT',
unstage(),
transitions={
'succeeded': 'MOVE_ROBOT_TO_OBJECT',
'failed': 'SET_ACTION_LIB_FAILURE'
})
# execute robot motion
smach.StateMachine.add(
'MOVE_ROBOT_TO_OBJECT',
gbs.send_and_wait_events_combined(
event_in_list=
[('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_start')],
event_out_list=
[('/wbc/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_success', True)],
timeout_duration=5),
transitions={
'success': 'STOP_MOVE_BASE_TO_OBJECT',
'timeout': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
'failure': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'
})
# send stop event_in to arm motion component
smach.StateMachine.add(
'STOP_MOVE_BASE_TO_OBJECT',
send_event(
'/wbc/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_stop'),
transitions={'success': 'SELECT_PREGRASP_PLANNER_INPUT_2'})
# send stop event_in to arm motion component and return failure
smach.StateMachine.add(
'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
send_event(
'/wbc/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_stop'),
transitions={'success': 'SELECT_PREGRASP_PLANNER_INPUT_2'})
########################################## PREGRASP PIPELINE 2 ##################################################
smach.StateMachine.add(
'SELECT_PREGRASP_PLANNER_INPUT_2',
gbs.send_event([
('/mir_manipulation/mux_pregrasp_pose/select',
'/mcr_perception/object_selector/output/object_pose')
]),
transitions={'success': 'SELECT_OBJECT_2'})
smach.StateMachine.add(
'SELECT_OBJECT_2',
select_object('/mcr_perception/cavity_pose_selector/object_name'),
transitions={'success': 'CHECK_IF_OBJECT_IS_AVAILABLE_2'})
smach.StateMachine.add(
'CHECK_IF_OBJECT_IS_AVAILABLE_2',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/cavity_pose_selector/event_in',
'e_trigger')
],
event_out_list=[
('/mcr_perception/cavity_pose_selector/event_out',
'e_success', True)
],
timeout_duration=10),
transitions={
'success': 'PLAN_ARM_MOTION_2',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
smach.StateMachine.add(
'PLAN_ARM_MOTION_2',
gbs.send_event([('/pregrasp_planner_pipeline/event_in', 'e_start')
]),
transitions={'success': 'WAIT_PLAN_ARM_MOTION_2'})
# wait for the result of the pregrasp planner
smach.StateMachine.add('WAIT_PLAN_ARM_MOTION_2',
wait_for_event(
'/pregrasp_planner_pipeline/event_out',
15.0),
transitions={
'success':
'STOP_PLAN_ARM_MOTION_2',
'timeout':
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
'failure':
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'
})
smach.StateMachine.add('STOP_PLAN_ARM_MOTION_2',
gbs.send_event([
('/pregrasp_planner_pipeline/event_in',
'e_stop')
]),
transitions={'success': 'MOVE_ARM_TO_OBJECT_2'})
# execute robot motion
smach.StateMachine.add(
'MOVE_ARM_TO_OBJECT_2',
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)],
event_in_list=[('/waypoint_trajectory_generation/event_in',
'e_start')],
event_out_list=[('/waypoint_trajectory_generation/event_out',
'e_success', True)],
timeout_duration=10),
transitions={
'success': 'STOP_MOVE_ARM_TO_OBJECT_2',
'timeout': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
'failure': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'
})
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT_2',
gbs.send_event([('/waypoint_trajectory_generation/event_out',
'e_stop')]),
transitions={'success': 'OPEN_GRIPPER'})
#smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECTH_FAILURE', send_event('/move_arm_planned/event_in','e_stop'),
################################ When failure place the object
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
gbs.send_event([('/waypoint_trajectory_generation/event_out',
'e_stop')]),
transitions={'success': 'MOVE_ARM_TO_DEFAULT_PLACE'})
smach.StateMachine.add('MOVE_ARM_TO_DEFAULT_PLACE',
gms.move_arm("10cm/pose4"),
transitions={
'succeeded': 'OPEN_GRIPPER_DEFAULT',
'failed': 'MOVE_ARM_TO_DEFAULT_PLACE'
})
smach.StateMachine.add('OPEN_GRIPPER_DEFAULT',
gms.control_gripper('open'),
transitions={'succeeded': 'MOVE_ARM_BACK'})
smach.StateMachine.add('MOVE_ARM_BACK',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_FAILURE',
'failed': 'SET_ACTION_LIB_FAILURE'
})
################################
# close gripper
smach.StateMachine.add('OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'WIGGLE_ARM_LEFT'})
###################################
# wiggling the arm for precision placement
smach.StateMachine.add('WIGGLE_ARM_LEFT',
ppt_wiggle_arm(wiggle_offset=-0.12, joint=0),
transitions={
'succeeded': 'WIGGLE_ARM_RIGHT',
'failed': 'WIGGLE_ARM_RIGHT'
})
smach.StateMachine.add('WIGGLE_ARM_RIGHT',
ppt_wiggle_arm(wiggle_offset=0.24, joint=0),
transitions={
'succeeded': 'WIGGLE_ARM_FORWARD',
'failed': 'WIGGLE_ARM_FORWARD'
})
# Now moving perpendicular
smach.StateMachine.add('WIGGLE_ARM_FORWARD',
ppt_wiggle_arm(wiggle_offset=-0.12, joint=3),
transitions={
'succeeded': 'WIGGLE_ARM_BACKWARD',
'failed': 'WIGGLE_ARM_BACKWARD'
})
smach.StateMachine.add('WIGGLE_ARM_BACKWARD',
ppt_wiggle_arm(wiggle_offset=0.24, joint=3),
transitions={
'succeeded': 'MOVE_ARM_TO_HOLD',
'failed': 'MOVE_ARM_TO_HOLD'
})
###################################
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_TO_HOLD'
})
# set action lib result
smach.StateMachine.add(
'SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'RESET_PREGRASP_PLANNER_INPUT_SUCCESS'})
smach.StateMachine.add(
'RESET_PREGRASP_PLANNER_INPUT_SUCCESS',
send_event('/mir_manipulation/mux_pregrasp_pose/select',
'/mcr_perception/object_selector/output/object_pose'),
transitions={'success': 'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add(
'SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD_FAILURE'})
smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILURE',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'OVERALL_FAILED',
'failed': 'MOVE_ARM_TO_HOLD_FAILURE'
})
# smach viewer
sis = smach_ros.IntrospectionServer(
'insert_object_smach_viewer', sm,
'/INSERT_OBJECT_IN_CAVITY_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='insert_object_in_cavity_server',
action_spec=InsertObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='insert_object_goal',
feedback_key='insert_object_feedback',
result_key='insert_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('insert_wheel_in_axis_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['insert_object_goal'],
output_keys=['insert_object_feedback', 'insert_object_result'])
with sm:
smach.StateMachine.add(
'PUBLISH_REFERENCE_FRAME',
gbs.send_event([('/static_transform_publisher_node/event_in',
'e_start')]),
transitions={'success': 'GET_AXIS_POSE'})
smach.StateMachine.add(
'GET_AXIS_POSE',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/car_wheel_axis_detector/input/event_in',
'e_trigger')
],
event_out_list=[(
'/mcr_perception/car_wheel_axis_detector/output/event_out',
'e_done', True)],
timeout_duration=10),
transitions={
'success': 'SHIFT_AXIS_POSE',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
smach.StateMachine.add('SHIFT_AXIS_POSE',
gbs.send_and_wait_events_combined(
event_in_list=[
('/wheel_axis_pose_shifter/event_in',
'e_start')
],
event_out_list=[
('/wheel_axis_pose_shifter/event_out',
'e_success', True)
],
timeout_duration=5),
transitions={
'success': 'PLAN_WHOLE_BODY_MOTION',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
smach.StateMachine.add(
'PLAN_WHOLE_BODY_MOTION',
send_event('/whole_body_motion_calculator_pipeline/event_in',
'e_start'),
transitions={'success': 'WAIT_PLAN_WHOLE_BODY_MOTION'})
# wait for the result of the pregrasp planner
smach.StateMachine.add(
'WAIT_PLAN_WHOLE_BODY_MOTION',
wait_for_event('/whole_body_motion_calculator_pipeline/event_out',
15.0),
transitions={
'success': 'CALCULATE_BASE_MOTION',
'timeout': 'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE',
'failure': 'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE'
})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add(
'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE',
send_event('/whole_body_motion_calculator_pipeline/event_in',
'e_stop'),
transitions={'success': 'PLAN_ARM_MOTION'
}) # go to select pose input and plan arm motion
smach.StateMachine.add('CALCULATE_BASE_MOTION',
gbs.send_and_wait_events_combined(
event_in_list=[
('/base_motion_calculator/event_in',
'e_start')
],
event_out_list=[
('/base_motion_calculator/event_out',
'e_success', True)
],
timeout_duration=5),
transitions={
'success': 'STOP_PLAN_WHOLE_BODY_MOTION',
'timeout': 'STOP_MOVE_BASE_TO_OBJECT',
'failure': 'STOP_MOVE_BASE_TO_OBJECT'
})
# wbc pipeline was successful, so lets stop it since its work is done
smach.StateMachine.add(
'STOP_PLAN_WHOLE_BODY_MOTION',
send_event('/whole_body_motion_calculator_pipeline/event_in',
'e_stop'),
transitions={'success': 'SETUP_MOVE_ARM_PRE_STAGE'})
############################################### unstage start ########################################
smach.StateMachine.add('SETUP_MOVE_ARM_PRE_STAGE',
SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE',
gms.move_arm(),
transitions={
'succeeded': 'OPEN_GRIPPER_TO_GRASP',
'failed': 'MOVE_ARM_PRE_STAGE'
})
smach.StateMachine.add(
'OPEN_GRIPPER_TO_GRASP',
gms.control_gripper('open'),
transitions={'succeeded': 'SETUP_MOVE_ARM_STAGE'})
smach.StateMachine.add('SETUP_MOVE_ARM_STAGE',
SetupMoveArm('final'),
transitions={'succeeded': 'MOVE_ARM_STAGE'})
smach.StateMachine.add('MOVE_ARM_STAGE',
gms.move_arm(),
transitions={
'succeeded': 'CLOSE_GRIPPER',
'failed': 'MOVE_ARM_STAGE'
})
smach.StateMachine.add(
'CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'SETUP_MOVE_ARM_PRE_STAGE_AGAIN'})
smach.StateMachine.add(
'SETUP_MOVE_ARM_PRE_STAGE_AGAIN',
SetupMoveArm('pre'),
transitions={'succeeded': 'MOVE_ARM_PRE_STAGE_AGAIN'})
smach.StateMachine.add('MOVE_ARM_PRE_STAGE_AGAIN',
gms.move_arm(),
transitions={
'succeeded': 'MOVE_ARM_TO_HOLD_1',
'failed': 'MOVE_ARM_PRE_STAGE_AGAIN'
})
smach.StateMachine.add('MOVE_ARM_TO_HOLD_1',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'MOVE_ROBOT_TO_OBJECT',
'failed': 'MOVE_ARM_TO_HOLD_1'
})
############################################### unstage end ########################################
# execute robot motion
smach.StateMachine.add(
'MOVE_ROBOT_TO_OBJECT',
gbs.send_and_wait_events_combined(
event_in_list=
[('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_start')],
event_out_list=
[('/wbc/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_success', True)],
timeout_duration=5),
transitions={
'success': 'STOP_MOVE_BASE_TO_OBJECT',
'timeout': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
'failure': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'
})
# send stop event_in to arm motion component
smach.StateMachine.add(
'STOP_MOVE_BASE_TO_OBJECT',
gbs.send_event([(
'/wbc/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_stop')]),
transitions={'success': 'PLAN_ARM_MOTION'})
# send stop event_in to arm motion component and return failure
smach.StateMachine.add(
'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
gbs.send_event([(
'/wbc/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_stop')]),
transitions={'success': 'PLAN_ARM_MOTION'})
########################################## PREGRASP PIPELINE 2 ##################################################
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)
],
timeout_duration=15),
transitions={
'success':
'STOP_PLAN_ARM_MOTION',
'timeout':
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
'failure':
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'
})
smach.StateMachine.add('STOP_PLAN_ARM_MOTION',
gbs.send_event([
('/pregrasp_planner_pipeline/event_in',
'e_stop')
]),
transitions={'success': 'MOVE_ARM_TO_OBJECT'})
# execute robot motion
smach.StateMachine.add(
'MOVE_ARM_TO_OBJECT',
gbs.send_and_wait_events_combined(
event_in_list=[('/waypoint_trajectory_generation/event_in',
'e_start')],
event_out_list=[('/waypoint_trajectory_generation/event_out',
'e_success', True)],
timeout_duration=10),
transitions={
'success': 'STOP_MOVE_ARM_TO_OBJECT_2',
'timeout': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
'failure': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'
})
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT_2',
gbs.send_event([('/waypoint_trajectory_generation/event_out',
'e_stop')]),
transitions={'success': 'STOP_LINEAR_COMPONENTS_1'})
#smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECTH_FAILURE', send_event('/move_arm_planned/event_in','e_stop'),
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
gbs.send_event([('/waypoint_trajectory_generation/event_out',
'e_stop')]),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
########################################## LINEAR MOTION PIPELINE ##################################################
smach.StateMachine.add('STOP_LINEAR_COMPONENTS_1',
gbs.send_event([
('/poses_to_move/event_in', 'e_stop'),
('/cartesian_controller_demo/event_in',
'e_stop')
]),
transitions={'success': 'PLAN_LINEAR_APPROACH'})
smach.StateMachine.add('PLAN_LINEAR_APPROACH',
gbs.send_and_wait_events_combined(
event_in_list=[('/poses_to_move/event_in',
'e_start')],
event_out_list=[('/poses_to_move/event_out',
'e_success', True)],
timeout_duration=10),
transitions={
'success': 'MOVE_ARM_LINEAR',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
smach.StateMachine.add('MOVE_ARM_LINEAR',
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_LINEAR_COMPONENTS_2',
'timeout': 'STOP_LINEAR_COMPONENTS_2',
'failure': 'STOP_LINEAR_COMPONENTS_2'
})
smach.StateMachine.add('STOP_LINEAR_COMPONENTS_2',
gbs.send_event([
('/poses_to_move/event_in', 'e_stop'),
('/cartesian_controller_demo/event_in',
'e_stop')
]),
transitions={'success': 'OPEN_GRIPPER'})
########################################## LINEAR MOTION PIPELINE END ##################################################
# close gripper
smach.StateMachine.add('OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_TO_HOLD'
})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add(
'SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD_FAILURE'})
smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILURE',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'OVERALL_FAILED',
'failed': 'MOVE_ARM_TO_HOLD_FAILURE'
})
# smach viewer
sis = smach_ros.IntrospectionServer('insert_wheel_smach_viewer', sm,
'/INSERT_WHEEL_IN_AXIS_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='insert_wheel_in_axis_server',
action_spec=InsertObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='insert_object_goal',
feedback_key='insert_object_feedback',
result_key='insert_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node("planning_coordinator")
problem_file = rospy.get_param("~problem_file", None)
domain_file = rospy.get_param("~domain_file", None)
planner = rospy.get_param("~planner", "mercury")
if problem_file is None or domain_file is None:
rospy.logfatal(
"Either domain and/or problem file not provided. Exiting.")
sys.exit(1)
# Create a SMACH state machine
sm = smach.StateMachine(
outcomes=["OVERALL_FAILURE", "OVERALL_PREEMPT", "OVERALL_SUCCESS"])
sm.userdata.already_once_executed = False
# counter for how many times did it enter check_execution_already_started()
sm.userdata.check_execution_counter = 0
sm.userdata.domain_file = domain_file
sm.userdata.problem_file = problem_file
sm.userdata.planner = planner
# Open the container
with sm:
# upload intrinsic knowledge
smach.StateMachine.add(
"UPLOAD_INTRINSIC_KNOWLEDGE",
gbs.send_event([("/upload_knowledge/event_in", "e_trigger")]),
transitions={"success": "LOOK_FOR_UNFINISHED_GOALS"},
)
# TODO:wait for upload intrinsic knowledge succes response
# send even in to look for goals component to process and see if there are unfinished goals on the knowledge base
smach.StateMachine.add(
"LOOK_FOR_UNFINISHED_GOALS",
gbs.send_and_wait_events_combined(
event_in_list=[
("/knowledge_base_analyzer/pending_goals/event_in",
"e_start")
],
event_out_list=[(
"/knowledge_base_analyzer/pending_goals/event_out",
"e_goals_available",
True,
)],
timeout_duration=5,
),
transitions={
"success": "GENERATE_PDDL_PROBLEM",
"timeout": "CHECK_EXECUTION_ALREADY_STARTED",
"failure": "CHECK_EXECUTION_ALREADY_STARTED",
},
)
smach.StateMachine.add(
"CHECK_EXECUTION_ALREADY_STARTED",
check_execution_already_started(max_count=3),
transitions={
"success": "MOVE_TO_EXIT",
"failure": "RE_ADD_GOALS"
},
)
smach.StateMachine.add(
"RE_ADD_GOALS",
re_add_goals(),
transitions={
"success": "LOOK_FOR_UNFINISHED_GOALS",
"failure": "LOOK_FOR_UNFINISHED_GOALS",
},
)
# generate problem from knowledge base snapshot
smach.StateMachine.add(
"GENERATE_PDDL_PROBLEM",
gbs.send_and_wait_events_combined(
event_in_list=[
("/mir_planning/pddl_problem_generator/event_in",
"e_trigger")
],
event_out_list=[(
"/mir_planning/pddl_problem_generator/event_out",
"e_success",
True,
)],
timeout_duration=5,
),
transitions={
"success": "PLAN",
"timeout": "GENERATE_PDDL_PROBLEM",
"failure": "OVERALL_FAILURE",
},
)
# plan task (mode can be either PlanGoal.NORMAL or PlanGoal.FAST)
smach.StateMachine.add(
"PLAN",
plan_task(mode=PlanGoal.NORMAL),
transitions={
"success": "EXECUTE_PLAN",
"failure": "LOOK_FOR_UNFINISHED_GOALS",
},
)
smach.StateMachine.add(
"EXECUTE_PLAN",
execute_plan(),
transitions={
"success": "LOOK_FOR_UNFINISHED_GOALS",
"failure": "LOOK_FOR_UNFINISHED_GOALS",
"preempted": "OVERALL_PREEMPT",
},
)
smach.StateMachine.add(
"MOVE_TO_EXIT",
gas.move_base("EXIT"),
transitions={
"success": "OVERALL_SUCCESS",
"failed": "MOVE_TO_EXIT"
},
)
# Create a thread to execute the smach container
smach_thread = threading.Thread(target=sm.execute)
smach_thread.start()
while not rospy.is_shutdown() and smach_thread.isAlive():
rospy.sleep(1.0)
if rospy.is_shutdown():
rospy.logwarn("ctrl + c detected!!! preempting smach execution")
# Request the container to preempt
sm.request_preempt()
smach_thread.join()
def main():
rospy.init_node('align_with_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['align_with_object_goal'],
output_keys = ['align_with_object_feedback', 'align_with_object_result'])
with sm:
#add states to the container
# publish name of object to which we want to align
smach.StateMachine.add('PUBLISH_OBJECT_NAME', select_object('/move_base_relative/pose_selector/selected_object_name'),
transitions={'success':'SELECT_POSE_FOR_OBJECT'})
# trigger pose selector. this will use the object name to output the corresponding pose (in map/odom frame)
smach.StateMachine.add('SELECT_POSE_FOR_OBJECT', gbs.send_and_wait_events_combined(
event_in_list=[('/move_base_relative/pose_selector/event_in','e_trigger')],
event_out_list=[('/move_base_relative/pose_selector/event_out','e_selected', True)],
timeout_duration=10),
transitions={'success':'START_DIRECT_BASE_CONTROLLER',
'timeout':'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'}) # object is not in the list
# call direct base controller
# Note: the pose out from pose selector needs to be mapped to input pose of direct base controller
# time out of 20 seconds is set
smach.StateMachine.add('START_DIRECT_BASE_CONTROLLER', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_navigation/direct_base_controller/coordinator/event_in','e_start')],
event_out_list=[('/mcr_navigation/direct_base_controller/coordinator/event_out','e_success', True), # if this happens it has succeeded
('/mcr_navigation/collision_velocity_filter/event_out', 'e_zero_velocities_forwarded', False)], # if this happens we stop
timeout_duration=20),
transitions={'success':'STOP_CONTROLLER_WITH_SUCCESS',
'timeout':'STOP_CONTROLLER_WITH_FAILURE',
'failure':'STOP_CONTROLLER_WITH_FAILURE'}) # this means we're done computing poses for all recognized objects from this pose
# stop controller with success
smach.StateMachine.add('STOP_CONTROLLER_WITH_SUCCESS', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop')],
event_out_list=[('/mcr_navigation/direct_base_controller/coordinator/event_out','e_stopped', True)],
timeout_duration=1),
transitions={'success':'SET_ACTION_LIB_SUCCESS',
'timeout':'SET_ACTION_LIB_SUCCESS',
'failure':'SET_ACTION_LIB_FAILURE'})
# stop controller with failure
smach.StateMachine.add('STOP_CONTROLLER_WITH_FAILURE', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop')],
event_out_list=[('/mcr_navigation/direct_base_controller/coordinator/event_out','e_stopped', True)],
timeout_duration=1),
transitions={'success':'SET_ACTION_LIB_FAILURE',
'timeout':'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True),
transitions={'succeeded':'OVERALL_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False),
transitions={'succeeded':'OVERALL_FAILED'})
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'align_with_object_server',
action_spec = AlignWithObjectAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'align_with_object_goal',
feedback_key = 'align_with_object_feedback',
result_key = 'align_with_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def __init__(self, base_shift=[0.0, 0.0, 0.0]):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'failed', 'timeout'],
input_keys=[],
output_keys=[])
with self:
# get pose from action lib as string, convert to pose stamped and publish
smach.StateMachine.add('SHIFT_BASE_COMMAND',
ShiftBaseCommand(base_shift),
transitions={'succeeded': 'MOVE_BASE'})
# send event_in to move base to a pose
smach.StateMachine.add(
'MOVE_BASE',
gbs.send_and_wait_events_combined(
event_in_list=[
('/mcr_perception/percieve_location_server/pose_transformer/event_in',
'e_start'),
],
event_out_list=
[('/mcr_perception/percieve_location_server/pose_transformer/event_out',
'e_success', True)],
timeout_duration=50),
transitions={
'success': 'ADJUST_BASE',
'timeout': 'timeout',
'failure': 'failed'
})
# call direct base controller to fine adjust the base to the final desired pose
# (navigation tolerance is set to a wide tolerance)
smach.StateMachine.add(
'ADJUST_BASE',
gbs.send_and_wait_events_combined(
event_in_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_start')],
event_out_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_success', True)],
timeout_duration=5
), # this is a tradeoff between speed and accuracy, set a higher value for accuracy increase
transitions={
'success': 'STOP_CONTROLLER_WITH_SUCCESS',
'timeout': 'STOP_CONTROLLER_WITH_SUCCESS',
'failure': 'STOP_CONTROLLER_WITH_FAILURE'
})
# stop controller with success
smach.StateMachine.add(
'STOP_CONTROLLER_WITH_SUCCESS',
gbs.send_and_wait_events_combined(
event_in_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_stop')],
event_out_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_stopped', True)],
timeout_duration=1),
transitions={
'success': 'succeeded',
'timeout': 'succeeded',
'failure': 'failed'
})
# stop controller with failure
smach.StateMachine.add(
'STOP_CONTROLLER_WITH_FAILURE',
gbs.send_and_wait_events_combined(
event_in_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_stop')],
event_out_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_stopped', True)],
timeout_duration=1),
transitions={
'success': 'failed',
'timeout': 'failed',
'failure': 'failed'
})
def main():
rospy.init_node('planning_coordinator')
problem_file = rospy.get_param('~problem_file', None)
domain_file = rospy.get_param('~domain_file', None)
planner = rospy.get_param('~planner', 'mercury')
if problem_file is None or domain_file is None:
rospy.logfatal(
'Either domain and/or problem file not provided. Exiting.')
sys.exit(1)
# Create a SMACH state machine
sm = smach.StateMachine(
outcomes=['OVERALL_FAILURE', 'OVERALL_PREEMPT', 'OVERALL_SUCCESS'])
sm.userdata.already_once_executed = False
# counter for how many times did it enter check_execution_already_started()
sm.userdata.check_execution_counter = 0
sm.userdata.domain_file = domain_file
sm.userdata.problem_file = problem_file
sm.userdata.planner = planner
# Open the container
with sm:
# upload intrinsic knowledge
smach.StateMachine.add(
'UPLOAD_INTRINSIC_KNOWLEDGE',
gbs.send_event([('/upload_knowledge/event_in', 'e_trigger')]),
transitions={'success': 'LOOK_FOR_UNFINISHED_GOALS'})
# TODO:wait for upload intrinsic knowledge succes response
# send even in to look for goals component to process and see if there are unfinished goals on the knowledge base
smach.StateMachine.add(
'LOOK_FOR_UNFINISHED_GOALS',
gbs.send_and_wait_events_combined(
event_in_list=[
('/knowledge_base_analyzer/pending_goals/event_in',
'e_start')
],
event_out_list=[
('/knowledge_base_analyzer/pending_goals/event_out',
'e_goals_available', True)
],
timeout_duration=5),
transitions={
'success': 'GENERATE_PDDL_PROBLEM',
'timeout': 'CHECK_EXECUTION_ALREADY_STARTED',
'failure': 'CHECK_EXECUTION_ALREADY_STARTED'
})
smach.StateMachine.add('CHECK_EXECUTION_ALREADY_STARTED',
check_execution_already_started(max_count=3),
transitions={
'success': 'MOVE_TO_EXIT',
'failure': 'RE_ADD_GOALS'
})
smach.StateMachine.add('RE_ADD_GOALS',
re_add_goals(),
transitions={
'success': 'LOOK_FOR_UNFINISHED_GOALS',
'failure': 'LOOK_FOR_UNFINISHED_GOALS'
})
# generate problem from knowledge base snapshot
smach.StateMachine.add(
'GENERATE_PDDL_PROBLEM',
gbs.send_and_wait_events_combined(
event_in_list=[
('/task_planning/pddl_problem_generator_node/event_in',
'e_trigger')
],
event_out_list=[
('/task_planning/pddl_problem_generator_node/event_out',
'e_success', True)
],
timeout_duration=5),
transitions={
'success': 'PLAN',
'timeout': 'GENERATE_PDDL_PROBLEM',
'failure': 'OVERALL_FAILURE'
})
# plan task (mode can be either PlanGoal.NORMAL or PlanGoal.FAST)
smach.StateMachine.add('PLAN',
plan_task(mode=PlanGoal.NORMAL),
transitions={
'success': 'EXECUTE_PLAN',
'failure': 'LOOK_FOR_UNFINISHED_GOALS'
})
smach.StateMachine.add('EXECUTE_PLAN',
execute_plan(),
transitions={
'success': 'LOOK_FOR_UNFINISHED_GOALS',
'failure': 'LOOK_FOR_UNFINISHED_GOALS',
'preempted': 'OVERALL_PREEMPT'
})
smach.StateMachine.add('MOVE_TO_EXIT',
gas.move_base('EXIT'),
transitions={
'success': 'OVERALL_SUCCESS',
'failed': 'MOVE_TO_EXIT'
})
# Create a thread to execute the smach container
smach_thread = threading.Thread(target=sm.execute)
smach_thread.start()
while not rospy.is_shutdown() and smach_thread.isAlive():
rospy.sleep(1.0)
if rospy.is_shutdown():
rospy.logwarn("ctrl + c detected!!! preempting smach execution")
# Request the container to preempt
sm.request_preempt()
smach_thread.join()
def main():
rospy.init_node("perceive_location_server")
sleep_time = rospy.get_param("~sleep_time", 1.0)
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
# Open the container
sm.userdata.arm_pose_list = [
"look_at_workspace_from_near",
"look_at_workspace_from_near_left",
"look_at_workspace_from_near_right",
]
sm.userdata.arm_pose_index = 0
base_x_offset = rospy.get_param("~base_x_offset", 0.0)
base_y_offset = rospy.get_param("~base_y_offset", 0.25)
base_theta_offset = rospy.get_param("~base_theta_offset", 0.0)
sm.userdata.base_pose_list = [
{
"x": 0.0,
"y": 0.0,
"theta": 0.0
},
{
"x": base_x_offset,
"y": base_y_offset,
"theta": base_theta_offset
},
{
"x": base_x_offset,
"y": -base_y_offset,
"theta": -base_theta_offset
},
]
sm.userdata.base_pose_index = 0
with sm:
# approach to platform
smach.StateMachine.add(
"SETUP",
Setup(),
transitions={"succeeded": "PUBLISH_REFERENCE_FRAME"},
)
# publish a static frame which will be used as reference for perceived objs
smach.StateMachine.add(
"PUBLISH_REFERENCE_FRAME",
gbs.send_event([("/static_transform_publisher_node/event_in",
"e_start")]),
transitions={"success": "SET_DBC_PARAMS"},
)
# publish a static frame which will be used as reference for perceived objs
smach.StateMachine.add(
"SET_DBC_PARAMS",
gbs.set_named_config("dbc_pick_object"),
transitions={
"success": "START_OBJECT_LIST_MERGER",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"START_OBJECT_LIST_MERGER",
gbs.send_and_wait_events_combined(
event_in_list=[
("/mcr_perception/object_list_merger/event_in", "e_start"),
("/mcr_perception/object_selector/event_in", "e_start"),
],
event_out_list=[(
"/mcr_perception/object_list_merger/event_out",
"e_started",
True,
)],
timeout_duration=5,
),
transitions={
"success": "GET_MOTION_TYPE",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"GET_MOTION_TYPE",
GetMotionType(),
transitions={
"base_motion": "SET_APPROPRIATE_BASE_POSE",
"arm_motion": "SET_APPROPRIATE_ARM_POSE",
},
)
smach.StateMachine.add(
"SET_APPROPRIATE_BASE_POSE",
SetupMoveBaseWithDBC(),
transitions={
"pose_set": "MOVE_BASE_WITH_DBC",
"tried_all": "POPULATE_RESULT_WITH_OBJECTS",
},
)
smach.StateMachine.add(
"MOVE_BASE_WITH_DBC",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mcr_navigation/direct_base_controller/coordinator/event_in",
"e_start",
)],
event_out_list=[(
"/mcr_navigation/direct_base_controller/coordinator/event_out",
"e_success",
True,
)],
timeout_duration=10,
),
transitions={
"success": "MOVE_ARM",
"timeout": "STOP_DBC",
"failure": "STOP_DBC",
},
)
smach.StateMachine.add(
"SET_APPROPRIATE_ARM_POSE",
SetupMoveArm(),
transitions={
"pose_set": "MOVE_ARM",
"tried_all": "STOP_OBJECT_LIST_MERGER",
},
)
# move arm to appropriate position
smach.StateMachine.add(
"MOVE_ARM",
gms.move_arm_and_gripper("open"),
transitions={
"succeeded": "WAIT_FOR_ARM_TO_STABILIZE",
"failed": "MOVE_ARM",
},
)
# move arm to appropriate position
smach.StateMachine.add(
"WAIT_FOR_ARM_TO_STABILIZE",
mir_gbs.wait_for(0.5),
transitions={
"succeeded": "START_OBJECT_RECOGNITION",
},
)
# New perception pipeline state machine
smach.StateMachine.add(
"START_OBJECT_RECOGNITION",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mir_perception/multimodal_object_recognition/event_in",
"e_start",
)],
event_out_list=[(
"/mir_perception/multimodal_object_recognition/event_out",
"e_done",
True,
)],
timeout_duration=10,
),
transitions={
"success": "STOP_RECOGNITION",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"STOP_RECOGNITION",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mir_perception/multimodal_object_recognition/event_in",
"e_stop",
)],
event_out_list=[(
"/mir_perception/multimodal_object_recognition/event_out",
"e_stopped",
True,
)],
timeout_duration=5,
),
transitions={
"success": "GET_MOTION_TYPE",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"STOP_OBJECT_LIST_MERGER",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_list_merger/event_in",
"e_stop")],
event_out_list=[(
"/mcr_perception/object_list_merger/event_out",
"e_stopped",
True,
)],
timeout_duration=5,
),
transitions={
"success": "PUBLISH_MERGED_OBJECT_LIST",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"PUBLISH_MERGED_OBJECT_LIST",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mcr_perception/object_list_merger/event_in",
"e_trigger",
)],
event_out_list=[(
"/mcr_perception/object_list_merger/event_out",
"e_done",
True,
)],
timeout_duration=5,
),
transitions={
"success": "CHECK_IF_OBJECTS_FOUND",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"CHECK_IF_OBJECTS_FOUND",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mcr_perception/object_list_merger/object_found_event_in",
"e_trigger",
)],
event_out_list=[(
"/mcr_perception/object_list_merger/object_found_event_out",
"e_objects_found",
True,
)],
timeout_duration=5.0,
),
transitions={
"success": "POPULATE_RESULT_WITH_OBJECTS",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
# populate action server result with perceived objects
smach.StateMachine.add(
"POPULATE_RESULT_WITH_OBJECTS",
PopulateResultWithObjects(),
transitions={"succeeded": "OVERALL_SUCCESS"},
)
smach.StateMachine.add(
"STOP_DBC",
gbs.send_and_wait_events_combined(
event_in_list=[(
"/mcr_navigation/direct_base_controller/coordinator/event_in",
"e_stop",
)],
event_out_list=[(
"/mcr_navigation/direct_base_controller/coordinator/event_out",
"e_stopped",
True,
)],
timeout_duration=10,
),
transitions={
"success": "OVERALL_FAILED",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer(
"perceive_location_smach_viewer",
sm,
"/PERCEIVE_LOCATION_SMACH_VIEWER",
)
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="perceive_location_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('move_base_safe_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'OVERALL_PREEMPTED'],
input_keys=['move_base_safe_goal'],
output_keys=['move_base_safe_feedback', 'move_base_safe_result'])
with sm:
#add states to the container
# smach.StateMachine.add('USE_FAR_RANGE_CAMERA_FOR_BARRIER_TAPE', gbs.set_named_config('far_range_camera_barrier_tape'),
# transitions={'success':'SUBSCRIBE_TO_POINT_CLOUD_BARRIER',
# 'failure':'SUBSCRIBE_TO_POINT_CLOUD_BARRIER',
# 'timeout':'SUBSCRIBE_TO_POINT_CLOUD_BARRIER'})
#
# smach.StateMachine.add('SUBSCRIBE_TO_POINT_CLOUD_BARRIER', gbs.send_event([('/mcr_perception/mux_pointcloud_barrier/select', '/arm_cam3d/depth_registered/points')]),
# transitions={'success':'SETUP_MOVE_ARM'})
smach.StateMachine.add('CHECK_DONT_BE_SAFE',
CheckDontBeSafe(),
transitions={
'safe': 'START_BARRIER_TAPE_DETECTION',
'unsafe': 'SETUP_MOVE_BASE'
})
#smach.StateMachine.add('START_BARRIER_TAPE_DETECTION', gbs.send_event([('/mcr_perception/barrier_tape_detection/fake_event', 'e_trigger')]),
smach.StateMachine.add(
'START_BARRIER_TAPE_DETECTION',
gbs.send_event([('/mcr_perception/barrier_tape_detection/event_in',
'e_trigger')]),
transitions={'success': 'SETUP_MOVE_ARM'})
smach.StateMachine.add('SETUP_MOVE_ARM',
SetupMoveArm(),
transitions={
'succeeded': 'MOVE_ARM',
'failed': 'SETUP_MOVE_ARM'
})
'''
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open_wide'),
transitions={'succeeded': 'MOVE_ARM'})
smach.StateMachine.add('MOVE_ARM', gms.move_arm(),
transitions={'succeeded': 'SETUP_MOVE_BASE',
'failed': 'MOVE_ARM'})
'''
smach.StateMachine.add('MOVE_ARM',
gms.move_arm_and_gripper('open_wide'),
transitions={
'succeeded': 'SETUP_MOVE_BASE',
'failed': 'MOVE_ARM'
})
# get pose from action lib as string, convert to pose stamped and publish
smach.StateMachine.add('SETUP_MOVE_BASE',
SetupMoveBase('/move_base_wrapper/pose_in'),
transitions={
'succeeded': 'SET_DBC_PARAMS',
'failed':
'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE',
'preempted': 'OVERALL_PREEMPTED'
})
smach.StateMachine.add('SET_DBC_PARAMS',
gbs.set_named_config('dbc_move_base'),
transitions={
'success': 'MOVE_BASE',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# send event_in to move base to a pose
# add events to mir_look at and mcr_converter
smach.StateMachine.add(
'MOVE_BASE',
gbs.
send_and_wait_events_combined(event_in_list=[
('/move_base_wrapper/event_in', 'e_start'),
('/mcr_common/twist_to_motion_direction_conversion/event_in',
'e_start'),
('/mir_manipulation/look_at_point/event_in', 'e_start')
],
event_out_list=[
('/move_base_wrapper/event_out',
'e_success', True)
],
timeout_duration=50),
transitions={
'success': 'ADJUST_BASE',
'timeout': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE',
'failure': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE'
})
# call direct base controller to fine adjust the base to the final desired pose
# (navigation tolerance is set to a wide tolerance)
smach.StateMachine.add(
'ADJUST_BASE',
gbs.send_and_wait_events_combined(
event_in_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_start'),
('/mir_manipulation/look_at_point/event_in', 'e_stop')],
event_out_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_success', True)],
timeout_duration=5
), # this is a tradeoff between speed and accuracy, set a higher value for accuracy increase
transitions={
'success': 'STOP_CONTROLLER_WITH_SUCCESS',
'timeout': 'STOP_CONTROLLER_WITH_SUCCESS',
'failure': 'STOP_CONTROLLER_WITH_FAILURE'
})
# stop controller with success
smach.StateMachine.add(
'STOP_CONTROLLER_WITH_SUCCESS',
gbs.send_and_wait_events_combined(
event_in_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_stop')],
event_out_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_stopped', True)],
timeout_duration=1),
transitions={
'success': 'STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS',
'timeout': 'STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS',
'failure': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE'
})
# stop controller with failure
smach.StateMachine.add(
'STOP_CONTROLLER_WITH_FAILURE',
gbs.send_and_wait_events_combined(
event_in_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_stop')],
event_out_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_stopped', True)],
timeout_duration=1),
transitions={
'success': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE',
'timeout': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE',
'failure': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE'
})
smach.StateMachine.add(
'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE',
gbs.send_event([('/mcr_perception/barrier_tape_detection/event_in',
'e_stop')]),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add(
'STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS',
gbs.send_event([('/mcr_perception/barrier_tape_detection/event_in',
'e_stop')]),
transitions={'success': 'ALIGN_WITH_WORKSPACE'})
# smach.StateMachine.add('SET_ACTION_LIB_FAILURE_RESET_CAMERA', gbs.set_named_config('close_range_camera_perception'),
# transitions={'success':'UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_FAILURE',
# 'failure':'UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_FAILURE',
# 'timeout':'UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_FAILURE'})
#
# smach.StateMachine.add('SET_ACTION_LIB_SUCCESS_RESET_CAMERA', gbs.set_named_config('close_range_camera_perception'),
# transitions={'success':'UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_SUCCESS',
# 'failure':'UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_SUCCESS',
# 'timeout':'UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_SUCCESS'})
#
# smach.StateMachine.add('UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_SUCCESS', gbs.send_event([('/mcr_perception/mux_pointcloud_barrier/select', '/empty_topic')]),
# transitions={'success':'SET_ACTION_LIB_SUCCESS'})
#
# smach.StateMachine.add('UNSUBSCRIBE_FROM_POINT_CLOUD_BARRIER_FAILURE', gbs.send_event([('/mcr_perception/mux_pointcloud_barrier/select', '/empty_topic')]),
# transitions={'success':'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add(
'ALIGN_WITH_WORKSPACE',
AlignWithWorkspace(),
#transitions={'succeeded':'RESET_STATIC_TRANSFORM_FOR_PERCEPTION'})
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'SET_ACTION_LIB_SUCCESS'
})
smach.StateMachine.add(
'SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
#transitions={'succeeded':'RESET_STATIC_TRANSFORM_FOR_PERCEPTION'})
transitions={'succeeded': 'OVERALL_SUCCESS'})
# generates a pose based on the previous string object topic received
#smach.StateMachine.add('RESET_STATIC_TRANSFORM_FOR_PERCEPTION', gbs.send_event([('/static_transform_publisher_node/event_in', 'e_start')]),
# transitions={'success':'OVERALL_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'OVERALL_FAILED'})
# Construct action server wrapper
asw = ActionServerWrapper(server_name='move_base_safe_server',
action_spec=MoveBaseSafeAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['OVERALL_PREEMPTED'],
goal_key='move_base_safe_goal',
feedback_key='move_base_safe_feedback',
result_key='move_base_safe_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
# Open the container
rospy.init_node('pick_object_wbc_server')
# Construct state machine
sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['goal'],
output_keys=['feedback', 'result'])
with sm:
smach.StateMachine.add(
'SELECT_OBJECT',
SelectObject('/mcr_perception/object_selector/input/object_name'),
transitions={'succeeded': 'GENERATE_OBJECT_POSE'})
# generates a pose of object
smach.StateMachine.add(
'GENERATE_OBJECT_POSE',
gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/object_selector/event_in',
'e_trigger')],
event_out_list=[('/mcr_perception/object_selector/event_out',
'e_selected', True)],
timeout_duration=10),
transitions={
'success': 'OPEN_GRIPPER',
'timeout': 'OVERALL_FAILED',
'failure': 'OVERALL_FAILED'
})
smach.StateMachine.add('OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'SET_DBC_PARAMS'})
smach.StateMachine.add('SET_DBC_PARAMS',
gbs.set_named_config('dbc_pick_object'),
transitions={
'success': 'MOVE_ROBOT_AND_PICK',
'timeout': 'OVERALL_FAILED',
'failure': 'OVERALL_FAILED'
})
# whole body control command. It moves direct base controller and
# calls pre-grasp planner, and (optionally) moves arm to object pose
smach.StateMachine.add('MOVE_ROBOT_AND_PICK',
gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in', 'e_start')
],
event_out_list=[('/wbc/event_out',
'e_success', True)],
timeout_duration=50),
transitions={
'success':
'CLOSE_GRIPPER',
'timeout':
'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
'failure':
'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'
})
smach.StateMachine.add('STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
gbs.send_event([
('/waypoint_trajectory_generation/event_in',
'e_start'), ('/wbc/event_in', 'e_stop')
]),
transitions={'success': 'OVERALL_FAILED'})
smach.StateMachine.add(
'CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'MOVE_ARM_TO_STAGE_INTERMEDIATE'})
# move arm to stage_intemediate position
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE',
gms.move_arm('stage_intermediate'),
transitions={
'succeeded': 'VERIFY_OBJECT_GRASPED',
'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE'
})
smach.StateMachine.add(
'VERIFY_OBJECT_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=5),
transitions={
'success': 'OVERALL_SUCCESS',
'timeout': 'OVERALL_FAILED',
'failure': 'OVERALL_FAILED'
})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = IntrospectionServer('pick_object_smach_viewer', sm,
'/PICK_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='wbc_pick_object_server',
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='goal',
feedback_key='feedback',
result_key='result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main(mokeup=False):
# Open the container
rospy.init_node('insert_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['insert_object_goal'],
output_keys = ['insert_object_feedback', 'insert_object_result'])
with sm:
if not mokeup:
smach.StateMachine.add('SELECT_OBJECT', select_object('/mcr_perception/object_selector/input/object_name'),
transitions={'success':'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('GENERATE_OBJECT_POSE', send_event('/mcr_perception/object_selector/event_in', 'e_trigger'),
transitions={'success':'CHECK_IF_OBJECT_IS_AVAILABLE'})
# waits for object selector response, if failure this means that the string published previously was not found in object list
smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', wait_for_event('/mcr_perception/object_selector/event_out', 1.0),
transitions={'success':'PLAN_WHOLE_BODY_MOTION',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add('PLAN_WHOLE_BODY_MOTION', send_event('/whole_body_motion_calculator_pipeline/event_in','e_start'),
transitions={'success':'WAIT_PLAN_WHOLE_BODY_MOTION'})
# wait for the result of the pregrasp planner
smach.StateMachine.add('WAIT_PLAN_WHOLE_BODY_MOTION', wait_for_event('/whole_body_motion_calculator_pipeline/event_out', 15.0),
transitions={'success':'CALCULATE_BASE_MOTION',
'timeout': 'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE',
'failure':'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE'})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add('STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE', send_event('/whole_body_motion_calculator_pipeline/event_in','e_stop'),
transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'}) # go to select pose input and plan arm motion
smach.StateMachine.add('CALCULATE_BASE_MOTION', gbs.send_and_wait_events_combined(
event_in_list=[('/base_motion_calculator/event_in','e_start')],
event_out_list=[('/base_motion_calculator/event_out','e_success', True)],
timeout_duration=5),
transitions={'success':'STOP_PLAN_WHOLE_BODY_MOTION',
'timeout':'STOP_MOVE_BASE_TO_OBJECT',
'failure':'STOP_MOVE_BASE_TO_OBJECT'})
# wbc pipeline was successful, so lets stop it since its work is done
smach.StateMachine.add('STOP_PLAN_WHOLE_BODY_MOTION', send_event('/whole_body_motion_calculator_pipeline/event_in','e_stop'),
transitions={'success':'UNSTAGE_OBJECT'})
smach.StateMachine.add('UNSTAGE_OBJECT', unstage(),
transitions={'succeeded': 'MOVE_ROBOT_TO_OBJECT',
'failed' : 'SET_ACTION_LIB_FAILURE'})
# execute robot motion
smach.StateMachine.add('MOVE_ROBOT_TO_OBJECT', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in','e_start')],
event_out_list=[('/wbc/mcr_navigation/direct_base_controller/coordinator/event_out','e_success', True)],
timeout_duration=5),
transitions={'success':'STOP_MOVE_BASE_TO_OBJECT',
'timeout':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
'failure':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'})
# send stop event_in to arm motion component
smach.StateMachine.add('STOP_MOVE_BASE_TO_OBJECT',
send_event('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop'),
transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'})
# send stop event_in to arm motion component and return failure
smach.StateMachine.add('STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
send_event('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop'),
transitions={'success':'SELECT_PREGRASP_PLANNER_INPUT_2'})
########################################## PREGRASP PIPELINE 2 ##################################################
# based on a published pose, calls pregrasp planner to generate a graspable pose
smach.StateMachine.add('SELECT_PREGRASP_PLANNER_INPUT_2', send_event('/pregrasp_planner_pipeline/event_in','e_start'),
transitions={'success':'WAIT_PLAN_ARM_MOTION_2'})
# wait for the result of the pregrasp planner
smach.StateMachine.add('WAIT_PLAN_ARM_MOTION_2', wait_for_event('/pregrasp_planner_pipeline/event_out', 15.0),
transitions={'success':'STOP_PLAN_ARM_MOTION_2',
'timeout': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
'failure':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'})
smach.StateMachine.add('STOP_PLAN_ARM_MOTION_2', send_event('/pregrasp_planner_pipeline/event_in','e_stop'),
transitions={'success':'MOVE_ARM_TO_OBJECT_2'})
# execute robot motion
smach.StateMachine.add('MOVE_ARM_TO_OBJECT_2', gbs.send_and_wait_events_combined(
event_in_list=[('/waypoint_trajectory_generation/event_in','e_start')],
event_out_list=[('/waypoint_trajectory_generation/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'STOP_MOVE_ARM_TO_OBJECT_2',
'timeout':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2',
'failure':'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2'})
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT_2', send_event('/waypoint_trajectory_generation/event_out','e_stop'),
transitions={'success':'OPEN_GRIPPER'})
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE_2', send_event('/waypoint_trajectory_generation/event_out','e_stop'),
transitions={'success':'MOVE_ARM_TO_DEFAULT_PLACE'})
# close gripper
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open_narrow'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'SET_ACTION_LIB_SUCCESS',
'failed':'MOVE_ARM_TO_HOLD'})
# place in case of failure
smach.StateMachine.add('MOVE_ARM_TO_DEFAULT_PLACE', gms.move_arm("10cm/pose4"),
transitions={'succeeded':'OPEN_GRIPPER_DEFAULT',
'failed':'MOVE_ARM_TO_DEFAULT_PLACE'})
smach.StateMachine.add('OPEN_GRIPPER_DEFAULT', gms.control_gripper('open_narrow'),
transitions={'succeeded': 'MOVE_ARM_BACK'})
smach.StateMachine.add('MOVE_ARM_BACK', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'SET_ACTION_LIB_SUCCESS',
'failed':'SET_ACTION_LIB_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True),
transitions={'succeeded':'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False),
transitions={'succeeded':'OVERALL_FAILED'})
"""smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILURE', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'OVERALL_FAILED',
'failed':'OVERALL_FAILED'})"""
# smach viewer
sis = smach_ros.IntrospectionServer('insert_object_smach_viewer', sm, '/INSERT_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'insert_object_server',
action_spec = InsertObjectAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'insert_object_goal',
feedback_key = 'insert_object_feedback',
result_key = 'insert_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
# Open the container
rospy.init_node("pick_object_wbc_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
smach.StateMachine.add(
"SELECT_OBJECT",
SelectObject("/mcr_perception/object_selector/input/object_name"),
transitions={"succeeded": "GENERATE_OBJECT_POSE"},
)
# generates a pose of object
smach.StateMachine.add(
"GENERATE_OBJECT_POSE",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_selector/event_in",
"e_trigger")],
event_out_list=[(
"/mcr_perception/object_selector/event_out",
"e_selected",
True,
)],
timeout_duration=10,
),
transitions={
"success": "OPEN_GRIPPER",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "SET_DBC_PARAMS"},
)
smach.StateMachine.add(
"SET_DBC_PARAMS",
gbs.set_named_config("dbc_pick_object"),
transitions={
"success": "MOVE_ROBOT_AND_PICK",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
# whole body control command. It moves direct base controller and
# calls pre-grasp planner, and (optionally) moves arm to object pose
smach.StateMachine.add(
"MOVE_ROBOT_AND_PICK",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "CLOSE_GRIPPER",
"timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
"failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
gbs.send_event([
("/waypoint_trajectory_generation/event_in", "e_start"),
("/wbc/event_in", "e_stop"),
]),
transitions={"success": "OVERALL_FAILED"},
)
smach.StateMachine.add(
"CLOSE_GRIPPER",
gms.control_gripper("close"),
transitions={"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE"},
)
# move arm to stage_intemediate position
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "VERIFY_OBJECT_GRASPED",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE",
},
)
smach.StateMachine.add(
"VERIFY_OBJECT_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=5,
),
transitions={
"success": "OVERALL_SUCCESS",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer("pick_object_smach_viewer", sm,
"/PICK_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="wbc_pick_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main(mokeup=False):
# Open the container
rospy.init_node('pick_object_wbc_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['pick_object_wbc_goal'],
output_keys = ['pick_object_wbc_feedback', 'pick_object_wbc_result'])
with sm:
if not mokeup:
smach.StateMachine.add('SELECT_OBJECT', select_object('/mcr_perception/object_selector/input/object_name'),
transitions={'success':'OPEN_GRIPPER'})
# open gripper
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'),
transitions={'succeeded': 'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('GENERATE_OBJECT_POSE', send_event('/mcr_perception/object_selector/event_in', 'e_trigger'),
transitions={'success':'CHECK_IF_OBJECT_IS_AVAILABLE'})
# waits for object selector response, if failure this means that the string published previously was not found in object list
smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', wait_for_event('/mcr_perception/object_selector/event_out', 1.0),
transitions={'success':'SET_DBC_PARAMS',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_pick_object'),
transitions={'success':'MOVE_ROBOT_TO_OBJECT',
'timeout':'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add('MOVE_ROBOT_TO_OBJECT', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in','e_start')],
event_out_list=[('/wbc/event_out','e_success', True)],
timeout_duration=25),
transitions={'success':'STOP_MOVE_ROBOT_TO_OBJECT',
'timeout':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
'failure':'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'})
# execute robot motion
smach.StateMachine.add('STOP_MOVE_ROBOT_TO_OBJECT', gbs.send_event([('/waypoint_trajectory_generation/event_in','e_start')]),
transitions={'success':'CLOSE_GRIPPER'})
smach.StateMachine.add('STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE', gbs.send_event([('/waypoint_trajectory_generation/event_in','e_start')]),
transitions={'success':'SET_ACTION_LIB_FAILURE'})
# close gripper
smach.StateMachine.add('CLOSE_GRIPPER', gms.control_gripper('close'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'VERIFY_OBJECT_GRASPED',
'failed':'MOVE_ARM_TO_HOLD'})
smach.StateMachine.add('VERIFY_OBJECT_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=5),
transitions={'success':'SET_ACTION_LIB_SUCCESS',
'timeout':'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True),
transitions={'succeeded':'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False),
transitions={'succeeded':'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('pick_object_smach_viewer', sm, '/PICK_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'wbc_pick_object_server',
action_spec = PickObjectWBCAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'pick_object_wbc_goal',
feedback_key = 'pick_object_wbc_feedback',
result_key = 'pick_object_wbc_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('move_base_safe_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED', 'OVERALL_PREEMPTED'],
input_keys=['goal'],
output_keys=['feedback', 'result'])
with sm:
smach.StateMachine.add('CHECK_DONT_BE_SAFE',
CheckDontBeSafe(),
transitions={
'safe': 'START_BARRIER_TAPE_DETECTION',
'unsafe': 'SETUP_MOVE_BASE'
})
smach.StateMachine.add(
'START_BARRIER_TAPE_DETECTION',
gbs.send_event([('/mir_perception/barrier_tape_detection/event_in',
'e_start')]),
transitions={'success': 'MOVE_ARM'})
smach.StateMachine.add('MOVE_ARM',
gms.move_arm('barrier_tape', blocking=False),
transitions={
'succeeded': 'SETUP_MOVE_BASE',
'failed': 'MOVE_ARM'
})
# get pose from action lib as string, convert to pose stamped and publish
smach.StateMachine.add('SETUP_MOVE_BASE',
SetupMoveBase('/move_base_wrapper/pose_in'),
transitions={
'succeeded': 'SET_DBC_PARAMS',
'failed':
'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE',
'preempted': 'OVERALL_PREEMPTED'
})
smach.StateMachine.add('SET_DBC_PARAMS',
gbs.set_named_config('dbc_move_base'),
transitions={
'success': 'MOVE_BASE',
'timeout': 'OVERALL_FAILED',
'failure': 'OVERALL_FAILED'
})
# send event_in to move base to a pose
# add events to mir_look at and mcr_converter
smach.StateMachine.add(
'MOVE_BASE',
gbs.
send_and_wait_events_combined(event_in_list=[
('/move_base_wrapper/event_in', 'e_start'),
('/mcr_common/twist_to_motion_direction_conversion/event_in',
'e_start'),
('/mir_manipulation/look_at_point/event_in', 'e_start')
],
event_out_list=[
('/move_base_wrapper/event_out',
'e_success', True)
],
timeout_duration=50),
transitions={
'success': 'SETUP_MOVE_ARM_AFTER_MOVE_BASE',
'timeout': 'RESET_BARRIER_TAPE',
'failure': 'RESET_BARRIER_TAPE'
})
smach.StateMachine.add(
'RESET_BARRIER_TAPE',
gbs.send_event([('/mir_perception/barrier_tape_detection/event_in',
'e_reset')]),
transitions={'success': 'SETUP_MOVE_BASE_AGAIN'})
smach.StateMachine.add('SETUP_MOVE_BASE_AGAIN',
SetupMoveBase('/move_base_wrapper/pose_in'),
transitions={
'succeeded': 'MOVE_BASE_AGAIN',
'failed':
'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE',
'preempted': 'OVERALL_PREEMPTED'
})
smach.StateMachine.add(
'MOVE_BASE_AGAIN',
gbs.
send_and_wait_events_combined(event_in_list=[
('/move_base_wrapper/event_in', 'e_start'),
('/mcr_common/twist_to_motion_direction_conversion/event_in',
'e_start'),
('/mir_manipulation/look_at_point/event_in', 'e_start')
],
event_out_list=[
('/move_base_wrapper/event_out',
'e_success', True)
],
timeout_duration=50),
transitions={
'success': 'SETUP_MOVE_ARM_AFTER_MOVE_BASE',
'timeout': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE',
'failure': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE'
})
# send arm to a position depending on next action
smach.StateMachine.add('SETUP_MOVE_ARM_AFTER_MOVE_BASE',
SetupMoveArmAfterMoveBase(),
transitions={
'succeeded': 'MOVE_ARM_AFTER_MOVE_BASE',
'skipped': 'ADJUST_BASE',
'failed': 'SETUP_MOVE_ARM_AFTER_MOVE_BASE'
})
smach.StateMachine.add('MOVE_ARM_AFTER_MOVE_BASE',
gms.move_arm(blocking=False),
transitions={
'succeeded': 'ADJUST_BASE',
'failed': 'ADJUST_BASE'
})
# call direct base controller to fine adjust the base to the final desired pose
# (navigation tolerance is set to a wide tolerance)
smach.StateMachine.add(
'ADJUST_BASE',
gbs.send_and_wait_events_combined(
event_in_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_start'),
('/mir_manipulation/look_at_point/event_in', 'e_stop')],
event_out_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_success', True)],
timeout_duration=5
), # this is a tradeoff between speed and accuracy, set a higher value for accuracy increase
transitions={
'success': 'STOP_CONTROLLER_WITH_SUCCESS',
'timeout': 'STOP_CONTROLLER_WITH_SUCCESS',
'failure': 'STOP_CONTROLLER_WITH_FAILURE'
})
# stop controller with success
smach.StateMachine.add(
'STOP_CONTROLLER_WITH_SUCCESS',
gbs.send_and_wait_events_combined(
event_in_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_stop')],
event_out_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_stopped', True)],
timeout_duration=1),
transitions={
'success': 'STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS',
'timeout': 'STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS',
'failure': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE'
})
# stop controller with failure
smach.StateMachine.add(
'STOP_CONTROLLER_WITH_FAILURE',
gbs.send_and_wait_events_combined(
event_in_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_stop')],
event_out_list=
[('/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_stopped', True)],
timeout_duration=1),
transitions={
'success': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE',
'timeout': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE',
'failure': 'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE'
})
smach.StateMachine.add(
'STOP_BARRIER_TAPE_DETECTION_WITH_FAILURE',
gbs.send_event([('/mir_perception/barrier_tape_detection/event_in',
'e_stop')]),
transitions={'success': 'OVERALL_FAILED'})
smach.StateMachine.add(
'STOP_BARRIER_TAPE_DETECTION_WITH_SUCCESS',
gbs.send_event([('/mir_perception/barrier_tape_detection/event_in',
'e_stop')]),
transitions={'success': 'ALIGN_WITH_WORKSPACE'})
smach.StateMachine.add('ALIGN_WITH_WORKSPACE',
AlignWithWorkspace(),
transitions={
'succeeded': 'OVERALL_SUCCESS',
'failed': 'OVERALL_SUCCESS'
})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = smach_ros.IntrospectionServer('move_base_safe_smach_viewer', sm,
'/MOVE_BASE_SAFE_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='move_base_safe_server',
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['OVERALL_PREEMPTED'],
goal_key='goal',
feedback_key='feedback',
result_key='result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
# Open the container
rospy.init_node("insert_object_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
smach.StateMachine.add(
"SELECT_OBJECT",
SelectObject("/mcr_perception/object_selector/input/object_name"),
transitions={"succeeded": "GENERATE_OBJECT_POSE"},
)
# generates a pose of object
smach.StateMachine.add(
"GENERATE_OBJECT_POSE",
gbs.send_and_wait_events_combined(
event_in_list=[("/mcr_perception/object_selector/event_in",
"e_trigger")],
event_out_list=[(
"/mcr_perception/object_selector/event_out",
"e_selected",
True,
)],
timeout_duration=10,
),
transitions={
"success": "SET_DBC_PARAMS",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"SET_DBC_PARAMS",
gbs.set_named_config("dbc_pick_object"),
transitions={
"success": "MOVE_ROBOT_AND_TRY_PLACING",
"timeout": "OVERALL_FAILED",
"failure": "OVERALL_FAILED",
},
)
smach.StateMachine.add(
"MOVE_ROBOT_AND_TRY_PLACING",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_try")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "UNSTAGE_OBJECT",
"timeout": "UNSTAGE_OBJECT",
"failure": "UNSTAGE_OBJECT",
},
)
smach.StateMachine.add(
"UNSTAGE_OBJECT",
gas.unstage_object(),
transitions={
"success": "RE_GENERATE_OBJECT_POSE",
"failed": "OVERALL_FAILED",
},
)
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
"RE_GENERATE_OBJECT_POSE",
gbs.send_event([("/mcr_perception/object_selector/event_in",
"e_re_trigger")]),
transitions={"success": "MOVE_ROBOT_AND_PLACE"},
)
# execute robot motion
smach.StateMachine.add(
"MOVE_ROBOT_AND_PLACE",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=20,
),
transitions={
"success": "OPEN_GRIPPER",
"timeout": "MOVE_ARM_TO_DEFAULT_PLACE",
"failure": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_DEFAULT_PLACE",
gms.move_arm("look_at_turntable"),
transitions={
"succeeded": "MOVE_ARM_CARTESIAN",
"failed": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_CARTESIAN",
gbs.send_and_wait_events_combined(
event_in_list=[("/insert_object_workaroud/event_in", "e_start")
],
event_out_list=[("/insert_object_workaroud/event_out",
"e_success", True)],
timeout_duration=10,
),
transitions={
"success": "OPEN_GRIPPER",
"timeout": "STOP_ARM_CARTESIAN_MOTION",
"failure": "OPEN_GRIPPER",
},
)
smach.StateMachine.add(
"STOP_ARM_CARTESIAN_MOTION",
gbs.send_event([("/insert_object_workaroud/event_in", "e_stop")]),
transitions={"success": "OPEN_GRIPPER"},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open_narrow"),
transitions={"succeeded": "MOVE_ARM_TO_HOLD"},
)
# move arm to HOLD position
smach.StateMachine.add(
"MOVE_ARM_TO_HOLD",
gms.move_arm("look_at_turntable"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_HOLD",
},
)
sm.register_transition_cb(transition_cb)
sm.register_start_cb(start_cb)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = smach_ros.IntrospectionServer("insert_object_smach_viewer", sm,
"/INSERT_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="insert_object_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
# Open the container
rospy.init_node("calibrate_pick_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
# publish a static frame which will be used as reference for perceived objs
smach.StateMachine.add(
"STOP_REFERENCE_FRAME_PUB",
gbs.send_event([("/static_transform_publisher_node/event_in",
"e_stop")]),
transitions={"success": "OPEN_GRIPPER"},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "MOVE_ARM"},
)
smach.StateMachine.add('MOVE_ARM',
gms.move_arm('look_at_workspace_from_near'),
transitions={
'succeeded': 'PUBLISH_REFERENCE_FRAME',
'failed': 'MOVE_ARM'
})
# publish a static frame which will be used as reference for perceived objs
smach.StateMachine.add(
"PUBLISH_REFERENCE_FRAME",
gbs.send_event([("/static_transform_publisher_node/event_in",
"e_start")]),
transitions={"success": "WAIT_FOR_EVENT_FROM_USER"},
)
smach.StateMachine.add('WAIT_FOR_EVENT_FROM_USER',
WaitForEventFromUser(),
transitions={
'success': 'MOVE_ROBOT_AND_PICK',
'failure': 'OVERALL_FAILED'
})
# whole body control command. It moves direct base controller and
# calls pre-grasp planner, and (optionally) moves arm to object pose
smach.StateMachine.add(
"MOVE_ROBOT_AND_PICK",
gbs.send_and_wait_events_combined(
event_in_list=[("/wbc/event_in", "e_start")],
event_out_list=[("/wbc/event_out", "e_success", True)],
timeout_duration=50,
),
transitions={
"success": "OVERALL_SUCCESS",
"timeout": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
"failure": "STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
},
)
smach.StateMachine.add(
"STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE",
gbs.send_event([
("/waypoint_trajectory_generation/event_in", "e_start"),
("/wbc/event_in", "e_stop"),
]),
transitions={"success": "OVERALL_FAILED"},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer("calibrate_pick_smach_viewer", sm,
"/CALIBRATE_PICK_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="calibrate_pick_server",
action_spec=GenericExecuteAction,
wrapped_container=sm,
succeeded_outcomes=["OVERALL_SUCCESS"],
aborted_outcomes=["OVERALL_FAILED"],
preempted_outcomes=["PREEMPTED"],
goal_key="goal",
feedback_key="feedback",
result_key="result",
)
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main(mokeup=False):
# Open the container
rospy.init_node('pick_object_wbc_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['pick_object_wbc_goal'],
output_keys = ['pick_object_wbc_feedback', 'pick_object_wbc_result'])
with sm:
if not mokeup:
# publish object as string to mcr_perception_selectors -> object_selector, this component then publishes
# pose in base_link reference frame when e_trigger is sent (next state)
smach.StateMachine.add('SELECT_OBJECT', select_object('/mcr_perception/object_selector/input/object_name'),
transitions={'success':'OPEN_GRIPPER'})
# open gripper
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'),
transitions={'succeeded': 'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('GENERATE_OBJECT_POSE', send_event('/mcr_perception/object_selector/event_in', 'e_trigger'),
transitions={'success':'CHECK_IF_OBJECT_IS_AVAILABLE'})
# waits for object selector response, if failure this means that the string published previously was not found in object list
smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', wait_for_event('/mcr_perception/object_selector/event_out', 1.0),
transitions={'success':'COLLISION_VELOCITY_CHECKER',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add('COLLISION_VELOCITY_CHECKER', gbs.send_and_wait_events_combined(
event_in_list=[("/base_collision_checker_pipeline/pose_transformer/event_in",'e_start'),
('/base_collision_checker_pipeline/base_collision_checker_node/event_in','e_start')],
event_out_list=[('/base_collision_checker_pipeline/pose_transformer/event_out','e_success', True),
('/base_collision_checker_pipeline/base_collision_checker_node/event_out','e_success', True)],
timeout_duration=30),
transitions={'success':'STOP_COLLISION_CHECKER',
'timeout':'STOP_COLLISION_CHECKER_FAILURE',
'failure':'STOP_COLLISION_CHECKER_FAILURE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('STOP_COLLISION_CHECKER', send_event('/base_collision_checker_pipeline/base_collision_checker_node/event_in', 'e_stop'),
transitions={'success':'STOP_POSE_TRANSFORMER'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('STOP_POSE_TRANSFORMER', send_event('/base_collision_checker_pipeline/pose_transformer/event_in', 'e_stop'),
transitions={'success':'SET_ACTION_LIB_SUCCESS'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('STOP_COLLISION_CHECKER_FAILURE', send_event('/base_collision_checker_pipeline/base_collision_checker_node/event_in', 'e_stop'),
transitions={'success':'STOP_POSE_TRANSFORMER_FAILURE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add('STOP_POSE_TRANSFORMER_FAILURE', send_event('/base_collision_checker_pipeline/pose_transformer/event_in', 'e_stop'),
transitions={'success':'SET_ACTION_LIB_FAILURE'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True),
transitions={'succeeded':'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False),
transitions={'succeeded':'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('pick_object_smach_viewer', sm, '/PICK_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'wbc_pick_object_server',
action_spec = PickObjectWBCAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'pick_object_wbc_goal',
feedback_key = 'pick_object_wbc_feedback',
result_key = 'pick_object_wbc_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('move_base_safe_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED','OVERALL_PREEMPTED'],
input_keys = ['move_base_safe_goal'],
output_keys = ['move_base_safe_feedback', 'move_base_safe_result'])
with sm:
#add states to the container
smach.StateMachine.add('SETUP_MOVE_ARM', SetupMoveArm(),
transitions={'succeeded': 'MOVE_ARM',
'failed': 'SETUP_MOVE_ARM'})
smach.StateMachine.add('MOVE_ARM', gms.move_arm(),
transitions={'succeeded': 'SETUP_MOVE_BASE',
'failed': 'MOVE_ARM'})
# get pose from action lib as string, convert to pose stamped and publish
smach.StateMachine.add('SETUP_MOVE_BASE', SetupMoveBase('/move_base_wrapper/pose_in'),
transitions={'succeeded': 'MOVE_BASE',
'failed': 'SET_ACTION_LIB_FAILURE',
'preempted':'OVERALL_PREEMPTED'})
# send event_in to move base to a pose
smach.StateMachine.add('MOVE_BASE', gbs.send_and_wait_events_combined(
event_in_list=[('/move_base_wrapper/event_in','e_start')],
event_out_list=[('/move_base_wrapper/event_out','e_success', True)],
timeout_duration=50),
transitions={'success':'ADJUST_BASE',
'timeout':'SET_ACTION_LIB_FAILURE',
'failure':'SETUP_MOVE_BASE'})
# call direct base controller to fine adjust the base to the final desired pose
# (navigation tolerance is set to a wide tolerance)
smach.StateMachine.add('ADJUST_BASE', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_navigation/direct_base_controller/coordinator/event_in','e_start')],
event_out_list=[('/mcr_navigation/direct_base_controller/coordinator/event_out','e_success', True)],
timeout_duration=5), # this is a tradeoff between speed and accuracy, set a higher value for accuracy increase
transitions={'success':'STOP_CONTROLLER_WITH_SUCCESS',
'timeout':'STOP_CONTROLLER_WITH_SUCCESS',
'failure':'STOP_CONTROLLER_WITH_FAILURE'})
# stop controller with success
smach.StateMachine.add('STOP_CONTROLLER_WITH_SUCCESS', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop')],
event_out_list=[('/mcr_navigation/direct_base_controller/coordinator/event_out','e_stopped', True)],
timeout_duration=1),
transitions={'success':'SET_ACTION_LIB_SUCCESS',
'timeout':'SET_ACTION_LIB_SUCCESS',
'failure':'SET_ACTION_LIB_FAILURE'})
# stop controller with failure
smach.StateMachine.add('STOP_CONTROLLER_WITH_FAILURE', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_navigation/direct_base_controller/coordinator/event_in','e_stop')],
event_out_list=[('/mcr_navigation/direct_base_controller/coordinator/event_out','e_stopped', True)],
timeout_duration=1),
transitions={'success':'SET_ACTION_LIB_FAILURE',
'timeout':'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True),
#transitions={'succeeded':'RESET_STATIC_TRANSFORM_FOR_PERCEPTION'})
transitions={'succeeded':'OVERALL_SUCCESS'})
# generates a pose based on the previous string object topic received
#smach.StateMachine.add('RESET_STATIC_TRANSFORM_FOR_PERCEPTION', gbs.send_event([('/static_transform_publisher_node/event_in', 'e_start')]),
# transitions={'success':'OVERALL_SUCCESS'})
smach.StateMachine.add('SET_ACTION_LIB_FAILURE', SetActionLibResult(False),
transitions={'succeeded':'OVERALL_FAILED'})
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'move_base_safe_server',
action_spec = MoveBaseSafeAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['OVERALL_PREEMPTED'],
goal_key = 'move_base_safe_goal',
feedback_key = 'move_base_safe_feedback',
result_key = 'move_base_safe_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('insert_cavity_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED'],
input_keys = ['goal'],
output_keys = ['feedback', 'result'])
with sm:
# publish object as string to mcr_perception_selectors -> cavity,
# this component then publishes pose in base_link reference frame
smach.StateMachine.add('SELECT_OBJECT', SelectObject(
'/mcr_perception/cavity_pose_selector/object_name'),
transitions={'succeeded':'CHECK_IF_OBJECT_IS_AVAILABLE'})
smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/cavity_pose_selector/event_in','e_trigger')],
event_out_list=[('/mcr_perception/cavity_pose_selector/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'SET_DBC_PARAMS',
'timeout':'UNSTAGE_OBJECT',
'failure':'UNSTAGE_OBJECT'})
smach.StateMachine.add('SET_DBC_PARAMS', gbs.set_named_config('dbc_pick_object'),
transitions={'success':'MOVE_ROBOT_AND_TRY_PLACING',
'timeout':'OVERALL_FAILED',
'failure':'OVERALL_FAILED'})
smach.StateMachine.add('MOVE_ROBOT_AND_TRY_PLACING', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in','e_try')],
event_out_list=[('/wbc/event_out','e_success', True)],
timeout_duration=50),
transitions={'success':'STOP_POSE_SELECTOR',
'timeout':'STOP_POSE_SELECTOR',
'failure':'STOP_POSE_SELECTOR'})
smach.StateMachine.add('STOP_POSE_SELECTOR', gbs.send_event(
[('/mcr_perception/cavity_pose_selector/event_in', 'e_stop')]),
transitions={'success':'PERCEIVE_CAVITY'})
# perceive cavity again after moving in front of the desired cavity
smach.StateMachine.add('PERCEIVE_CAVITY', gas.perceive_cavity(),
transitions={'success': 'SELECT_OBJECT_AGAIN',
'failed' : 'OVERALL_FAILED'})
smach.StateMachine.add('SELECT_OBJECT_AGAIN', SelectObject(
'/mcr_perception/cavity_pose_selector/object_name'),
transitions={'succeeded':'CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN'})
smach.StateMachine.add('CHECK_IF_OBJECT_IS_AVAILABLE_AGAIN', gbs.send_and_wait_events_combined(
event_in_list=[('/mcr_perception/cavity_pose_selector/event_in','e_trigger')],
event_out_list=[('/mcr_perception/cavity_pose_selector/event_out','e_success', True)],
timeout_duration=10),
transitions={'success':'UNSTAGE_OBJECT',
'timeout':'OVERALL_FAILED',
'failure':'OVERALL_FAILED'})
smach.StateMachine.add('UNSTAGE_OBJECT', gas.unstage_object(),
transitions={'success': 'TRY_INSERTING',
'failed' : 'OVERALL_FAILED'})
# execute robot motion
smach.StateMachine.add('TRY_INSERTING', gbs.send_and_wait_events_combined(
event_in_list=[('/wbc/event_in','e_start_arm_only')],
event_out_list=[('/wbc/event_out','e_success', True)],
timeout_duration=20),
transitions={'success':'OPEN_GRIPPER',
'timeout':'STAGE_OBJECT',
'failure':'STAGE_OBJECT'})
smach.StateMachine.add('STAGE_OBJECT', gas.stage_object(),
transitions={'success': 'OVERALL_FAILED',
'failed' : 'OVERALL_FAILED'})
# close gripper
smach.StateMachine.add('OPEN_GRIPPER', gms.control_gripper('open'),
transitions={'succeeded': 'WIGGLE_ARM'})
# wiggling the arm for precision placement
smach.StateMachine.add('WIGGLE_ARM', ppt_wiggle_arm(wiggle_offset=-0.12, joint=0),
transitions={'succeeded':'MOVE_ARM_TO_HOLD',
'failed':'MOVE_ARM_TO_HOLD'})
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("look_at_turntable"),
transitions={'succeeded':'STOP_POSE_SELECTOR_FINAL',
'failed':'MOVE_ARM_TO_HOLD'})
# sending e_stop to pose selector
smach.StateMachine.add('STOP_POSE_SELECTOR_FINAL', gbs.send_event(
[('/mcr_perception/cavity_pose_selector/event_in', 'e_stop')]),
transitions={'success':'OVERALL_SUCCESS'})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = IntrospectionServer('insert_cavity_smach_viewer', sm, '/INSERT_CAVITY_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name = 'insert_cavity_server',
action_spec = GenericExecuteAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['PREEMPTED'],
goal_key = 'goal',
feedback_key = 'feedback',
result_key = 'result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main(mokeup=False):
# Open the container
rospy.init_node('pick_object_wbc_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['pick_object_wbc_goal'],
output_keys=['pick_object_wbc_feedback', 'pick_object_wbc_result'])
with sm:
if not mokeup:
# publish object as string to mcr_perception_selectors -> object_selector, this component then publishes
# pose in base_link reference frame when e_trigger is sent (next state)
smach.StateMachine.add(
'SELECT_OBJECT',
select_object(
'/mcr_perception/object_selector/input/object_name'),
transitions={'success': 'OPEN_GRIPPER'})
# open gripper
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'GENERATE_OBJECT_POSE'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
'GENERATE_OBJECT_POSE',
send_event('/mcr_perception/object_selector/event_in',
'e_trigger'),
transitions={'success': 'CHECK_IF_OBJECT_IS_AVAILABLE'})
# waits for object selector response, if failure this means that the string published previously was not found in object list
smach.StateMachine.add(
'CHECK_IF_OBJECT_IS_AVAILABLE',
wait_for_event('/mcr_perception/object_selector/event_out',
1.0),
transitions={
'success': 'SELECT_PREGRASP_PLANNER_INPUT',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
'''
# reconfigure pregrasp planner for pick
smach.StateMachine.add('RECONFIGURE_PREGRASP_PARAMS', gbs.set_named_config('pick_normal_pregrasp'),
transitions={'success':'PLAN_WHOLE_BODY_MOTION',
'failure':'SET_ACTION_LIB_FAILURE',
'timeout': 'RECONFIGURE_PREGRASP_PARAMS'})
'''
smach.StateMachine.add(
'SELECT_PREGRASP_PLANNER_INPUT',
send_event('/mir_manipulation/mux_pregrasp_pose/select',
'/whole_body_motion_calculator_pipeline/pose_out'),
transitions={'success': 'PLAN_WHOLE_BODY_MOTION'})
smach.StateMachine.add(
'PLAN_WHOLE_BODY_MOTION',
send_event('/whole_body_motion_calculator_pipeline/event_in',
'e_start'),
transitions={'success': 'WAIT_PLAN_WHOLE_BODY_MOTION'})
# wait for the result of the pregrasp planner
smach.StateMachine.add(
'WAIT_PLAN_WHOLE_BODY_MOTION',
wait_for_event('/whole_body_motion_calculator_pipeline/event_out',
15.0),
transitions={
'success': 'PLAN_ARM_MOTION',
'timeout': 'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE',
'failure': 'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE'
})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add(
'STOP_PLAN_WHOLE_BODY_MOTION_WITH_FAILURE',
send_event('/whole_body_motion_calculator_pipeline/event_in',
'e_stop'),
transitions={'success': 'STOP_PLAN_ARM_MOTION_WITH_FAILURE'})
# based on a published pose, calls pregrasp planner to generate a graspable pose
smach.StateMachine.add('PLAN_ARM_MOTION',
send_event(
'/pregrasp_planner_pipeline/event_in',
'e_start'),
transitions={'success': 'WAIT_PLAN_ARM_MOTION'})
# wait for the result of the pregrasp planner
smach.StateMachine.add('WAIT_PLAN_ARM_MOTION',
wait_for_event(
'/pregrasp_planner_pipeline/event_out',
15.0),
transitions={
'success': 'STOP_PLAN_ARM_MOTION',
'timeout': 'PLAN_WHOLE_BODY_MOTION',
'failure': 'PLAN_WHOLE_BODY_MOTION'
})
smach.StateMachine.add(
'STOP_PLAN_ARM_MOTION',
send_event('/pregrasp_planner_pipeline/event_in', 'e_stop'),
transitions={'success': 'CALCULATE_BASE_MOTION'})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add(
'STOP_PLAN_ARM_MOTION_WITH_FAILURE',
send_event('/pregrasp_planner_pipeline/event_in', 'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add(
'CALCULATE_BASE_MOTION',
gbs.send_and_wait_events_combined(
#event_in_list=[('/base_motion_calculator/event_in','e_start'),
event_in_list=[('/base_motion_calculator/event_in', 'e_start')
],
#('/base_collision_checker_pipeline/base_collision_checker_node/event_in','e_start')],
#event_out_list=[('/base_motion_calculator/event_out','e_success', True),
event_out_list=[('/base_motion_calculator/event_out',
'e_success', True)],
#('/base_collision_checker_pipeline/base_collision_checker_node/event_out','e_success', True)],
timeout_duration=30),
transitions={
'success': 'STOP_PLAN_WHOLE_BODY_MOTION',
'timeout': 'STOP_MOVE_BASE_TO_OBJECT',
'failure': 'PLAN_WHOLE_BODY_MOTION'
}) # select next pose from the sampled poses
# wbc pipeline was successful, so lets stop it since its work is done
smach.StateMachine.add(
'STOP_PLAN_WHOLE_BODY_MOTION',
send_event('/whole_body_motion_calculator_pipeline/event_in',
'e_stop'),
transitions={'success': 'MOVE_ROBOT_TO_OBJECT'})
# execute robot motion
smach.StateMachine.add(
'MOVE_ROBOT_TO_OBJECT',
gbs.send_and_wait_events_combined(
event_in_list=[ #('/move_arm_planned/event_in','e_start'),
#('/base_motion_calculator/event_in','e_start'),
('/wbc/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_start')
],
event_out_list=
[ #('/move_arm_planned/event_out','e_success', True),
#('/base_motion_calculator/event_out','e_success', True),
('/mcr_navigation/collision_velocity_filter/event_out',
'e_reduced_velocities_forwarded', False),
('/wbc/mcr_navigation/direct_base_controller/coordinator/event_out',
'e_success', True)
],
timeout_duration=10),
transitions={
'success': 'STOP_MOVE_BASE_TO_OBJECT',
'timeout': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
'failure': 'STOP_MOVE_BASE_TO_OBJECT'
})
# send stop event_in to arm motion component
smach.StateMachine.add(
'STOP_MOVE_BASE_TO_OBJECT',
send_event(
'/wbc/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_stop'),
transitions={'success': 'STOP_TRAJECTORY_GENERATOR'})
# send stop event_in to arm motion component and return failure
smach.StateMachine.add(
'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
send_event(
'/wbc/mcr_navigation/direct_base_controller/coordinator/event_in',
'e_stop'),
transitions={'success': 'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE'})
smach.StateMachine.add(
'STOP_TRAJECTORY_GENERATOR',
send_event('/waypoint_trajectory_generation/event_out', 'e_stop'),
transitions={'success': 'SELECT_PREGRASP_PLANNER_INPUT_2'})
smach.StateMachine.add(
'SELECT_PREGRASP_PLANNER_INPUT_2',
send_event('/mir_manipulation/mux_pregrasp_pose/select',
'/mcr_perception/object_selector/output/object_pose'),
transitions={'success': 'PUBLISH_OBJECT_POSE'})
# pregrasp planner failed or timeout, stop the component and then return overall failure
smach.StateMachine.add('PUBLISH_OBJECT_POSE',
publish_object_pose(),
transitions={
'succeeded':
'PLAN_ARM_MOTION_2',
'failed':
'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'
})
# based on a published pose, calls pregrasp planner to generate a graspable pose
smach.StateMachine.add(
'PLAN_ARM_MOTION_2',
send_event('/pregrasp_planner_pipeline/event_in', 'e_start'),
transitions={'success': 'WAIT_PLAN_ARM_MOTION_2'})
# wait for the result of the pregrasp planner
smach.StateMachine.add('WAIT_PLAN_ARM_MOTION_2',
wait_for_event(
'/pregrasp_planner_pipeline/event_out',
15.0),
transitions={
'success':
'STOP_PLAN_ARM_MOTION_2',
'timeout':
'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
'failure':
'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'
})
smach.StateMachine.add('STOP_PLAN_ARM_MOTION_2',
send_event(
'/pregrasp_planner_pipeline/event_in',
'e_stop'),
transitions={'success': 'MOVE_ARM_TO_OBJECT'})
# execute robot motion
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)],
event_in_list=[('/waypoint_trajectory_generation/event_in',
'e_start')],
event_out_list=[('/waypoint_trajectory_generation/event_out',
'e_success', True)],
timeout_duration=10),
transitions={
'success': 'STOP_MOVE_ARM_TO_OBJECT',
'timeout': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE',
'failure': 'STOP_MOVE_ROBOT_TO_OBJECT_WITH_FAILURE'
})
#smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE', send_event('/move_arm_planned/event_in','e_stop'),
smach.StateMachine.add(
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE',
send_event('/waypoint_trajectory_generation/event_out', 'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
smach.StateMachine.add('STOP_MOVE_ARM_TO_OBJECT',
send_event(
'/waypoint_trajectory_generation/event_out',
'e_stop'),
transitions={'success': 'CLOSE_GRIPPER'})
# close gripper
smach.StateMachine.add('CLOSE_GRIPPER',
gms.control_gripper('close'),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD'})
'''
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':'MOVE_ARM_TO_HOLD',
'timeout':'CLOSE_GRIPPER',
'failure':'CLOSE_GRIPPER'})
'''
# move arm to HOLD position
smach.StateMachine.add('MOVE_ARM_TO_HOLD',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_TO_HOLD'
})
'''
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_ACTION_LIB_SUCCESS',
'timeout':'SET_ACTION_LIB_FAILURE',
'failure':'SET_ACTION_LIB_FAILURE'})
'''
# set action lib result
smach.StateMachine.add(
'SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'RESET_PREGRASP_PLANNER_INPUT_SUCCESS'})
smach.StateMachine.add(
'RESET_PREGRASP_PLANNER_INPUT_SUCCESS',
send_event('/mir_manipulation/mux_pregrasp_pose/select',
'/mcr_perception/object_selector/output/object_pose'),
transitions={'success': 'OVERALL_SUCCESS'})
# set action lib result
smach.StateMachine.add(
'SET_ACTION_LIB_FAILURE',
SetActionLibResult(False),
transitions={'succeeded': 'MOVE_ARM_TO_HOLD_FAILURE'})
smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILURE',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded':
'RESET_PREGRASP_PLANNER_INPUT_FAILURE',
'failed':
'RESET_PREGRASP_PLANNER_INPUT_FAILURE'
})
smach.StateMachine.add(
'RESET_PREGRASP_PLANNER_INPUT_FAILURE',
send_event('/mir_manipulation/mux_pregrasp_pose/select',
'/mcr_perception/object_selector/output/object_pose'),
transitions={'success': 'OVERALL_FAILED'})
# smach viewer
sis = smach_ros.IntrospectionServer('pick_object_smach_viewer', sm,
'/PICK_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='wbc_pick_object_server',
action_spec=PickObjectWBCAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='pick_object_wbc_goal',
feedback_key='pick_object_wbc_feedback',
result_key='pick_object_wbc_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
