def main():
rospy.init_node('align_with_workspace_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS','OVERALL_FAILED','OVERALL_PREEMPTED'],
input_keys = ['align_with_workspace_goal', 'align_with_workspace_result'],
output_keys = ['align_with_workspace_feedback', 'align_with_workspace_result'])
with sm:
smach.StateMachine.add('ALIGN_BASE', SetupAlignWithWorkspace(),
transitions={'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'SET_ACTION_LIB_FAILURE',
'preempted':'OVERALL_PREEMPTED'})
smach.StateMachine.add('SET_ACTION_LIB_SUCCESS', SetActionLibResult(True),
#transitions={'succeeded':'RESET_STATIC_TRANSFORM_FOR_PERCEPTION'})
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_workspace_server',
action_spec = AlignWithWorkspaceAction,
wrapped_container = sm,
succeeded_outcomes = ['OVERALL_SUCCESS'],
aborted_outcomes = ['OVERALL_FAILED'],
preempted_outcomes = ['OVERALL_PREEMPTED'],
goal_key = 'align_with_workspace_goal',
feedback_key = 'align_with_workspace_feedback',
result_key = 'align_with_workspace_result')
asw.run_server()
rospy.spin()
def main():
rospy.init_node('skill_name_server')
# Construct state machine
sm = SkillName()
# Smach viewer
sis = smach_ros.IntrospectionServer('skill_name_smach_viewer', sm,
'/SKILL_NAME_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name='skill_name_server',
action_spec=SkillNameAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='S_goal',
feedback_key='skill_name_feedback',
result_key='skill_name_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('pick_object')
sm = smach.StateMachine(outcomes=['picked',
'pick_failed',
'preempted',
'error'],
input_keys=['goal'],
output_keys=['feedback',
'result'])
with sm:
sm.userdata.pose_arm_default = geometry_msgs.PoseStamped()
sm.userdata.pose_arm_default.header.stamp = rospy.Time.now()
sm.userdata.pose_arm_default.header.frame_id = "/base_footprint"
sm.userdata.pose_arm_default.pose.position.x = 0.1
sm.userdata.pose_arm_default.pose.position.y = 0.0
sm.userdata.pose_arm_default.pose.position.z = 0.5
sm.userdata.pose_arm_default.pose.orientation.w = 1.0
sm.userdata.result = pick_and_place_msgs.PickObjectResult()
smach.StateMachine.add('ParseGoal',
ParseGoal(),
remapping={'goal':'goal',
'feedback':'feedback',
'object_pose':'object_pose'},
transitions={'parsed':'PickObject'})
sm_pick = pick_object_sm.createSM()
smach.StateMachine.add('PickObject',
sm_pick,
remapping={'object_name':'object_name',
'object_pose':'object_pose',
'pose_arm_default':'pose_arm_default'},
transitions={'picked':'picked',
'pick_failed':'pick_failed',
'preempted':'preempted',
'error':'error'})
asw = ActionServerWrapper('pick_object',
pick_and_place_msgs.PickObjectAction,
wrapped_container = sm,
goal_key = 'goal',
feedback_key = 'feedback',
result_key = 'result',
succeeded_outcomes = ['picked'],
aborted_outcomes = ['pick_failed','error'],
preempted_outcomes = ['preempted'])
asw.run_server()
rospy.spin()
return
def main():
rospy.init_node('place_object')
sm = smach.StateMachine(
outcomes=['placed', 'place_failed', 'preempted', 'error'],
input_keys=['goal'],
output_keys=['feedback', 'result'])
with sm:
sm.userdata.result = pick_and_place_msgs.PickObjectResult()
smach.StateMachine.add('ParseGoal',
ParseGoal(),
remapping={
'goal': 'goal',
'feedback': 'feedback',
'place_pose': 'place_pose'
},
transitions={'parsed': 'PlaceObject'})
sm_place = place_object_sm.createSM()
smach.StateMachine.add('PlaceObject',
sm_place,
remapping={
'object_name': 'object_name',
'place_pose': 'place_pose'
},
transitions={
'placed': 'placed',
'place_failed': 'place_failed',
'preempted': 'preempted',
'error': 'error'
})
asw = ActionServerWrapper('place_object',
pick_and_place_msgs.PlaceObjectAction,
wrapped_container=sm,
goal_key='goal',
feedback_key='feedback',
result_key='result',
succeeded_outcomes=['placed'],
aborted_outcomes=['place_failed', 'error'],
preempted_outcomes=['preempted'])
asw.run_server()
rospy.spin()
return
def main():
sm_top = StateMachine(
outcomes = ['success',
'localize_failed',
'navi_failed',
'get_failed',
'preempted'],
input_keys = ['goal_message',
'result_message'],
output_keys = ['result_message'])
with sm_top:
StateMachine.add(
'LOCALIZATION',
Localization(),
transitions = {'localize':'GET_COORDINATE',
'not_localize':'localize_failed'},
remapping = {'goal_in':'goal_message'})
StateMachine.add(
'GET_COORDINATE',
GetCootdinate(),
transitions = {'get':'NAVIGATION',
'not_get':'get_failed'},
remapping = {'coord_out':'person_coord'})
StateMachine.add(
'NAVIGATION',
Navigation(),
transitions = {'arrive':'success',
'not_arrive':'navi_failed'},
remapping = {'result_message':'result_message',
'coord_in':'person_coord'})
asw = ActionServerWrapper(
'approach_person',
ApproachPersonAction,
wrapped_container = sm_top,
succeeded_outcomes = ['success'],
aborted_outcomes = ['find_failed',
'get_failed',
'navi_failed'],
preempted_outcomes = ['preempted'],
goal_key = 'goal_message',
result_key = 'result_message')
asw.run_server()
rospy.spin()
def test_action_server_wrapper(self):
"""Test action server wrapper."""
sq = Sequence(['succeeded', 'aborted', 'preempted'], 'succeeded')
sq.register_input_keys(['goal', 'action_goal', 'action_result'])
sq.register_output_keys(['action_result'])
with sq:
Sequence.add(
'GOAL_KEY',
SimpleActionState("reference_action",
TestAction,
goal_key='action_goal'))
Sequence.add(
'GOAL_SLOTS',
SimpleActionState("reference_action",
TestAction,
goal_slots=['goal']))
@cb_interface(input_keys=['action_result'],
output_keys=['action_result'])
def res_cb(ud, status, res):
ud.action_result.result = res.result + 1
Sequence.add(
'RESULT_CB',
SimpleActionState("reference_action",
TestAction,
goal=g1,
result_cb=res_cb))
asw = ActionServerWrapper('reference_action_sm',
TestAction,
sq,
succeeded_outcomes=['succeeded'],
aborted_outcomes=['aborted'],
preempted_outcomes=['preempted'],
expand_goal_slots=True)
asw.run_server()
ac = SimpleActionClient('reference_action_sm', TestAction)
ac.wait_for_server(rospy.Duration(30))
assert ac.send_goal_and_wait(
g1, rospy.Duration(30)) == GoalStatus.SUCCEEDED
assert ac.get_result().result == 1
def __init__(self,
server_name,
wrapped_container,
succeeded_outcomes=["succeeded"],
aborted_outcomes=["aborted"],
preempted_outcomes=["preempted"],
goal_key='action_goal',
feedback_key='action_feedback',
result_key='action_result',
goal_slots_map={},
feedback_slots_map={},
result_slots_map={},
expand_goal_slots=False,
pack_result_slots=False):
ActionServerWrapper.__init__(
self, server_name, ExecuteSmachStateMachineAction,
wrapped_container, succeeded_outcomes, aborted_outcomes,
preempted_outcomes, goal_key, feedback_key, result_key,
goal_slots_map, feedback_slots_map, result_slots_map,
expand_goal_slots, pack_result_slots)
def test_action_preemption(self):
"""Test action preemption"""
sq = Sequence(['succeeded', 'aborted', 'preempted'], 'succeeded')
class SlowRunningState(State):
def __init__(self):
State.__init__(self,
outcomes=['succeeded', 'aborted', 'preempted'])
def execute(self, ud):
start_time = rospy.Time.now()
while rospy.Time.now() - start_time < rospy.Duration(10):
rospy.sleep(0.05)
if self.preempt_requested():
self.service_preempt()
return 'preempted'
return 'succeeded'
with sq:
Sequence.add('PREEMPT_ME', SlowRunningState())
asw = ActionServerWrapper('preempt_action_sm',
TestAction,
sq,
succeeded_outcomes=['succeeded'],
aborted_outcomes=['aborted'],
preempted_outcomes=['preempted'])
asw.run_server()
ac = SimpleActionClient('preempt_action_sm', TestAction)
ac.wait_for_server(rospy.Duration(30))
ac.send_goal(g1)
rospy.sleep(5.0)
ac.cancel_goal()
start_time = rospy.Time.now()
while ac.get_state() == GoalStatus.ACTIVE and rospy.Time.now(
) - start_time < rospy.Duration(30):
rospy.sleep(0.5)
assert ac.get_state() == GoalStatus.PREEMPTED
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("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():
rospy.init_node('place_object_server')
# Construct state machine
sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['goal', 'feedback'],
output_keys=['feedback', 'result'])
with sm:
#add states to the container
smach.StateMachine.add('CHECK_IF_SHELF_INITIAL',
CheckIfLocationIsShelf(),
transitions={
'shelf': 'MOVE_ARM_TO_PRE_PLACE_SHELF',
'not_shelf': 'MOVE_ARM_TO_PRE_PLACE'
})
smach.StateMachine.add('MOVE_ARM_TO_PRE_PLACE_SHELF',
gms.move_arm("pre_place_shelf"),
transitions={
'succeeded': 'START_PLACE_POSE_SELECTOR',
'failed': 'MOVE_ARM_TO_PRE_PLACE_SHELF'
})
smach.StateMachine.add('MOVE_ARM_TO_PRE_PLACE',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'START_PLACE_POSE_SELECTOR',
'failed': 'MOVE_ARM_TO_PRE_PLACE'
})
smach.StateMachine.add(
'START_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/event_in',
'e_start')]),
transitions={'success': 'GET_POSE_TO_PLACE_OBJECT'})
smach.StateMachine.add(
'GET_POSE_TO_PLACE_OBJECT',
GetPoseToPlaceOject(
'/mcr_perception/place_pose_selector/platform_name',
'/mcr_perception/place_pose_selector/place_pose',
'/mcr_perception/place_pose_selector/event_out', 15.0),
transitions={
'succeeded': 'MOVE_ARM_TO_PLACE_OBJECT',
'failed': 'MOVE_ARM_TO_DEFAULT_PLACE'
})
smach.StateMachine.add('MOVE_ARM_TO_DEFAULT_PLACE',
gms.move_arm("15cm/pose4"),
transitions={
'succeeded': 'OPEN_GRIPPER',
'failed': 'MOVE_ARM_TO_DEFAULT_PLACE'
})
smach.StateMachine.add('MOVE_ARM_TO_PLACE_OBJECT',
gms.move_arm(),
transitions={
'succeeded': 'OPEN_GRIPPER',
'failed': 'OPEN_GRIPPER'
})
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'STOP_PLACE_POSE_SELECTOR'})
smach.StateMachine.add(
'STOP_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/event_in',
'e_stop')]),
transitions={'success': 'CHECK_IF_SHELF'})
smach.StateMachine.add('CHECK_IF_SHELF',
CheckIfLocationIsShelf(),
transitions={
'shelf': 'MOVE_ARM_TO_SAFE_1',
'not_shelf': 'MOVE_ARM_TO_NEUTRAL'
})
smach.StateMachine.add('MOVE_ARM_TO_NEUTRAL',
gms.move_arm("barrier_tape"),
transitions={
'succeeded': 'OVERALL_SUCCESS',
'failed': 'MOVE_ARM_TO_NEUTRAL'
})
smach.StateMachine.add('MOVE_ARM_TO_SAFE_1',
gms.move_arm("shelf_intermediate_2"),
transitions={
'succeeded': 'MOVE_ARM_TO_SAFE_2',
'failed': 'MOVE_ARM_TO_SAFE_1'
})
smach.StateMachine.add('MOVE_ARM_TO_SAFE_2',
gms.move_arm("pre_place_shelf"),
transitions={
'succeeded': 'MOVE_ARM_TO_SAFE_3',
'failed': 'MOVE_ARM_TO_SAFE_2'
})
smach.StateMachine.add('MOVE_ARM_TO_SAFE_3',
gms.move_arm("platform_left_pre"),
transitions={
'succeeded': 'OVERALL_SUCCESS',
'failed': 'MOVE_ARM_TO_SAFE_3'
})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = IntrospectionServer('place_object_smach_viewer', sm,
'/STAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='place_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():
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('perceive_cavity_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['perceive_cavity_goal'],
output_keys=['perceive_cavity_feedback', 'perceive_cavity_result'])
sm.userdata.next_arm_pose_index = 0
# Open the container
with sm:
# approach to platform
smach.StateMachine.add(
'GET_GOAL',
getGoal(),
transitions={'succeeded': 'PUBLISH_REFERENCE_FRAME_FOR_WBC'})
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
'PUBLISH_REFERENCE_FRAME_FOR_WBC',
gbs.send_event([('/static_transform_publisher_node/event_in',
'e_start')]),
transitions={'success': 'START_POSE_SELECTOR'})
# start the cavity pose selector to accumulate the poses
smach.StateMachine.add(
'START_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/cavity_pose_selector/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_straight_at_workspace_left',
'look_straight_at_workspace_right'
]),
transitions={
'succeeded': 'LOOK_AROUND', # next pose selected
'completed':
'SET_ACTION_LIB_SUCCESS', # we've run out of poses to select, which means we've gone through the list
'failed': 'SET_ACTION_LIB_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_CAVITIES',
'failed': 'LOOK_AROUND'
})
# trigger perception pipeline
smach.StateMachine.add('RECOGNIZE_CAVITIES',
gps.find_cavities(retries=1),
transitions={
'cavities_found': 'SELECT_NEXT_LOOK_POSE',
'no_cavities_found': 'SELECT_NEXT_LOOK_POSE'
})
# 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_cavity_smach_viewer', sm,
'/PERCEIVE_CAVITY_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='perceive_cavity_server',
action_spec=PerceiveLocationAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='perceive_cavity_goal',
feedback_key='perceive_cavity_feedback',
result_key='perceive_cavity_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
NewGoalState(ws),
transitions={'succeeded': 'MOVE_TO'})
smach.StateMachine.add('MOVE_TO',
SimpleActionState('/motion_controller/move_to',
MoveToAction,
goal_key='action_goal'),
transitions={'preempted': 'aborted',
'aborted': 'WALLFOLLOWER'})
smach.StateMachine.add('WALLFOLLOWER',
ws,
transitions={'leave_wall': 'MOVE_TO'})
# Wrap an action server around the state machine.
asw = ActionServerWrapper('~move_to',
MoveToAction,
wrapped_container=sm,
succeeded_outcomes=['succeeded'],
aborted_outcomes=['aborted, unreachable_goal'],
preempted_outcomes=['preempted'])
# Similarly to how it is done in MotionController, create a combination of
# listened and re-publisher to allow the user to send a goal as a message
# (not as an action service request).
action_goal_pub = rospy.Publisher('~move_to/goal', MoveToActionGoal)
def simple_goal_cb(target_pose):
rospy.loginfo('Received target pose through the "simple goal" topic. '
'Wrapping it in the action message and forwarding to '
'the server.')
msg = MoveToActionGoal()
msg.header.stamp = rospy.Time.now()
msg.goal.target_pose = target_pose
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()
smach.StateMachine.add('MOVE_TO',
SimpleActionState('/motion_controller/move_to',
MoveToAction,
goal_key='action_goal'),
transitions={
'preempted': 'aborted',
'aborted': 'WALLFOLLOWER'
})
smach.StateMachine.add('WALLFOLLOWER',
ws,
transitions={'leave_wall': 'MOVE_TO'})
# Wrap an action server around the state machine.
asw = ActionServerWrapper('~move_to',
MoveToAction,
wrapped_container=sm,
succeeded_outcomes=['succeeded'],
aborted_outcomes=['aborted, unreachable_goal'],
preempted_outcomes=['preempted'])
# Similarly to how it is done in MotionController, create a combination of
# listened and re-publisher to allow the user to send a goal as a message
# (not as an action service request).
action_goal_pub = rospy.Publisher('~move_to/goal',
MoveToActionGoal,
queue_size=100)
def simple_goal_cb(target_pose):
rospy.loginfo('Received target pose through the "simple goal" topic. '
'Wrapping it in the action message and forwarding to '
'the server.')
msg = MoveToActionGoal()
def main():
rospy.init_node('smach_example_state_machine')
# 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 -> 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': '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': 'SET_TOP_HOLE_CONFIG'})
# reconfigure top hole shifter
smach.StateMachine.add('SET_TOP_HOLE_CONFIG',
gbs.set_named_config('top_hole_shifter'),
transitions={
'success': 'GENERATE_TOP_HOLE_POSE',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'SET_TOP_HOLE_CONFIG'
})
# receive a pose and offset's up, result: top hole pose (another pose)
smach.StateMachine.add(
'GENERATE_TOP_HOLE_POSE',
send_event('/top_hole_pose_shifter/event_in', 'e_start'),
transitions={'success': 'WAIT_FOR_CREATE_TOP_HOLE_POSE'})
# wait for pose shifter to give response
smach.StateMachine.add('WAIT_FOR_CREATE_TOP_HOLE_POSE',
wait_for_event(
'/top_hole_pose_shifter/event_out',
timeout_duration=5.0),
transitions={
'success': 'SET_TOP_HOLE_PREGRASP_CONFIG',
'timeout': 'SET_ACTION_LIB_FAILURE',
'failure': 'SET_ACTION_LIB_FAILURE'
})
# reconfigure top hole pregrasp params
smach.StateMachine.add('SET_TOP_HOLE_PREGRASP_CONFIG',
gbs.set_named_config('top_hole_pregrasp'),
transitions={
'success': 'PLAN_ARM_MOTION',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'SET_TOP_HOLE_PREGRASP_CONFIG'
})
# based on a published pose, calls pregrasp planner to generate a graspable pose
smach.StateMachine.add(
'PLAN_ARM_MOTION',
send_event('/top_hole_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('/top_hole_pregrasp_planner_pipeline/event_out',
3.0),
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('/top_hole_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('/top_hole_pregrasp_planner_pipeline/event_in',
'e_stop'),
transitions={'success': 'SET_ACTION_LIB_FAILURE'})
# move arm to pregrasp planned pose
smach.StateMachine.add(
'MOVE_ARM_TO_OBJECT',
send_event('/top_hole_move_arm_planned/event_in', 'e_start'),
transitions={'success': 'WAIT_MOVE_ARM_TO_OBJECT'})
# wait for the arm motion to finish
smach.StateMachine.add('WAIT_MOVE_ARM_TO_OBJECT',
wait_for_event(
'/top_hole_move_arm_planned/event_out',
10.0),
transitions={
'success':
'STOP_MOVE_ARM_TO_OBJECT',
'timeout':
'STOP_MOVE_ARM_TO_OBJECT_WITH_FAILURE',
'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('/top_hole_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('/top_hole_move_arm_planned/event_in', 'e_stop'),
transitions={'success': 'CONFIGURE_POSES_TO_MOVE_DOWN'})
# reconfigure the pose
smach.StateMachine.add('CONFIGURE_POSES_TO_MOVE_DOWN',
gbs.set_named_config('move_down_shifter'),
transitions={
'success': 'PLAN_LINEAR_MOTION_DOWN',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'CONFIGURE_POSES_TO_MOVE_DOWN'
})
# linear motion down states
smach.StateMachine.add(
'PLAN_LINEAR_MOTION_DOWN',
send_event('/poses_to_move/event_in', 'e_start'),
transitions={'success': 'WAIT_FOR_PLAN_LINEAR_MOTION_DOWN'})
smach.StateMachine.add('WAIT_FOR_PLAN_LINEAR_MOTION_DOWN',
wait_for_event('/poses_to_move/event_out',
timeout_duration=5.0),
transitions={
'success': 'MOVE_TO_CARTESIAN_POSE_DOWN',
'failure': 'MOVE_ARM_TO_HOLD_FAILED',
'timeout': 'MOVE_ARM_TO_HOLD_FAILED'
})
# linear motion common states
smach.StateMachine.add(
'MOVE_TO_CARTESIAN_POSE_DOWN',
send_event('/cartesian_controller_demo/event_in', 'e_start'),
transitions={'success': 'WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_DOWN'})
smach.StateMachine.add('WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_DOWN',
wait_for_event(
'/cartesian_controller_demo/event_out',
timeout_duration=5.0),
transitions={
'success': 'OPEN_GRIPPER',
'timeout': 'MOVE_ARM_TO_HOLD_FAILED',
'failure': 'MOVE_ARM_TO_HOLD_FAILED'
})
# open gripper
smach.StateMachine.add('OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'OPEN_GRIPPER_AGAIN'})
# open gripper again (replace later on with padmaja stuff)
smach.StateMachine.add(
'OPEN_GRIPPER_AGAIN',
gms.control_gripper('open'),
transitions={'succeeded': 'CONFIGURE_POSES_TO_MOVE_UP'})
#reconfigure
smach.StateMachine.add('CONFIGURE_POSES_TO_MOVE_UP',
gbs.set_named_config('move_up_shifter'),
transitions={
'success': 'PLAN_LINEAR_MOTION_UP',
'failure': 'SET_ACTION_LIB_FAILURE',
'timeout': 'CONFIGURE_POSES_TO_MOVE_UP'
})
# linear motion up states
smach.StateMachine.add(
'PLAN_LINEAR_MOTION_UP',
send_event('/poses_to_move/event_in', 'e_start'),
transitions={'success': 'WAIT_FOR_PLAN_LINEAR_MOTION_UP'})
smach.StateMachine.add('WAIT_FOR_PLAN_LINEAR_MOTION_UP',
wait_for_event('/poses_to_move/event_out',
timeout_duration=5.0),
transitions={
'success': 'MOVE_TO_CARTESIAN_POSE_UP',
'timeout': 'MOVE_ARM_TO_HOLD',
'failure': 'MOVE_ARM_TO_HOLD'
})
# linear motion common states
smach.StateMachine.add(
'MOVE_TO_CARTESIAN_POSE_UP',
send_event('/cartesian_controller_demo/event_in', 'e_start'),
transitions={'success': 'WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_UP'})
smach.StateMachine.add('WAIT_RESULT_MOVE_TO_CARTESIAN_POSE_UP',
wait_for_event(
'/cartesian_controller_demo/event_out',
timeout_duration=5.0),
transitions={
'success': 'MOVE_ARM_TO_HOLD',
'timeout': 'MOVE_ARM_TO_HOLD',
'failure': 'MOVE_ARM_TO_HOLD'
})
# move arm to HOLD
smach.StateMachine.add('MOVE_ARM_TO_HOLD',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_SUCCESS',
'failed': 'MOVE_ARM_TO_HOLD'
})
# move arm to HOLD but provided that some old component failed
smach.StateMachine.add('MOVE_ARM_TO_HOLD_FAILED',
gms.move_arm("look_at_turntable"),
transitions={
'succeeded': 'SET_ACTION_LIB_FAILURE',
'failed': 'MOVE_ARM_TO_HOLD_FAILED'
})
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'})
# 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():
rospy.init_node("place_object_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal", "feedback"],
output_keys=["feedback", "result"],
)
with sm:
# add states to the container
smach.StateMachine.add(
"CHECK_IF_SHELF_INITIAL",
CheckIfLocationIsShelf(),
transitions={
"shelf": "MOVE_ARM_TO_PRE_PLACE_SHELF",
"not_shelf": "MOVE_ARM_TO_PRE_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_PRE_PLACE_SHELF",
gms.move_arm("pre_place_shelf"),
transitions={
"succeeded": "START_PLACE_POSE_SELECTOR",
"failed": "MOVE_ARM_TO_PRE_PLACE_SHELF",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_PRE_PLACE",
gms.move_arm("look_at_turntable"),
transitions={
"succeeded": "START_PLACE_POSE_SELECTOR",
"failed": "MOVE_ARM_TO_PRE_PLACE",
},
)
smach.StateMachine.add(
"START_PLACE_POSE_SELECTOR",
gbs.send_event([("/mcr_perception/place_pose_selector/event_in",
"e_start")]),
transitions={"success": "GET_POSE_TO_PLACE_OBJECT"},
)
smach.StateMachine.add(
"GET_POSE_TO_PLACE_OBJECT",
GetPoseToPlaceOject(
"/mcr_perception/place_pose_selector/platform_name",
"/mcr_perception/place_pose_selector/place_pose",
"/mcr_perception/place_pose_selector/event_out",
15.0,
),
transitions={
"succeeded": "MOVE_ARM_TO_PLACE_OBJECT",
"failed": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_DEFAULT_PLACE",
gms.move_arm("15cm/pose4"),
transitions={
"succeeded": "OPEN_GRIPPER",
"failed": "MOVE_ARM_TO_DEFAULT_PLACE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_PLACE_OBJECT",
gms.move_arm(),
transitions={
"succeeded": "OPEN_GRIPPER",
"failed": "OPEN_GRIPPER",
},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open"),
transitions={"succeeded": "STOP_PLACE_POSE_SELECTOR"},
)
smach.StateMachine.add(
"STOP_PLACE_POSE_SELECTOR",
gbs.send_event([("/mcr_perception/place_pose_selector/event_in",
"e_stop")]),
transitions={"success": "CHECK_IF_SHELF"},
)
smach.StateMachine.add(
"CHECK_IF_SHELF",
CheckIfLocationIsShelf(),
transitions={
"shelf": "MOVE_ARM_TO_SAFE_1",
"not_shelf": "MOVE_ARM_TO_NEUTRAL",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_NEUTRAL",
gms.move_arm("barrier_tape"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_NEUTRAL",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SAFE_1",
gms.move_arm("shelf_intermediate_2"),
transitions={
"succeeded": "MOVE_ARM_TO_SAFE_2",
"failed": "MOVE_ARM_TO_SAFE_1",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SAFE_2",
gms.move_arm("pre_place_shelf"),
transitions={
"succeeded": "MOVE_ARM_TO_SAFE_3",
"failed": "MOVE_ARM_TO_SAFE_2",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_SAFE_3",
gms.move_arm("platform_left_pre"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_SAFE_3",
},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer("place_object_smach_viewer", sm,
"/STAGE_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="place_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():
rospy.init_node('place_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['place_object_goal', 'place_object_feedback'],
output_keys=['place_object_feedback', 'place_object_result'])
with sm:
#add states to the container
smach.StateMachine.add(
'START_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/event_in',
'e_start')]),
transitions={'success': 'GET_POSE_TO_PLACE_OBJECT'})
smach.StateMachine.add(
'GET_POSE_TO_PLACE_OBJECT',
GetPoseToPlaceOject(
'/mcr_perception/place_pose_selector/platform_name',
'/mcr_perception/place_pose_selector/place_pose',
'/mcr_perception/place_pose_selector/event_out', 15.0),
transitions={
'succeeded': 'MOVE_ARM_TO_PLACE_OBJECT',
'failed': 'SET_ACTION_LIB_FAILURE'
})
smach.StateMachine.add('MOVE_ARM_TO_PLACE_OBJECT',
gms.move_arm(),
transitions={
'succeeded': 'OPEN_GRIPPER',
'failed': 'OPEN_GRIPPER'
})
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open'),
transitions={'succeeded': 'STOP_PLACE_POSE_SELECTOR'})
"""smach.StateMachine.add('MOVE_ARM_TO_HOLD', gms.move_arm("navigation"),
transitions={'succeeded':'STOP_PLACE_POSE_SELECTOR',
'failed':'MOVE_ARM_TO_HOLD'})"""
smach.StateMachine.add(
'STOP_PLACE_POSE_SELECTOR',
gbs.send_event([('/mcr_perception/place_pose_selector/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'})
# smach viewer
sis = smach_ros.IntrospectionServer('place_object_smach_viewer', sm,
'/STAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='place_object_server',
action_spec=PlaceObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='place_object_goal',
feedback_key='place_object_feedback',
result_key='place_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def main():
rospy.init_node('unstage_object_server')
# Construct state machine
sm = smach.StateMachine(outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['goal'],
output_keys=['feedback', 'result'])
with sm:
#add states to the container
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE',
gms.move_arm('stage_intermediate'),
transitions={
'succeeded':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_2',
'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE'
})
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_2',
gms.move_arm('stage_intermediate_2'),
transitions={
'succeeded': 'SETUP_MOVE_ARM_PRE_STAGE',
'failed': 'MOVE_ARM_TO_STAGE_INTERMEDIATE_2'
})
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_and_gripper('open_narrow'),
transitions={
'succeeded': 'SETUP_MOVE_ARM_STAGE',
'failed': 'MOVE_ARM_PRE_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'})
#TODO: verify if object is grasped or not
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_STAGE_INTERMEDIATE_2_FINAL',
'failed': 'MOVE_ARM_PRE_STAGE_AGAIN'
})
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL',
gms.move_arm('stage_intermediate_2'),
transitions={
'succeeded':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL',
'failed':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL'
})
smach.StateMachine.add('MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL',
gms.move_arm('stage_intermediate'),
transitions={
'succeeded': 'OVERALL_SUCCESS',
'failed':
'MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL'
})
# smach viewer
if rospy.get_param('~viewer_enabled', False):
sis = smach_ros.IntrospectionServer('unstage_object_smach_viewer', sm,
'/UNSTAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='unstage_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_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 tasker():
rospy.init_node('tasker')
wfg = WaitForGoalState() # We don't want multiple subscribers so we need one WaitForGoal state
runner = SMACHRunner(rgoap_subclasses)
## sub machines
sq_move_to_new_goal = Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded')
with sq_move_to_new_goal:
Sequence.add('WAIT_FOR_GOAL', wfg)
Sequence.add('MOVE_BASE_RGOAP', MoveBaseRGOAPState(runner))
sq_autonomous_rgoap = Sequence(outcomes=['preempted'],
connector_outcome='succeeded')
with sq_autonomous_rgoap:
Sequence.add('SLEEP_UNTIL_ENABLED', get_sleep_until_smach_enabled_smach())
Sequence.add('AUTONOMOUS_RGOAP', AutonomousRGOAPState(runner),
transitions={'succeeded':'SLEEP_UNTIL_ENABLED',
'aborted':'SLEEP'})
Sequence.add('SLEEP', SleepState(5),
transitions={'succeeded':'SLEEP_UNTIL_ENABLED'})
## tasker machine
sm_tasker = StateMachine(outcomes=['succeeded', 'aborted', 'preempted',
'field_error', 'undefined_task'],
input_keys=['task_goal'])
with sm_tasker:
## add all tasks to be available
# states using rgoap
StateMachine.add('MOVE_TO_NEW_GOAL_RGOAP', sq_move_to_new_goal)
StateMachine.add('INCREASE_AWARENESS_RGOAP', IncreaseAwarenessRGOAPState(runner))
StateMachine.add('AUTONOMOUS_RGOAP_CYCLE', sq_autonomous_rgoap)
StateMachine.add('AUTONOMOUS_RGOAP_SINGLE_GOAL', AutonomousRGOAPState(runner))
# states from uashh_smach
StateMachine.add('LOOK_AROUND', get_lookaround_smach())
StateMachine.add('GLIMPSE_AROUND', get_lookaround_smach(glimpse=True))
StateMachine.add('MOVE_ARM_CRAZY', get_lookaround_smach(crazy=True))
StateMachine.add('MOVE_TO_RANDOM_GOAL', get_random_goal_smach())
StateMachine.add('MOVE_TO_NEW_GOAL_AND_RETURN', task_go_and_return.get_go_and_return_smach())
StateMachine.add('PATROL_TO_NEW_GOAL', task_patrol.get_patrol_smach())
StateMachine.add('MOVE_AROUND', task_move_around.get_move_around_smach())
StateMachine.add('SLEEP_FIVE_SEC', SleepState(5))
## now the task receiver is created and automatically links to
## all task states added above
task_states_labels = sm_tasker.get_children().keys()
task_states_labels.sort() # sort alphabetically and then by _RGOAP
task_states_labels.sort(key=lambda label: '_RGOAP' in label, reverse=True)
task_receiver_transitions = {'undefined_outcome':'undefined_task'}
task_receiver_transitions.update({l:l for l in task_states_labels})
StateMachine.add('TASK_RECEIVER',
UserDataToOutcomeState(task_states_labels,
'task_goal',
lambda ud: ud.task_id),
task_receiver_transitions)
sm_tasker.set_initial_state(['TASK_RECEIVER'])
rospy.loginfo('tasker starting, available tasks: %s', ', '.join(task_states_labels))
pub = rospy.Publisher('/task/available_tasks', String, latch=True)
thread.start_new_thread(rostopic.publish_message, (pub, String, [', '.join(task_states_labels)], 1))
asw = ActionServerWrapper('activate_task', TaskActivationAction,
wrapped_container=sm_tasker,
succeeded_outcomes=['succeeded'],
aborted_outcomes=['aborted', 'undefined_task'],
preempted_outcomes=['preempted'],
goal_key='task_goal'
)
# Create and start the introspection server
sis = IntrospectionServer('smach_tasker_action', sm_tasker, '/SM_ROOT')
sis.start()
asw.run_server()
rospy.spin()
sis.stop()
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_cavity_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
sm.userdata.next_arm_pose_index = 0
# Open the container
with sm:
# approach to platform
smach.StateMachine.add(
"SETUP",
Setup(),
transitions={"succeeded": "PUBLISH_REFERENCE_FRAME_FOR_WBC"},
)
# generates a pose based on the previous string object topic received
smach.StateMachine.add(
"PUBLISH_REFERENCE_FRAME_FOR_WBC",
gbs.send_event([("/static_transform_publisher_node/event_in",
"e_start")]),
transitions={"success": "START_POSE_SELECTOR"},
)
# start the cavity pose selector to accumulate the poses
smach.StateMachine.add(
"START_POSE_SELECTOR",
gbs.send_event([("/mcr_perception/cavity_pose_selector/event_in",
"e_start")]),
transitions={"success": "LOOK_AROUND"},
)
# move arm to selected pose
smach.StateMachine.add(
"LOOK_AROUND",
gms.move_arm("look_at_workspace_from_near", blocking=True),
transitions={
"succeeded": "RECOGNIZE_CAVITIES",
"failed": "LOOK_AROUND",
},
)
# trigger perception pipeline
smach.StateMachine.add(
"RECOGNIZE_CAVITIES",
gps.find_cavities(retries=1),
transitions={
"cavities_found": "POPULATE_RESULT_WITH_CAVITIES",
"no_cavities_found": "OVERALL_FAILED",
},
)
# populate action server result with perceived objects
smach.StateMachine.add(
"POPULATE_RESULT_WITH_CAVITIES",
PopulateResultWithCavities(),
transitions={"succeeded": "OVERALL_SUCCESS"},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = IntrospectionServer("perceive_cavity_smach_viewer", sm,
"/PERCEIVE_CAVITY_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="perceive_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':'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('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('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('find_object')
sm = StateMachine(outcomes=['success', 'aborted', 'preempted'],
input_keys=['goal'],
output_keys=['result'])
with sm:
sm.userdata.feedback = FindObjectFeedback()
sm.userdata.result = FindObjectResult()
sm.userdata.min_confidence = 0.5
sm.userdata.recognised_objects = object_recognition_msgs.msg.RecognizedObjectArray(
)
sm.userdata.object_names = list()
# sm.userdata.objects_info = list()
sm.userdata.error_code = int()
sm.userdata.error_message = str()
sm.userdata.tf_listener = tf.TransformListener()
smach.StateMachine.add('Prepare',
Prepare(),
remapping={
'goal': 'goal',
'min_confidence': 'min_confidence',
'table_pose': 'table_pose',
'look_around': 'look_around',
'feedback': 'feedback',
'tf_listener': 'tf_listener'
},
transitions={'prepared': 'FindObject'})
sm_find_object = find_object_sm.createSM()
smach.StateMachine.add('FindObject',
sm_find_object,
remapping={
'table_pose': 'table_pose',
'look_around': 'look_around',
'min_confidence': 'min_confidence',
'recognised_objects': 'recognised_objects',
'object_names': 'object_names',
'error_message': 'error_message',
'error_code': 'error_code',
'tf_listener': 'tf_listener'
},
transitions={
'object_found': 'Finalise',
'no_objects_found': 'Finalise',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('Finalise',
Finalise(),
remapping={
'recognised_objects': 'recognised_objects',
'object_names': 'object_names',
'error_message': 'error_message',
'error_code': 'error_code',
'result': 'result',
'feedback': 'feedback',
'result': 'result'
},
transitions={'prepared': 'success'})
asw = ActionServerWrapper('find_object',
FindObjectAction,
wrapped_container=sm,
goal_key='goal',
feedback_key='feedback',
result_key='result',
succeeded_outcomes=['success'],
aborted_outcomes=['aborted'],
preempted_outcomes=['preempted'])
asw.run_server()
rospy.spin()
def main():
rospy.init_node('unstage_object_server')
# Construct state machine
sm = smach.StateMachine(
outcomes=['OVERALL_SUCCESS', 'OVERALL_FAILED'],
input_keys=['unstage_object_goal'],
output_keys=['unstage_object_feedback', 'unstage_object_result'])
with sm:
#add states to the container
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',
'failed': 'MOVE_ARM_PRE_STAGE'
})
smach.StateMachine.add(
'OPEN_GRIPPER',
gms.control_gripper('open_release'),
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': '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'})
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('SET_ACTION_LIB_SUCCESS',
SetActionLibResult(True),
transitions={'succeeded': 'OVERALL_SUCCESS'})
# smach viewer
sis = smach_ros.IntrospectionServer('unstage_object_smach_viewer', sm,
'/UNSTAGE_OBJECT_SMACH_VIEWER')
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(server_name='unstage_object_server',
action_spec=UnStageObjectAction,
wrapped_container=sm,
succeeded_outcomes=['OVERALL_SUCCESS'],
aborted_outcomes=['OVERALL_FAILED'],
preempted_outcomes=['PREEMPTED'],
goal_key='unstage_object_goal',
feedback_key='unstage_object_feedback',
result_key='unstage_object_result')
# Run the server in a background thread
asw.run_server()
rospy.spin()
def __init__(self, heritage):
# Creation of State Machine and definition of its outcomes
self.agent_sm = StateMachine(outcomes=['completed', 'failed'])
with self.agent_sm:
# Initialization of the dictionary containing every Action Service Wrapper
self.asw_dicc = {}
### ACTION SERVER ADVERTISING ###
# Add a state where drone does nothing but wait to receive a service order
StateMachine.add(
'action_server_advertiser',
CBState(self.action_server_advertiser_stcb,
cb_kwargs={
'heritage': heritage,
'asw_dicc': self.asw_dicc
}), {'completed': 'completed'})
# Every action service wrapper is similar following a structure defined by SMACH.
# Each wrapper is defined and added to
# To understand what they do, see callbacks below
### TAKE-OFF STATE MACHINE & WRAPPER ###
self.take_off_sm = StateMachine(outcomes=['completed', 'failed'],
input_keys=['action_goal'])
self.asw_dicc['take_off'] = ActionServerWrapper(
'/magna/GS_Agent_{}/take_off_command'.format(heritage.ID),
TakeOffAction,
self.take_off_sm, ['completed'], ['failed'], ['preempted'],
goal_key='action_goal',
result_key='action_result')
with self.take_off_sm:
StateMachine.add(
'take_off',
CBState(self.take_off_stcb,
input_keys=['action_goal'],
cb_kwargs={'heritage': heritage}),
{'completed': 'completed'})
### LAND STATE MACHINE & WRAPPER ###
self.land_sm = StateMachine(outcomes=['completed', 'failed'],
input_keys=['action_goal'])
self.asw_dicc['land'] = ActionServerWrapper(
'/magna/GS_Agent_{}/land_command'.format(heritage.ID),
LandAction,
self.land_sm, ['completed'], ['failed'], ['preempted'],
goal_key='action_goal',
result_key='action_result')
with self.land_sm:
StateMachine.add(
'land',
CBState(self.land_stcb,
input_keys=['action_goal'],
cb_kwargs={'heritage': heritage}),
{'completed': 'completed'})
### BASIC MOVE STATE MACHINE & WRAPPER ###
self.basic_move_sm = StateMachine(
outcomes=[
'completed', 'failed', 'collision', 'low_battery',
'GS_critical_event'
],
input_keys=['action_goal', 'action_result'])
self.asw_dicc['basic_move'] = ActionServerWrapper(
'/magna/GS_Agent_{}/basic_move_command'.format(heritage.ID),
BasicMoveAction,
self.basic_move_sm, ['completed'], ['failed'],
['collision', 'low_battery', 'GS_critical_event'],
goal_key='action_goal',
result_key='action_result')
with self.basic_move_sm:
StateMachine.add(
'basic_move',
CBState(self.basic_move_stcb,
input_keys=['action_goal', 'action_result'],
cb_kwargs={'heritage': heritage}), {
'completed': 'completed',
'collision': 'collision',
'low_battery': 'low_battery',
'GS_critical_event': 'GS_critical_event'
})
### SET MISSION STATE MACHINE & WRAPPER ###
self.set_mission_sm = StateMachine(
outcomes=[
'completed', 'failed', 'collision', 'low_battery',
'GS_critical_event'
],
input_keys=['action_goal', 'action_result'])
self.asw_dicc['set_mission'] = ActionServerWrapper(
'/magna/GS_Agent_{}/set_mission_command'.format(heritage.ID),
SetMissionAction,
self.set_mission_sm, ['completed'], ['failed'],
['collision', 'low_battery', 'GS_critical_event'],
goal_key='action_goal',
result_key='action_result')
with self.set_mission_sm:
StateMachine.add(
'set_mission',
CBState(self.set_mission_stcb,
input_keys=[
'action_goal', 'action_result',
'_preempt_requested'
],
cb_kwargs={'heritage': heritage}), {
'completed': 'completed',
'collision': 'collision',
'low_battery': 'low_battery',
'GS_critical_event': 'GS_critical_event'
})
### FOLLOW PATH STATE MACHINE & WRAPPER ###
self.follow_path_sm = StateMachine(
outcomes=[
'completed', 'failed', 'collision', 'low_battery',
'GS_critical_event'
],
input_keys=['action_goal', 'action_result'])
self.asw_dicc['follow_path'] = ActionServerWrapper(
'/magna/GS_Agent_{}/follow_path_command'.format(heritage.ID),
FollowPathAction,
self.follow_path_sm, ['completed'], ['failed'],
['collision', 'low_battery', 'GS_critical_event'],
goal_key='action_goal',
result_key='action_result')
with self.follow_path_sm:
StateMachine.add(
'follow_path',
CBState(self.follow_path_stcb,
input_keys=[
'action_goal', 'action_result',
'_preempt_requested'
],
cb_kwargs={'heritage': heritage}), {
'completed': 'completed',
'collision': 'collision',
'low_battery': 'low_battery',
'GS_critical_event': 'GS_critical_event'
})
# StateMachine.add('to_wp', self.follow_path_sm,
# {'completed':'action_server_advertiser'})
### FOLLOW Agent AD STATE MACHINE & WRAPPER###
self.follow_agent_ad_sm = StateMachine(
outcomes=[
'completed', 'failed', 'collision', 'low_battery',
'GS_critical_event'
],
input_keys=['action_goal', 'action_result'])
self.asw_dicc['follow_agent_ad'] = ActionServerWrapper(
'/magna/GS_Agent_{}/follow_agent_ad_command'.format(
heritage.ID),
FollowAgentADAction,
self.follow_agent_ad_sm, ['completed'], ['failed'],
['collision', 'low_battery', 'GS_critical_event'],
goal_key='action_goal',
result_key='action_result')
with self.follow_agent_ad_sm:
StateMachine.add(
'follow_agent_ad',
CBState(self.follow_agent_ad_stcb,
input_keys=[
'action_goal', 'action_result',
'_preempt_requested'
],
cb_kwargs={'heritage': heritage}), {
'completed': 'completed',
'collision': 'collision',
'low_battery': 'low_battery',
'GS_critical_event': 'GS_critical_event'
})
### FOLLOW Agent AP STATE MACHINE & WRAPPER###
self.follow_agent_ap_sm = StateMachine(
outcomes=[
'completed', 'failed', 'collision', 'low_battery',
'GS_critical_event'
],
input_keys=['action_goal', 'action_result'])
self.asw_dicc['follow_agent_ap'] = ActionServerWrapper(
'/magna/GS_Agent_{}/follow_agent_ap_command'.format(
heritage.ID),
FollowAgentAPAction,
self.follow_agent_ap_sm, ['completed'], ['failed'],
['collision', 'low_battery', 'GS_critical_event'],
goal_key='action_goal',
result_key='action_result')
with self.follow_agent_ap_sm:
StateMachine.add(
'follow_agent_ap',
CBState(self.follow_agent_ap_stcb,
input_keys=[
'action_goal', 'action_result',
'_preempt_requested'
],
cb_kwargs={'heritage': heritage}), {
'completed': 'completed',
'collision': 'collision',
'low_battery': 'low_battery',
'GS_critical_event': 'GS_critical_event'
})
if heritage.smach_view == True:
sis = smach_ros.IntrospectionServer(
'magna/Agent_{}_introspection'.format(heritage.ID),
self.agent_sm, '/Agent_{}'.format(heritage.ID))
sis.start()
def main():
rospy.init_node("stage_object_server")
# Construct state machine
sm = smach.StateMachine(
outcomes=["OVERALL_SUCCESS", "OVERALL_FAILED"],
input_keys=["goal"],
output_keys=["feedback", "result"],
)
with sm:
# add states to the container
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
gms.move_arm("stage_intermediate_2"),
transitions={
"succeeded": "SETUP_MOVE_ARM_PRE_STAGE",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_PRE_STAGE",
SetupMoveArm("pre"),
transitions={
"succeeded": "MOVE_ARM_PRE_STAGE",
"failed": "SETUP_MOVE_ARM_PRE_STAGE",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE",
gms.move_arm(),
transitions={
"succeeded": "SETUP_MOVE_ARM_STAGE",
"failed": "MOVE_ARM_PRE_STAGE",
},
)
smach.StateMachine.add(
"SETUP_MOVE_ARM_STAGE",
SetupMoveArm("final"),
transitions={
"succeeded": "MOVE_ARM_STAGE",
"failed": "SETUP_MOVE_ARM_STAGE",
},
)
smach.StateMachine.add(
"MOVE_ARM_STAGE",
gms.move_arm(),
transitions={
"succeeded": "OPEN_GRIPPER",
"failed": "MOVE_ARM_STAGE"
},
)
smach.StateMachine.add(
"OPEN_GRIPPER",
gms.control_gripper("open_narrow"),
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",
"failed": "SETUP_MOVE_ARM_PRE_STAGE_AGAIN",
},
)
smach.StateMachine.add(
"MOVE_ARM_PRE_STAGE_AGAIN",
gms.move_arm(blocking=False),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
"failed": "MOVE_ARM_PRE_STAGE_AGAIN",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
gms.move_arm("stage_intermediate_2"),
transitions={
"succeeded": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_2_FINAL",
},
)
smach.StateMachine.add(
"MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
gms.move_arm("stage_intermediate"),
transitions={
"succeeded": "OVERALL_SUCCESS",
"failed": "MOVE_ARM_TO_STAGE_INTERMEDIATE_FINAL",
},
)
# smach viewer
if rospy.get_param("~viewer_enabled", False):
sis = smach_ros.IntrospectionServer("stage_object_smach_viewer", sm,
"/STAGE_OBJECT_SMACH_VIEWER")
sis.start()
# Construct action server wrapper
asw = ActionServerWrapper(
server_name="stage_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()
