def main():
rospy.init_node('smach_ranking_controller')
bumper_sub = rospy.Subscriber("bumper", Bumper, bumper_cb)
cliff_sub = rospy.Subscriber("cliff_msg", Cliff, cliff_cb)
virtual_wall_sub = rospy.Subscriber("virtual_wall", Bool, virtual_wall_cb)
global vel_pub
vel_pub = rospy.Publisher("cmd_vel", Twist, queue_size=1)
rospy.on_shutdown(stop_cb)
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['aborted'])
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('FORWARD',
Forward(),
transitions={
'left_pressed': 'ROTATE_RIGHT_SEQ',
'right_pressed': 'ROTATE_LEFT_SEQ',
'both_pressed': 'ROTATE_LEFT_SEQ',
'virtual_wall_detected': 'ROTATE_LEFT_SEQ',
'cliff_left': 'ROTATE_RIGHT_SEQ',
'cliff_right': 'ROTATE_LEFT_SEQ'
})
smach.StateMachine.add('BACKWARD',
Backward(),
transitions={'done': 'FORWARD'})
right_sq = smach.Sequence(outcomes=['done', 'aborted'],
connector_outcome='done')
with right_sq:
smach.Sequence.add('BACKWARD', Backward())
smach.Sequence.add('ROTATE_RIGHT', RotateRight())
left_sq = smach.Sequence(outcomes=['done', 'aborted'],
connector_outcome='done')
with left_sq:
smach.Sequence.add('BACKWARD', Backward())
smach.Sequence.add('ROTATE_LEFT', RotateLeft())
smach.StateMachine.add('ROTATE_RIGHT_SEQ',
right_sq,
transitions={'done': 'FORWARD'})
smach.StateMachine.add('ROTATE_LEFT_SEQ',
left_sq,
transitions={'done': 'FORWARD'})
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('smach_introspection', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
_ = sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('smach_example_state_machine')
# Create a SMACH state machine
#sm = smach.StateMachine(outcomes = ['succeeded', 'aborted', 'preempted'])
sm = smach.Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded')
# Open the container
with sm:
'''
smach.StateMachine.add('FOO', Foo(), transitions={'succeeded':'BAR'})
smach.StateMachine.add('BAR', Bar(), transitions={'succeeded':'BAS'})
smach.StateMachine.add('BAS', Bas())
'''
smach.Sequence.add('FOO', Foo())
smach.Sequence.add('BAR', Bar())
smach.Sequence.add('BAS', Bas())
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
# Execute SMACH plan
outcome = sm.execute()
rospy.spin()
sis.stop()
def make_hydrophones(shared):
sm_travel = smach.StateMachine(['succeeded', 'preempted'])
with sm_travel:
smach.StateMachine.add('HYDROPHONES',
hydrophone_states.HydrophoneTravelState(shared, FREQ, FREQ_RANGE),
transitions={'failed': 'GO_AWAY'})
if constants.MODE == 'competition':
smach.StateMachine.add('GO_AWAY',
common_states.WaypointSeriesState(shared,
[lambda cur: cur.set_orientation(orientation_helpers.NORTH).turn_right_deg(22.5),
lambda cur: cur.forward(5)]),
transitions={'succeeded': 'HYDROPHONES'})
else:
smach.StateMachine.add('GO_AWAY',
common_states.WaypointSeriesState(shared,
[lambda cur: cur.set_orientation(orientation_helpers.NORTH).turn_left_deg(22.5),
lambda cur: cur.forward(5)]),
transitions={'succeeded': 'HYDROPHONES'})
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add('RETRACT',
subjugator_states.GasPoweredStickState(False))
smach.Sequence.add('DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(APPROACH_DEPTH)))
smach.Sequence.add('TRAVEL', sm_travel)
return sm
def main():
rospy.init_node('sekvenca1')
# Create a SMACH state machine
sm = smach.StateMachine(['succeeded','aborted','preempted'])
# Open the container
with sm:
# Wake up robot
smach.StateMachine.add('Wake_up', ServiceState('/pepper_robot/pose/wakeup', Empty),
transitions={'succeeded':'',
'aborted':'aborted',
'preempted':'preempted'})
sq = smach.Sequence(
outcomes = ['succeeded','aborted','preempted'],
connector_outcome = 'succeeded')
with sq:
smach.Sequence.add('MOVE_ARM_GRAB_PRE', MoveVerticalGripperPoseActionState())
smach.Sequence.add('MOVE_GRIPPER_OPEN', MoveGripperState(GRIPPER_MAX_WIDTH))
smach.Sequence.add('MOVE_ARM_GRAB', MoveVerticalGripperPoseActionState())
smach.Sequence.add('MOVE_GRIPPER_CLOSE', MoveGripperState(grab_width))
smach.Sequence.add('MOVE_ARM_GRAB_POST', MoveVerticalGripperPoseActionState())
# sekvenca1:
# tts
# pokret koji je sprzen u chorographu
#pokret koji je sekvenca za gripper
# kombinacija sinkronizacija tts+ pokret #govor preko servisa pozvati,
# Go to rest pose
smach.StateMachine.add('Rest', ServiceState('/pepper_robot/pose/rest', Empty),
transitions={'succeeded':'succeeded',
'aborted':'aborted',
'preempted':'preempted'})
def make_surface(shared):
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add(
'SURFACE',
common_states.WaypointState(shared, lambda cur: cur.depth(-.1)))
smach.Sequence.add('SLEEP', common_states.SleepState(5))
smach.Sequence.add(
'DOWN',
common_states.WaypointState(shared, lambda cur: cur.depth(.3)))
return sm
def create():
"""Creates the launch state machine.
:return The launch sequence machine object
"""
sm_launch = smach.Sequence(outcomes=['succeeded', 'preempted', 'failed'],
connector_outcome='succeeded')
with sm_launch:
smach.Sequence.add('WAIT FOR GAIT SERVER', WaitForGaitServerState())
return sm_launch
def make_grabber_drop(shared):
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared, lambda cur: cur.depth(2)))
smach.Sequence.add('OPEN_GRABBER',
subjugator_states.OpenGrabberState())
smach.Sequence.add(
'UP_DEPTH',
common_states.WaypointState(shared, lambda cur: cur.depth(.5)))
return sm
def make_starting_gate(shared):
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add('GO',
common_states.WaypointSeriesState(shared, [lambda cur: cur.depth(constants.PIPE_DEPTH),
lambda cur: cur.forward(10)]))
smach.Sequence.add('APPROACH',
common_states.VelocityState(shared,
numpy.array([constants.PIPE_SPEED, 0, 0])))
smach.Sequence.add('WAIT_PIPE',
legacy_vision_states.WaitForObjectsState(shared,
'find2_down_camera', 'pipe'),
transitions={'timeout': 'failed'})
return sm
def make_bins_return(shared):
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add('RETURN',
common_states.ReturnToWaypointState(shared, 'bins'))
smach.Sequence.add('WAIT_BINS',
legacy_vision_states.WaitForObjectsState(
shared,
'find2_down_camera',
'bins/all',
timeout=5),
transitions={'timeout': 'failed'})
smach.Sequence.add(
'CENTER_ALL',
legacy_vision_states.CenterObjectState(shared,
'find2_down_camera'))
smach.Sequence.add(
'ALIGN_ALL',
legacy_vision_states.AlignObjectState(shared,
'find2_down_camera',
body_vec_align=[0, 1, 0]))
smach.Sequence.add(
'CENTER_ALL_2',
legacy_vision_states.CenterObjectState(shared,
'find2_down_camera'))
smach.Sequence.add('TURN_DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(constants.PIPE_DEPTH)\
.turn_left_deg(90)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(
shared, numpy.array([constants.PIPE_SPEED, 0, 0])))
smach.Sequence.add('WAIT_PIPE',
legacy_vision_states.WaitForObjectsState(
shared, 'find2_down_camera', 'pipe'),
transitions={'timeout': 'failed'})
selector = legacy_vision_states.select_by_body_direction(
[0, 1 if constants.MODE == 'competition' else -1, 0])
smach.Sequence.add(
'CENTER_PIPE',
legacy_vision_states.CenterObjectState(shared, 'find2_down_camera',
selector))
smach.Sequence.add(
'TURN_HELP_PIPE',
common_states.WaypointState(
shared, lambda cur: cur.turn_left_deg(45) if constants.MODE ==
'competition' else cur.turn_right_deg(45)))
return sm
def main():
rospy.init_node("smach_example_sequence")
# Create a SMACH state machine
sq = smach.Sequence(outcomes=["succeeded"], connector_outcome="done")
# Open the container
with sq:
# Add states to the container
smach.Sequence.add("FOO", ExampleState())
smach.Sequence.add("BAR", ExampleState())
smach.Sequence.add("BAZ", ExampleState(), {"done": "succeeded"})
# Execute SMACH plan
outcome = sq.execute()
def main():
rospy.init_node('smach_example_sequence')
# Create a SMACH state machine
sq = smach.Sequence(outcomes=['succeeded'], connector_outcome='done')
# Open the container
with sq:
# Add states to the container
smach.Sequence.add('FOO', ExampleState())
smach.Sequence.add('BAR', ExampleState())
smach.Sequence.add('BAZ', ExampleState(), {'done': 'succeeded'})
# Execute SMACH plan
outcome = sq.execute()
def _test_look_around():
rospy.init_node('smach')
print POSES
print POSES_GLIMPSE
print POSES_CRAZY
sq = smach.Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded')
with sq:
smach.Sequence.add('LOOK_AROUND', get_lookaround_smach(util.SleepState(1)))
smach.Sequence.add('GLIMPSE_AROUND', get_lookaround_smach(util.SleepState(1),
glimpse=True))
util.execute_smach_container(sq, enable_introspection=True)
def get_lookaround_smach(interjacent_state=None, glimpse=False, crazy=False):
"""if the interjacent state is not omitted its outcomes must include
'succeeded' but can also be 'aborted' or 'preempted'.
"""
sq = smach.Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded')
poses = POSES_GLIMPSE if glimpse else POSES_CRAZY if crazy else POSES
with sq:
for i in range(len(poses)):
sq.add('LOOKAROUND_MOVE_ARM_%d' % i,
move_arm.get_move_arm_to_joints_positions_state(poses[i]))
if interjacent_state is not None:
sq.add('LOOKAROUND_INTERJACENT_STATE_%d' % i, interjacent_state)
return sq
def make_hydrophones_close(shared):
sm = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm:
smach.Sequence.add('DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(1.5)))
smach.Sequence.add('HYDROPHONES_TRAVEL',
hydrophone_states.HydrophoneTravelState(shared, FREQ, FREQ_RANGE),
transitions={'failed': 'HYDROPHONES_TRAVEL'})
smach.Sequence.add('HYDROPHONES_APPROACH',
hydrophone_states.HydrophoneApproachState(shared, FREQ, FREQ_RANGE),
transitions={'failed': 'succeeded'}) # Ensure we drop
smach.Sequence.add('BACKUP',
common_states.WaypointState(shared,
lambda cur: cur.backward(.3)))
return sm
def gen_sm_vis_exec(visualize=True, confirm=True, execute=True):
sm = smach.Sequence(outcomes=['succeeded', 'preempted', 'aborted'],
connector_outcome='succeeded')
sm.register_input_keys(['trajectory'])
msg = """Press key: [y = continue, r = repeat, a = abort]: """
qe = GenericAskState(msg,
answers={
"y": "succeeded",
"a": "aborted",
"r": "repeat"
})
with sm:
if visualize:
Sequence.add("VISUALIZE", TrajectoryVisualizeState())
if confirm and visualize:
Sequence.add("Q_EXECUTE", qe, transitions={'repeat': 'VISUALIZE'})
if execute:
Sequence.add("EXECUTE", TrajectoryExecuteState())
return sm
def main():
rospy.init_node('smach_example_sequence')
# Create a SMACH state machine
sq = smach.Sequence(outcomes=['succeeded'], connector_outcome='done')
# Open the container
with sq:
# Add states to the container
smach.Sequence.add('FOO', ExampleState())
smach.Sequence.add('BAR', ExampleState())
smach.Sequence.add('BAZ', ExampleState(), {'done': 'succeeded'})
# Execute SMACH plan
sis = smach_ros.IntrospectionServer('server_name', sq, '/SM_ROOT')
sis.start()
## Execute SMACH plan
outcome = sq.execute()
rospy.spin()
sis.stop()
def explore_house_sm():
"""
Explore house SM:
- segment map into rooms and plan visit sequence
- iterate over all rooms and explore following the planned sequence
"""
# segment house into rooms and plan visit sequence
plan_room_seq_sm = smach.Sequence(
outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded',
input_keys=[
'map_image', 'map_resolution', 'map_origin', 'robot_radius',
'segmented_map', 'room_information_in_meter',
'room_information_in_pixel'
],
output_keys=['room_sequence'])
with plan_room_seq_sm:
smach.Sequence.add('GET_ROBOT_POSE', GetRobotPose())
smach.Sequence.add('PLAN_ROOM_SEQUENCE',
PlanRoomSequence(),
remapping={
'robot_start_coordinate': 'robot_pose',
'input_map': 'map_image'
})
# explore a single room
explore_1_room_sm = smach.Sequence(
outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded',
input_keys=[
'map_image', 'map_resolution', 'map_origin', 'robot_radius',
'segmented_map', 'room_number', 'room_information_in_meter'
])
with explore_1_room_sm:
smach.Sequence.add('GET_ROBOT_POSE', GetRobotPose())
smach.Sequence.add('PLAN_ROOM_EXPL',
PlanRoomExploration(),
transitions={
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.Sequence.add('POSES_AS_PATH',
PosesAsPath(),
remapping={'poses': 'coverage_path_pose_stamped'})
smach.Sequence.add(
'GOTO_START_POSE',
GoToPose(
dist_tolerance=cfg.LOOSE_DIST_TOLERANCE, # just close enough
angle_tolerance=cfg.INF_ANGLE_TOLERANCE), # ignore orientation
transitions={
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'target_pose': 'start_pose'})
smach.Sequence.add('TRAVERSE_POSES', ExeSparsePath())
# iterate over all rooms and explore following the planned sequence
explore_house_it = smach.Iterator(
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[
'map_image', 'map_resolution', 'map_origin', 'robot_radius',
'segmented_map', 'room_sequence', 'room_information_in_meter'
],
output_keys=[],
it=lambda: sm.userdata.
room_sequence, # must be a lambda because we destroy the list TODO ehhh???
it_label='room_number',
exhausted_outcome='succeeded')
with explore_house_it:
smach.Iterator.set_contained_state(
'EXPLORE_1_ROOM',
explore_1_room_sm,
loop_outcomes=['succeeded', 'aborted'])
# Full SM: plan rooms visit sequence and explore each room in turn
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with sm:
smach.StateMachine.add('HAVE_SEGMENTED_MAP',
UDHasKey('segmented_map'),
transitions={
'true': 'PLAN_ROOM_SEQ',
'false': 'SEGMENT_ROOMS'
})
smach.StateMachine.add('SEGMENT_ROOMS',
SegmentRooms(),
transitions={
'succeeded': 'PLAN_ROOM_SEQ',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('PLAN_ROOM_SEQ',
plan_room_seq_sm,
transitions={
'succeeded': 'EXPLORE_HOUSE',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('EXPLORE_HOUSE',
explore_house_it,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
return sm
def make_manipulation(shared):
traj = PoseEditor.from_PoseTwistStamped_topic('/trajectory')
manipulation_desc = TargetDesc()
manipulation_desc.type = TargetDesc.TYPE_OBJECT
manipulation_desc.object_filename = roslib.packages.resource_file(
'auvsi_robosub', 'models', '2013/manipulation.obj')
manipulation_desc.prior_distribution.pose.orientation = Quaternion(
*traj.turn_left_deg(180).orientation)
manipulation_desc.disallow_yawing = True
manipulation_desc.min_dist = BOARD_DIST - 1
manipulation_desc.max_dist = BOARD_DIST + 3
wheel = TargetDesc()
wheel.type = TargetDesc.TYPE_OBJECT
wheel.object_filename = roslib.packages.resource_file(
'auvsi_robosub', 'models', '2013/manipulation_wheel.obj')
wheel.prior_distribution.pose.orientation = Quaternion(
*traj.turn_left_deg(180).orientation)
cov = numpy.zeros((6, 6))
a = numpy.array([traj.forward_vector]).T * 100
cov[3:, 3:] += a.dot(a.T)
wheel.prior_distribution.covariance = cov.flatten()
wheel.min_dist = WHEEL_DIST * .9
wheel.max_dist = WHEEL_DIST / .9
wheel.allow_rolling = True
wheel.disallow_yawing = True
# Create a SMACH state machine
sm = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'],
'succeeded')
# Open the container
with sm:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared, lambda cur: cur.depth(1.5)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.2, 0, 0])))
smach.Sequence.add(
'WAIT_MANIPULATION',
object_finder_states.WaitForObjectsState(
shared, 'find_forward', lambda: [manipulation_desc], .95))
smach.Sequence.add(
'STOP', common_states.VelocityState(shared, numpy.array([0, 0,
0])))
smach.Sequence.add('SLEEP', common_states.SleepState(5))
smach.Sequence.add(
'APPROACH_MANIPULATION',
object_finder_states.ApproachObjectState(shared, 'find_forward',
'forward_camera',
BOARD_DIST))
smach.Sequence.add(
'OPEN_LOOP_FORWARD',
common_states.WaypointState(
shared,
lambda cur: cur.forward(BOARD_DIST - WHEEL_DIST).left(.5)))
smach.Sequence.add(
'WAIT_WHEEL',
object_finder_states.WaitForObjectsState(shared, 'find_forward',
lambda: [wheel], .99))
smach.Sequence.add('WAIT_WHEEL_MORE', common_states.SleepState(5))
smach.Sequence.add(
'APPROACH_WHEEL',
object_finder_states.ApproachObjectState(shared, 'find_forward',
'forward_camera',
WHEEL_DIST))
smach.Sequence.add('EXTEND',
subjugator_states.GasPoweredStickState(True))
smach.Sequence.add('OPEN_LOOP_FORWARD2',
common_states.WaypointState(shared,
lambda cur: cur.forward(WHEEL_DIST-TURN_DIST)\
.relative([0, .06, .06])))
smach.Sequence.add(
'TURN',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.down(.2), lambda cur: cur.right(.2),
lambda cur: cur.up(.2), lambda cur: cur.left(.2),
lambda cur: cur.down(.2), lambda cur: cur.right(.2)
]))
smach.Sequence.add('RETRACT',
subjugator_states.GasPoweredStickState(False))
smach.Sequence.add(
'BACKUP',
common_states.WaypointState(shared, lambda cur: cur.backward(1)))
return sm
def make_bins(shared):
sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_approach:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(CENTER_DEPTH)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.2, 0, 0])))
smach.Sequence.add('WAIT_ALL',
legacy_vision_states.WaitForObjectsState(
shared, 'find2_down_camera', 'bins/all'),
transitions={'timeout': 'failed'})
smach.Sequence.add('EXTRA_FORWARD', common_states.SleepState(2))
smach.Sequence.add(
'RIGHT_TWEAK',
common_states.WaypointState(shared,
lambda cur: cur.turn_right_deg(25)))
sm_center = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_center:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(CENTER_DEPTH)))
smach.Sequence.add('WAIT_ALL',
legacy_vision_states.WaitForObjectsState(
shared,
'find2_down_camera',
'bins/all',
timeout=10),
transitions={'timeout': 'failed'})
smach.Sequence.add(
'CENTER_ALL',
legacy_vision_states.CenterObjectState(shared,
'find2_down_camera'))
smach.Sequence.add(
'ALIGN_ALL',
legacy_vision_states.AlignObjectState(shared,
'find2_down_camera',
body_vec_align=[0, 1, 0]))
smach.Sequence.add(
'CENTER_ALL_2',
legacy_vision_states.CenterObjectState(shared,
'find2_down_camera'))
smach.Sequence.add('SAVE_POS', common_states.SaveWaypointState('bins'))
sm_drops = []
for bin_string in DROP_ORDER:
sm_drop = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
sm_drops.append(sm_drop)
with sm_drop:
# this could be faster if CenterObjectState let you descend at the same time
smach.Sequence.add('WAIT_SINGLE',
legacy_vision_states.WaitForObjectsState(
shared,
'find2_down_camera',
'bins/single',
timeout=10),
transitions={'timeout': 'failed'})
selector = select_image_text_or_most_central(bin_string)
smach.Sequence.add(
'APPROACH_SINGLE',
legacy_vision_states.CenterApproachObjectState(
shared,
'find2_down_camera',
desired_scale=9000,
selector=selector))
smach.Sequence.add(
'DOWN',
common_states.WaypointState(
shared, lambda cur: cur.depth(3).backward(.15)))
smach.Sequence.add('SLEEP', common_states.SleepState(2))
smach.Sequence.add('DROP', subjugator_states.DropBallState())
smach.Sequence.add('SLEEP2', common_states.SleepState(2))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('APPROACH',
sm_approach,
transitions={'succeeded': 'CENTER_1'})
smach.StateMachine.add('CENTER_1',
sm_center,
transitions={
'succeeded': 'DROP_1',
'failed': 'APPROACH'
})
smach.StateMachine.add('DROP_1',
sm_drops[0],
transitions={
'succeeded': 'CENTER_2',
'failed': 'RETRY_CENTER_1'
})
smach.StateMachine.add('RETRY_CENTER_1',
common_states.CounterState(1),
transitions={
'succeeded': 'CENTER_1',
'exceeded': 'CENTER_2'
})
smach.StateMachine.add('CENTER_2',
sm_center,
transitions={'succeeded': 'DROP_2'})
smach.StateMachine.add('DROP_2',
sm_drops[1],
transitions={
'succeeded': 'succeeded',
'failed': 'RETRY_CENTER_2'
})
smach.StateMachine.add('RETRY_CENTER_2',
common_states.CounterState(1),
transitions={
'succeeded': 'CENTER_2',
'exceeded': 'succeeded'
})
return sm
def make_shooter(shared):
# Create a SMACH state machine
sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_approach:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared, lambda cur: cur.depth(DEPTH)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.1, 0, 0])))
sm_shoots = []
for color, shooter in zip(COLORS, ['left', 'right']):
sm_shoot = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
sm_shoots.append(sm_shoot)
with sm_shoot:
# smach.Sequence.add('WAIT_SHOOTER',
# legacy_vision_states.WaitForObjectsState(shared, 'find2_forward_camera',
# '/'.join(['shooter', color, 'box'])),
# transitions={'timeout': 'failed'})
# smach.Sequence.add('APPROACH_SHOOTER',
# legacy_vision_states.CenterApproachObjectState(shared, 'find2_forward_camera',
# desired_scale=BOARD_SCALE))
smach.Sequence.add('WAIT_HEXAGON',
legacy_vision_states.WaitForObjectsState(
shared,
'find2_forward_camera',
'/'.join(['shooter_flood', color, SIZE]),
timeout=15),
transitions={'timeout': 'failed'})
smach.Sequence.add(
'APPROACH_HEXAGON',
legacy_vision_states.CenterApproachObjectState(
shared,
'find2_forward_camera',
desired_scale=HEXAGON_SCALE,
gain=.5))
if shooter == 'left':
smach.Sequence.add('OPEN_LOOP_FORWARD',
common_states.WaypointState(shared,
lambda cur: cur.forward(ALIGN_FORWARD)\
.right(ALIGN_STRAFE)\
.up(ALIGN_UP)))
else:
smach.Sequence.add('OPEN_LOOP_FORWARD',
common_states.WaypointState(shared,
lambda cur: cur.forward(ALIGN_FORWARD)\
.left(ALIGN_STRAFE)\
.up(ALIGN_UP)))
smach.Sequence.add('SLEEP', common_states.SleepState(3))
smach.Sequence.add('SHOOT',
subjugator_states.ShootTorpedoState(shooter))
smach.Sequence.add('SLEEP2', common_states.SleepState(3))
sm_retreat = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_retreat:
smach.Sequence.add(
'RETREAT',
common_states.VelocityState(shared, numpy.array([-.2, 0, 0])))
smach.Sequence.add('WAIT', common_states.SleepState(4))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('APPROACH_SHOOT_1',
sm_approach,
transitions={'succeeded': 'SHOOT_1'})
smach.StateMachine.add('RETREAT_SHOOT_1',
sm_retreat,
transitions={'succeeded': 'SHOOT_1'})
smach.StateMachine.add('SHOOT_1',
sm_shoots[0],
transitions={
'succeeded': 'RETREAT_SHOOT_2',
'failed': 'RETREAT_SHOOT_1'
})
smach.StateMachine.add('RETREAT_SHOOT_2',
sm_retreat,
transitions={'succeeded': 'SHOOT_2'})
smach.StateMachine.add('SHOOT_2',
sm_shoots[1],
transitions={
'succeeded': 'DONE_BACKUP',
'failed': 'RETREAT_SHOOT_2'
})
smach.StateMachine.add('DONE_BACKUP',
common_states.WaypointState(
shared, lambda cur: cur.backward(.5)),
transitions={'succeeded': 'succeeded'})
return sm
def main():
rospy.init_node('commander')
com = smach.StateMachine(outcomes=['succeeded','aborted','preempted'])
with com:
smach.StateMachine.add('ARM',
smach_ros.SimpleActionState('arm_action',
DoArmAction,
goal=DoArmGoal(arm_cmd=DoArmGoal.ARM)),
{'succeeded':'WAYPOINTS',
'aborted':'aborted'})
waypoint_sequence = smach.Sequence(
outcomes=['succeeded','aborted','preempted'],
connector_outcome='succeeded')
with waypoint_sequence:
smach.Sequence.add('TAKEOFF',smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP0']))
smach.Sequence.add('WP1',smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP1']))
smach.Sequence.add('WP2',smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP2']))
smach.Sequence.add('WP3',smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP3']))
smach.Sequence.add('WP4',smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP0']))
smach.StateMachine.add('WAYPOINTS',waypoint_sequence,
transitions={'succeeded':'TRACK',
'preempted':'preempted',
'aborted':'aborted'})
track_machine = smach.Concurrence(outcomes=['locked','notready'],
default_outcome='notready',
outcome_map={'locked':
{ 'COV_MONITOR':'invalid',
'SEEK':'succeeded'}})
with track_machine:
smach.Concurrence.add('SEEK',smach_ros.SimpleActionState('track_action',
DoTrackAction,
goal=DoTrackGoal(alt_sp=15)))
smach.Concurrence.add('COV_MONITOR',smach_ros.MonitorState('odometry/filtered',
Odometry,
covariance_cb))
smach.StateMachine.add('TRACK',track_machine,
transitions={'notready':'TRACK',
'locked':'LAND'})
smach.StateMachine.add('LAND',smach_ros.SimpleActionState('land_action',
DoLandAction),
{'succeeded':'succeeded',
'aborted':'aborted'})
smach_introspect = smach_ros.IntrospectionServer('commander_viewer',com,'COMMANDER')
smach_introspect.start()
outcome = com.execute()
smach_introspect.stop()
rospy.loginfo('State Machine completed with status {0}'.format(outcome))
def createSequence(self):
return smach.Sequence(outcomes=[
'TASK_COMPLETED', 'TASK_INTERRUPTED', 'TASK_FAILED',
'TASK_TIMEOUT', 'MISSION_COMPLETED'
],
connector_outcome='TASK_COMPLETED')
def make_grabber(shared):
sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_approach:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(APPROACH_DEPTH)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.2, 0, 0])))
smach.Sequence.add('WAIT_PIZZA',
legacy_vision_states.WaitForObjectsState(
shared,
'find2_down_camera',
'wreath',
timeout=9999),
transitions={'timeout': 'failed'})
sm_descend_grab = smach.Sequence(
['succeeded', 'failed', 'empty', 'preempted'], 'succeeded')
with sm_descend_grab:
smach.Sequence.add(
'CENTER_APPROACH_PIZZA',
legacy_vision_states.CenterApproachObjectState(shared,
'find2_down_camera',
desired_scale=90,
gain=1))
smach.Sequence.add(
'ALIGN_PIZZA',
legacy_vision_states.AlignObjectState(shared, 'find2_down_camera'))
smach.Sequence.add(
'CENTER_APPROACH_PIZZA2',
legacy_vision_states.CenterApproachObjectState(shared,
'find2_down_camera',
desired_scale=120,
gain=.5))
smach.Sequence.add('OPEN_GRABBER',
subjugator_states.OpenGrabberState())
smach.Sequence.add(
'DOWN',
common_states.WaypointState(shared, lambda cur: cur.down(1.1)))
smach.Sequence.add('WAIT', common_states.SleepState(1))
smach.Sequence.add('CLOSE_GRABBER',
subjugator_states.CloseGrabberState())
sm_extra_grab = smach.Sequence(['succeeded', 'empty', 'preempted'],
'succeeded')
with sm_extra_grab:
smach.Sequence.add('OPEN_GRABBER',
subjugator_states.OpenGrabberState())
smach.Sequence.add(
'DOWN',
common_states.WaypointState(shared, lambda cur: cur.down(.1)))
smach.Sequence.add('WAIT', common_states.SleepState(1))
smach.Sequence.add('CLOSE_GRABBER',
subjugator_states.CloseGrabberState())
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('APPROACH',
sm_approach,
transitions={'succeeded': 'DESCEND_GRAB'})
smach.StateMachine.add('DESCEND_GRAB',
sm_descend_grab,
transitions={
'succeeded': 'UP',
'failed': 'APPROACH',
'empty': 'EXTRA_GRAB'
})
smach.StateMachine.add('EXTRA_GRAB',
sm_extra_grab,
transitions={
'succeeded': 'UP',
'empty': 'RETRY_GRAB_COUNTER'
})
smach.StateMachine.add(
'UP', common_states.WaypointState(shared,
lambda cur: cur.depth(.3)))
smach.StateMachine.add('RETRY_GRAB_COUNTER',
common_states.CounterState(1),
transitions={
'succeeded': 'DESCEND_GRAB',
'exceeded': 'APPROACH'
})
return sm
def make_shooter(shared):
# Create a SMACH state machine
sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_approach:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared, lambda cur: cur.depth(DEPTH)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.4, 0, 0])))
smach.Sequence.add('WAIT_SHOOTER',
object_finder_states.WaitForObjectsState(
shared, 'find_forward', shooter_desc_cb, .95),
transitions={'timeout': 'failed'})
sm_shoots = []
for color, shooter in zip(COLORS, ['left', 'right']):
sm_shoot = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
sm_shoots.append(sm_shoot)
with sm_shoot:
smach.Sequence.add(
'APPROACH_SHOOTER',
object_finder_states.ApproachObjectState(shared,
'find_forward',
'forward_camera',
BOARD_DIST,
marker=color))
smach.Sequence.add(
'OPEN_LOOP_FORWARD',
common_states.WaypointState(
shared,
lambda cur: cur.forward(BOARD_DIST - HEXAGON_DIST)))
smach.Sequence.add('WAIT_HEXAGON',
object_finder_states.WaitForObjectsState(
shared, 'find_forward',
lambda: hexagon_desc_cb(SIZE, color), .99),
transitions={'timeout': 'failed'})
smach.Sequence.add(
'APPROACH_HEXAGON',
object_finder_states.ApproachObjectState(
shared, 'find_forward', 'forward_camera', HEXAGON_DIST))
smach.Sequence.add('OPEN_LOOP_FORWARD2',
common_states.WaypointState(shared,
lambda cur: cur.forward(HEXAGON_DIST-SHOOT_DIST)\
.relative([0, .12, .18]
if shooter == 'left' else
[0, -.12, .18])))
smach.Sequence.add('SHOOT',
subjugator_states.ShootTorpedoState(shooter))
sm_retreat = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_retreat:
smach.Sequence.add(
'RETREAT',
common_states.WaypointState(
shared, lambda cur: cur.backward(BOARD_DIST + 1).depth(DEPTH)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.2, 0, 0])))
smach.Sequence.add('WAIT_SHOOTER',
object_finder_states.WaitForObjectsState(
shared, 'find_forward', shooter_desc_cb, .9),
transitions={'timeout': 'failed'})
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('APPROACH_SHOOT_1',
sm_approach,
transitions={'succeeded': 'SHOOT_1'})
smach.StateMachine.add('RETREAT_SHOOT_1',
sm_retreat,
transitions={'succeeded': 'SHOOT_1'})
smach.StateMachine.add('SHOOT_1',
sm_shoots[0],
transitions={
'succeeded': 'RETREAT_SHOOT_2',
'failed': 'RETREAT_SHOOT_1'
})
smach.StateMachine.add('RETREAT_SHOOT_2',
sm_retreat,
transitions={'succeeded': 'SHOOT_2'})
smach.StateMachine.add('SHOOT_2',
sm_shoots[1],
transitions={
'succeeded': 'DONE_BACKUP',
'failed': 'RETREAT_SHOOT_2'
})
smach.StateMachine.add('DONE_BACKUP',
common_states.WaypointState(
shared, lambda cur: cur.backward(.5)),
transitions={'succeeded': 'succeeded'})
return sm
def main():
rospy.init_node('sekvenca1')
# Create a SMACH state machine
sm = smach.StateMachine(['succeeded','aborted','preempted'])
# Open the container
with sm:
# Wake up robot
smach.StateMachine.add('Wake_up', ServiceState('/pepper_robot/pose/wakeup', Empty),
transitions={'succeeded':'',
'aborted':'aborted',
'preempted':'preempted'})
# Say word via tts service
smach.StateMachine.add('tts', ServiceState('/tts_speech', SetString, 'First, I will show you all the poses'),
transitions={'succeeded':'vocabulary',
'aborted':'aborted',
'preempted':'preempted'})
sq = smach.Sequence(
outcomes = ['succeeded','aborted','preempted'],
connector_outcome = 'succeeded')
with sq:
smach.Sequence.add('MOVE_ARM_GRAB_PRE', MoveVerticalGripperPoseActionState())
smach.Sequence.add('MOVE_GRIPPER_OPEN', MoveGripperState(GRIPPER_MAX_WIDTH))
smach.Sequence.add('MOVE_ARM_GRAB', MoveVerticalGripperPoseActionState())
smach.Sequence.add('MOVE_GRIPPER_CLOSE', MoveGripperState(grab_width))
smach.Sequence.add('MOVE_ARM_GRAB_POST', MoveVerticalGripperPoseActionState())
# sekvenca1:
# tts
# pokret koji je sprzen u chorographu
#pokret koji je sekvenca za gripper
# kombinacija sinkronizacija tts+ pokret #govor preko servisa pozvati,
# Say word via tts service
smach.StateMachine.add('tts', ServiceState('/tts_speech', SetString, 'First, I will show you all the poses'),
transitions={'succeeded':'vocabulary',
'aborted':'aborted',
'preempted':'preempted'})
# Get desired sequence from ASR
# first we need to feed vocabulari before starting asr
smach.StateMachine.add('vocabulary', ServiceState('/set_vocabulary', SetString, ''),
transitions={'succeeded':'asr',
'aborted':'aborted',
'preempted':'preempted'})
smach.StateMachine.add('asr', ServiceState('/start_asr', SetBool, True),
transitions={'succeeded':'rest',
'aborted':'aborted',
'preempted':'preempted'})
# Go to rest pose
smach.StateMachine.add('Rest', ServiceState('/pepper_robot/pose/rest', Empty),
transitions={'succeeded':'succeeded',
'aborted':'aborted',
'preempted':'preempted'})
def main():
rospy.init_node('commander')
com = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
with com:
smach.StateMachine.add(
'ARM',
smach_ros.SimpleActionState('arm_action',
DoArmAction,
goal=DoArmGoal(arm_cmd=DoArmGoal.ARM)),
{
'succeeded': 'WAYPOINTS',
'aborted': 'aborted'
})
waypoint_sequence = smach.Sequence(
outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded')
with waypoint_sequence:
smach.Sequence.add(
'TAKEOFF',
smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP0']))
smach.Sequence.add(
'WP1',
smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP1']))
smach.Sequence.add(
'WP2',
smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP2']))
smach.Sequence.add(
'WP3',
smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP3']))
smach.Sequence.add(
'WP4',
smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP2']))
smach.Sequence.add(
'WP5',
smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP3']))
smach.Sequence.add(
'WP6',
smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP1']))
smach.Sequence.add(
'WP7',
smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP3']))
smach.Sequence.add(
'WP8',
smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP2']))
smach.Sequence.add(
'WP9',
smach_ros.SimpleActionState('wp_action',
DoWPAction,
goal=waypoints['WP1']))
smach.StateMachine.add('WAYPOINTS',
waypoint_sequence,
transitions={
'succeeded': 'LAND',
'preempted': 'preempted',
'aborted': 'aborted'
})
smach.StateMachine.add(
'LAND', smach_ros.SimpleActionState('land_action', DoLandAction), {
'succeeded': 'succeeded',
'aborted': 'aborted'
})
smach_introspect = smach_ros.IntrospectionServer('commander_viewer', com,
'COMMANDER')
smach_introspect.start()
outcome = com.execute()
smach_introspect.stop()
rospy.loginfo('State Machine completed with status {0}'.format(outcome))
def make_hedge(shared):
sm_approach = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_approach:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared,
lambda cur: cur.depth(APPROACH_DEPTH)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.2, 0, 0])))
smach.Sequence.add('WAIT_HEDGE',
legacy_vision_states.WaitForObjectsState(
shared,
'find2_forward_camera',
'hedge',
timeout=25),
transitions={'timeout': 'failed'})
smach.Sequence.add('SLEEP',
common_states.SleepState(2)) # Get a bit closer
sm_center = smach.Sequence(['succeeded', 'failed', 'preempted'],
'succeeded')
with sm_center:
smach.Sequence.add(
'CENTER_APPROACH_HEDGE',
legacy_vision_states.CenterApproachObjectState(
shared, 'find2_forward_camera', desired_scale=180))
smach.Sequence.add(
'LEFT',
common_states.WaypointState(shared,
lambda cur: cur.turn_left_deg(90)))
smach.Sequence.add(
'GO', common_states.WaypointState(shared,
lambda cur: cur.right(4)))
smach.Sequence.add(
'RIGHT',
common_states.WaypointState(shared,
lambda cur: cur.turn_right_deg(90)))
sm_pipe = smach.Sequence(['succeeded', 'failed', 'preempted'], 'succeeded')
with sm_pipe:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(
shared, lambda cur: cur.depth(constants.PIPE_DEPTH)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(
shared, numpy.array([constants.PIPE_SPEED, 0, 0])))
smach.Sequence.add('WAIT_PIPE',
legacy_vision_states.WaitForObjectsState(
shared, 'find2_down_camera', 'pipe'),
transitions={'timeout': 'failed'})
smach.Sequence.add(
'STOP', common_states.WaypointState(shared, lambda cur: cur))
sm = smach.StateMachine(['succeeded', 'failed', 'preempted'])
with sm:
smach.StateMachine.add('APPROACH',
sm_approach,
transitions={
'succeeded': 'CENTER',
'failed': 'PIPE'
})
smach.StateMachine.add('CENTER',
sm_center,
transitions={
'succeeded': 'PIPE',
'failed': 'PIPE'
})
smach.StateMachine.add('PIPE',
sm_pipe,
transitions={'succeeded': 'succeeded'})
return sm
def make_manipulation(shared):
sm_wheel = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'],
'succeeded')
with sm_wheel:
smach.Sequence.add(
'OPEN_LOOP_FORWARD',
common_states.WaypointState(shared,
lambda cur: cur.forward(1.9),
speed=.25))
smach.Sequence.add(
'WAIT_WHEEL',
legacy_vision_states.WaitForObjectsState(shared,
'find2_forward_camera',
'grapes/grape',
timeout=10))
smach.Sequence.add(
'APPROACH_WHEEL',
legacy_vision_states.CenterObjectState(shared,
'find2_forward_camera',
gain=.2))
smach.Sequence.add('EXTEND',
subjugator_states.GasPoweredStickState(True))
smach.Sequence.add('OPEN_LOOP_FORWARD2',
common_states.WaypointState(shared,
lambda cur: cur.forward(.85)\
.relative([0, -0.02, -0.02])
.relative([0, .075, .075]), speed=.1))
smach.Sequence.add(
'TURN',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.left(.1), lambda cur: cur.down(.3),
lambda cur: cur.right(.45), lambda cur: cur.up(.45),
lambda cur: cur.left(.45), lambda cur: cur.down(.45),
lambda cur: cur.right(.45)
],
speed=.1))
smach.Sequence.add('RETRACT',
subjugator_states.GasPoweredStickState(False))
sm_move_lever = smach.StateMachine(
['succeeded', 'timeout', 'failed', 'preempted'], input_keys=['z'])
with sm_move_lever:
smach.StateMachine.add('COMPARE_Z',
CompareZState(),
transitions=dict(
higher='GO_DOWN',
lower='GO_UP',
))
smach.StateMachine.add(
'GO_UP',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.right(.08),
lambda cur: cur.down(.15),
lambda cur: cur.forward(1),
lambda cur: cur.up(.3),
lambda cur: cur.down(1),
]))
smach.StateMachine.add(
'GO_DOWN',
common_states.WaypointSeriesState(shared, [
lambda cur: cur.right(.08),
lambda cur: cur.up(.15),
lambda cur: cur.forward(1),
lambda cur: cur.down(.3),
lambda cur: cur.up(1),
]))
sm_lever = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'],
'succeeded')
with sm_lever:
smach.Sequence.add(
'OPEN_LOOP_FORWARD',
common_states.WaypointState(shared, lambda cur: cur.forward(1.9)))
smach.Sequence.add('SAVE_Z', SaveZState())
smach.Sequence.add(
'WAIT_LEVER',
legacy_vision_states.WaitForObjectsState(shared,
'find2_forward_camera',
'grapes/lever',
timeout=10))
smach.Sequence.add(
'APPROACH_LEVER',
legacy_vision_states.CenterObjectState(shared,
'find2_forward_camera',
gain=.2))
smach.Sequence.add('MOVE_LEVER', sm_move_lever)
# Create a SMACH state machine
sm = smach.Sequence(['succeeded', 'timeout', 'failed', 'preempted'],
'succeeded')
with sm:
smach.Sequence.add(
'DEPTH',
common_states.WaypointState(shared, lambda cur: cur.depth(2.5)))
smach.Sequence.add(
'APPROACH',
common_states.VelocityState(shared, numpy.array([.4, 0, 0])))
smach.Sequence.add(
'WAIT_MANIPULATION',
legacy_vision_states.WaitForObjectsState(shared,
'find2_forward_camera',
'grapes/board',
timeout=15))
smach.Sequence.add(
'APPROACH_MANIPULATION',
legacy_vision_states.CenterApproachObjectState(
shared, 'find2_forward_camera', desired_scale=130e3 / 3**2))
smach.Sequence.add(
'YAW',
common_states.WaypointState(
shared, lambda cur: cur.heading(
math.radians(-179 +
(180 if cur.forward_vector[0] > 0 else 0)))))
smach.Sequence.add(
'APPROACH_MANIPULATION_POST_YAW',
legacy_vision_states.CenterApproachObjectState(
shared, 'find2_forward_camera', desired_scale=130e3 / 3**2))
smach.Sequence.add('WHEEL',
sm_wheel,
transitions=dict(failed='BACKUP'))
smach.Sequence.add(
'BACKUP',
common_states.WaypointState(shared, lambda cur: cur.backward(3)))
smach.Sequence.add(
'REFIND',
common_states.VelocityState(shared, numpy.array([.1, 0, 0])))
smach.Sequence.add(
'WAIT_MANIPULATION2',
legacy_vision_states.WaitForObjectsState(shared,
'find2_forward_camera',
'grapes/board',
timeout=15))
smach.Sequence.add(
'APPROACH_MANIPULATION2',
legacy_vision_states.CenterApproachObjectState(
shared, 'find2_forward_camera', desired_scale=130e3 / 3**2))
smach.Sequence.add('LEVER', sm_lever)
smach.Sequence.add(
'BACKUP2',
common_states.WaypointState(shared, lambda cur: cur.backward(1)))
return sm
def object_gatherer_sm(target_types):
"""
Object gatherer SM:
- explore house while looking for tables
- approach each detected table and pick all the objects of the chosen types
"""
# look for tables state machine
detect_tables_sm = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted'],
output_keys=['table', 'table_pose'])
with detect_tables_sm:
smach.StateMachine.add('DETECT_TABLES',
MonitorTables(),
transitions={
'succeeded': 'VISITED_TABLE?',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('VISITED_TABLE?',
TableWasVisited(),
transitions={
'true': 'PAUSE_DETECTION',
'false': 'succeeded'
})
smach.StateMachine.add(
'PAUSE_DETECTION',
Sleep(2), # improbable to see a new table for a while
transitions={
'succeeded': 'DETECT_TABLES',
'aborted': 'aborted'
})
# gets called when ANY child state terminates
def child_term_cb(outcome_map):
# terminate all running states if FOO finished with outcome 'outcome3'
if outcome_map['EXPLORE_HOUSE']:
return True
# terminate all running states if BAR finished
if outcome_map['DETECT_TABLES']:
return True
# in all other case, just keep running, don't terminate anything
return False
# gets called when ALL child states are terminated
def out_cb(outcome_map):
if outcome_map['DETECT_TABLES'] == 'succeeded':
return 'detected'
if outcome_map['EXPLORE_HOUSE'] == 'succeeded':
return 'not_detected'
return outcome_map['EXPLORE_HOUSE']
# concurrence state machine: detect tables while exploring the house
search_sm = smach.Concurrence(
outcomes=['detected', 'not_detected', 'aborted', 'preempted'],
default_outcome='not_detected',
output_keys=['table', 'table_pose'],
child_termination_cb=child_term_cb,
outcome_cb=out_cb)
with search_sm:
smach.Concurrence.add('EXPLORE_HOUSE', ExploreHouse())
smach.Concurrence.add('DETECT_TABLES', detect_tables_sm)
# confirm detection state machine
confirm_sm = smach.Sequence(outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded',
input_keys=['table', 'table_pose'],
output_keys=['table', 'table_pose'])
with confirm_sm:
smach.Sequence.add('GET_ROBOT_POSE', GetRobotPose())
smach.Sequence.add('TURN_TO_TABLE_1',
LookToPose(),
remapping={'target_pose': 'table_pose'})
smach.Sequence.add('CONFIRM_TABLE_1', MonitorTables(2.0)) # 2s timeout
smach.Sequence.add('TURN_TO_TABLE_2',
LookToPose(),
remapping={'target_pose': 'table_pose'})
smach.Sequence.add('CONFIRM_TABLE_2', MonitorTables(2.0)) # 2s timeout
smach.Sequence.add('VALIDATE_SIZE', CheckTableSize())
# Full SM: explore the house and gather objects from all detected tables
sm = DoOnExitContainer(outcomes=[
'detected', 'not_detected', 'tray_full', 'aborted', 'preempted'
])
with sm:
smach.StateMachine.add('SEARCH',
search_sm,
transitions={
'detected': 'CONFIRM',
'not_detected': 'not_detected',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('CONFIRM',
confirm_sm,
transitions={
'succeeded': 'GATHER',
'aborted': 'DISCARD',
'preempted': 'preempted'
})
smach.StateMachine.add(
'DISCARD',
TableMarkVisited(valid=False), # mark as invalid
transitions={'succeeded': 'SEARCH'})
smach.StateMachine.add('GATHER',
GatherObjects(target_types),
transitions={
'succeeded': 'SEARCH',
'aborted': 'aborted',
'preempted': 'preempted',
'tray_full': 'tray_full'
})
DoOnExitContainer.add_finally(
'FOLD_ARM',
FoldArm()) # fold arm on exit regardless of the outcome
return sm
def GatherObjects(target_types):
"""
Object gatherer SM: approach the requested table and pick all the objects of the requested type
"""
# approach to the table
approach_table_sm = smach.Sequence(
outcomes=['succeeded', 'aborted', 'preempted'],
connector_outcome='succeeded',
input_keys=['table', 'table_pose'])
with approach_table_sm:
smach.Sequence.add('GET_ROBOT_POSE', GetRobotPose())
smach.Sequence.add('CALC_APPROACHES',
TableSidesPoses(cfg.APPROACH_DIST_TO_TABLE),
transitions={'no_valid_table': 'aborted'},
remapping={
'poses': 'approach_poses',
'closest_pose': 'closest_approach_pose'
})
smach.Sequence.add(
'APPROACH_TABLE',
GoToPose(), # use default tolerances; no precision needed here
transitions={
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'target_pose': 'closest_approach_pose'})
# detects objects over the table, and make a picking plan
make_pick_plan_sm = DoOnExitContainer(
outcomes=['succeeded', 'aborted', 'preempted', 'no_reachable_objs'],
input_keys=['table', 'table_pose', 'object_types'],
output_keys=['picking_poses', 'closest_picking_pose', 'picking_plan'])
with make_pick_plan_sm:
smach.StateMachine.add('GET_ROBOT_POSE',
GetRobotPose(),
transitions={'succeeded': 'CALC_PICK_POSES'})
smach.StateMachine.add('CALC_PICK_POSES',
TableSidesPoses(cfg.PICKING_DIST_TO_TABLE),
transitions={
'succeeded': 'CALC_APPROACHES',
'no_valid_table': 'aborted'
},
remapping={
'poses': 'picking_poses',
'closest_pose': 'closest_picking_pose'
})
# re-calculate approach poses when nearby for more precision
smach.StateMachine.add('CALC_APPROACHES',
TableSidesPoses(cfg.APPROACH_DIST_TO_TABLE),
transitions={
'succeeded': 'CALC_DETACH_POSES',
'no_valid_table': 'aborted'
},
remapping={
'poses': 'approach_poses',
'closest_pose': 'closest_approach_pose'
})
smach.StateMachine.add('CALC_DETACH_POSES',
TableSidesPoses(cfg.DETACH_DIST_FROM_TABLE),
transitions={
'succeeded': 'ALIGN_TO_TABLE',
'no_valid_table': 'aborted'
},
remapping={
'poses': 'detach_poses',
'closest_pose': 'closest_detach_pose'
})
# from approach pose we may still misidentify some objects, so better to come as close as possible
smach.StateMachine.add('ALIGN_TO_TABLE',
AlignToTable(),
transitions={
'succeeded': 'DETECT_OBJECTS',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'pose': 'closest_picking_pose'})
smach.StateMachine.add('DETECT_OBJECTS',
ObjectDetection(),
transitions={
'succeeded': 'TABLE_VISITED',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('TABLE_VISITED',
TableMarkVisited(),
transitions={'succeeded': 'TABLE_OBSTACLE'})
smach.StateMachine.add('TABLE_OBSTACLE',
TableAsObstacle(),
transitions={'succeeded': 'OBJECTS_FOUND?'},
remapping={'pose': 'table_pose'})
smach.StateMachine.add('OBJECTS_FOUND?',
ObjectsDetected(),
transitions={
'true': 'GROUP_OBJECTS',
'false': 'no_reachable_objs'
})
smach.StateMachine.add('GROUP_OBJECTS',
GroupObjects(),
transitions={'succeeded': 'MAKE_PICKING_PLAN'})
smach.StateMachine.add('MAKE_PICKING_PLAN', MakePickingPlan())
DoOnExitContainer.add_finally('CLEAR_P_SCENE', ClearPlanningScene())
# go to a picking location and pick all objects reachable from there
pick_objects_sm = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted', 'tray_full'],
input_keys=['table', 'picking_loc', 'object_types'])
with pick_objects_sm:
smach.StateMachine.add('PICK_LOC_FIELDS',
PickLocFields(),
transitions={'succeeded': 'GET_ROBOT_POSE'})
smach.StateMachine.add('GET_ROBOT_POSE',
GetRobotPose(),
transitions={'succeeded': 'AT_PICKING_POSE?'})
smach.StateMachine.add('AT_PICKING_POSE?',
AreSamePose(),
transitions={
'true': 'PICK_OBJECTS',
'false': 'GOTO_APPROACH'
},
remapping={
'pose1': 'robot_pose',
'pose2': 'picking_pose'
})
smach.StateMachine.add(
'GOTO_APPROACH',
GoToPose(), # use default tolerances; no precision needed here
transitions={'succeeded': 'ALIGN_TO_TABLE'},
remapping={'target_pose': 'approach_pose'})
smach.StateMachine.add('ALIGN_TO_TABLE',
AlignToTable(),
transitions={'succeeded': 'PICK_OBJECTS'},
remapping={'pose': 'picking_pose'})
smach.StateMachine.add('PICK_OBJECTS',
PickReachableObjs(),
transitions={'succeeded': 'CLEAR_MARKERS'})
smach.StateMachine.add('CLEAR_MARKERS',
ClearMarkers(),
transitions={
'succeeded': 'DETACH_FROM_TABLE',
'aborted': 'DETACH_FROM_TABLE'
})
smach.StateMachine.add('DETACH_FROM_TABLE',
DetachFromTable(),
remapping={'pose': 'detach_pose'})
# iterate over all picking locations in the plan
pick_objects_it = smach.Iterator(
outcomes=['succeeded', 'preempted', 'aborted', 'tray_full'],
input_keys=['table', 'picking_plan', 'object_types'],
output_keys=[],
it=lambda: pick_objects_it.userdata.picking_plan,
it_label='picking_loc',
exhausted_outcome='succeeded')
with pick_objects_it:
smach.Iterator.set_contained_state('',
pick_objects_sm,
loop_outcomes=['succeeded'])
# Full SM: approach the table, make a picking plan and execute it, double-checking that we left no object behind
sm = smach.StateMachine(
outcomes=['succeeded', 'aborted', 'preempted', 'tray_full'],
input_keys=['table', 'table_pose'])
sm.userdata.object_types = target_types
with sm:
smach.StateMachine.add('APPROACH_TABLE',
approach_table_sm,
transitions={
'succeeded': 'RE_DETECT_TABLE',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'RE_DETECT_TABLE',
MonitorTables(2.0),
transitions={
'succeeded':
'MAKE_PICK_PLAN', # re-detect when nearby for more precision,
'aborted': 'succeeded'
}) # or just succeed to give up if not seen again
smach.StateMachine.add('MAKE_PICK_PLAN',
make_pick_plan_sm,
transitions={
'succeeded': 'EXEC_PICK_PLAN',
'aborted': 'aborted',
'preempted': 'preempted',
'no_reachable_objs': 'succeeded'
})
smach.StateMachine.add(
'EXEC_PICK_PLAN',
pick_objects_it,
transitions={
'succeeded':
'DETECT_OBJECTS', # double-check that we left no object behind
'aborted':
'aborted', # restore original configuration on error
'preempted': 'preempted',
'tray_full': 'tray_full'
})
smach.StateMachine.add('DETECT_OBJECTS',
ObjectDetection(),
transitions={
'succeeded': 'OBJECTS_LEFT',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'OBJECTS_LEFT',
ObjectsDetected(),
transitions={
'true':
'APPROACH_TABLE', # at least one object left; restart picking
'false': 'succeeded'
}) # otherwise, we are done
return sm
