def __init__(self, robot, door_start_wp_designator, door_dest_wp_designator, approach_speed=0.1, push_speed=0.05, attempts=10):
"""
Push against a door until its open
:param robot: Robot on which to execute this state machine
:param door_entity_designator: The door entity. Defaults to None, which implies the door its in front of
This entity must have 2 fields in its data: push_start_waypoint and push_destination_waypoint,
which must both contain an ID of a waypoint.
push_start_waypoint is from where the robot will start pushing
push_destination_waypoint simply needs to be reachable for the door the be considered 'open'
:param approach_speed: Speed with which to approach the door
:param push_speed: Speed with which to push against the door
:return:
"""
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
#door_start_wp_designator = WaypointOfDoorDesignator(robot, door_entity_designator, direction='start', name='door_open_start')
#door_dest_wp_designator = WaypointOfDoorDesignator(robot, door_entity_designator, direction='destination', name='door_open_dest')
with self:
smach.StateMachine.add( 'GOTO_DOOR_START',
states.NavigateToWaypoint(robot, door_start_wp_designator, radius=0.05),
transitions={'arrived' :'PUSH_DOOR_ITERATOR',
'unreachable' :'failed',
'goal_not_defined' :'failed'})
# START REPEAT DOOR OPENING
push_door_iterator = smach.Iterator(outcomes=['open', 'closed', 'failed'],
it = lambda:range(0, attempts),
it_label='counter',
input_keys=[],
output_keys=[],
exhausted_outcome = 'failed')
with push_door_iterator:
push_door = smach.StateMachine( outcomes = ['open', 'closed', 'failed'])
with push_door:
smach.StateMachine.add( 'APPROACH_AGAINST_N',
ForceDriveToTouchDoor(robot, approach_speed),
transitions={'front':'closed',
'left':'CHECK_DOOR_PASSABLE',
'right':'CHECK_DOOR_PASSABLE',
'failed':'failed'})
smach.StateMachine.add( 'CHECK_DOOR_PASSABLE',
CheckDoorPassable(robot,
destination_designator=door_dest_wp_designator),
transitions={'blocked':'closed',
'passable':'open'})
smach.Iterator.set_contained_state( 'APPROACH_AGAINST',
push_door,
loop_outcomes=['failed', 'closed'],
break_outcomes=['open'])
smach.StateMachine.add( 'PUSH_DOOR_ITERATOR',
push_door_iterator,
{ 'open':'succeeded',
'closed':'failed',
'failed':'failed'})
def searchRoutine(yaw, initialAngle, finalAngle, left=True, center=False):
searchSpan = finalAngle - initialAngle
direction = searchSpan > 0
searchSpan = abs(searchSpan)
numIt = ceil(searchSpan / 20)
search = smach.Iterator(outcomes=['success', 'abort'],
input_keys=[],
output_keys=['outAngle'],
it=lambda: range(0, int(numIt)),
it_label='index',
exhausted_outcome='abort')
search.userdata.initialAngle = initialAngle
with search:
container_sm = smach.StateMachine(
outcomes=['success', 'abort', 'continue'],
output_keys=['outAngle'])
container_sm.userdata.timeout = 500
container_sm.userdata.angle = 20
container_sm.userdata.initialAngle = initialAngle
container_sm.userdata.outAngle = 0
with container_sm:
smach.StateMachine.add('InitialRotate',
RotateTo(yaw, increment=True, direction=1),
transitions={
'success': 'Localize',
'abort': 'abort'
},
remapping={
'timeout': 'timeout',
'angle': 'initialAngle'
})
smach.StateMachine.add('Rotate',
RotateTo(yaw,
increment=True,
direction=direction),
transitions={
'success': 'continue',
'abort': 'abort'
},
remapping={
'timeout': 'timeout',
'angle': 'angle'
})
smach.StateMachine.add('Localize',
Localize(True, left, center=center),
transitions={
'success': 'success',
'failure': 'Rotate'
},
remapping={'angle': 'outAngle'})
smach.Iterator.set_contained_state('SEARCH',
container_sm,
loop_outcomes=['continue'])
return search
def main():
rospy.init_node('smach_example_state_machine')
# Create a SMACH state machine
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
#sm.userdata.num = 5
sm.userdata.result = []
# Open the container
with sm:
# Add states to the container
smach.StateMachine.add('FOO',
Foo(),
transitions={
'succeeded': 'SUB',
'aborted': 'aborted',
'preempted': 'preempted'
})
sm_ite = smach.Iterator(
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['result'],
output_keys=['result'],
#it = lambda: range(sm.userdata.num),
#it = lambda: ['i', 'my', 'me'],
#it = ['i', 'my', 'me'],
#it = range(5),
#it = dynamic_iterator,
it=static_list,
it_label='it_data',
exhausted_outcome='succeeded')
with sm_ite:
# if BAR's result is 'succeeded' then loop, otherwise exit with result.
smach.Iterator.set_contained_state('BAR',
Bar(),
loop_outcomes=['succeeded'])
smach.StateMachine.add('SUB',
sm_ite,
transitions={
'succeeded': 'BAS',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.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 PlaceObject(attempts=3):
""" Place a given object, retrying up to a given number of times """
it = smach.Iterator(
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_name', 'support_surf', 'place_pose'],
output_keys=[],
it=lambda: range(0, attempts),
it_label='attempt',
exhausted_outcome='aborted')
with it:
sm = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'aborted', 'continue'],
input_keys=['object_name', 'support_surf', 'place_pose'],
output_keys=[])
with sm:
smach.StateMachine.add(
'PlaceObject',
smach_ros.SimpleActionState(
'place_object',
thorp_msgs.PlaceObjectAction,
goal_slots=['object_name', 'support_surf', 'place_pose'],
result_slots=[]),
remapping={
'object_name': 'object_name',
'support_surf': 'support_surf',
'place_pose': 'place_pose'
},
transitions={
'succeeded': 'succeeded',
'preempted': 'preempted',
'aborted': 'ClearOctomap'
})
smach.StateMachine.add('ClearOctomap',
smach_ros.ServiceState(
'clear_octomap', std_srvs.Empty),
transitions={
'succeeded': 'continue',
'preempted': 'preempted',
'aborted': 'aborted'
})
# TODOs:
# - we should open the gripper, in case we have picked an object
# - check error and, if collision between parts of the arm, move a bit the arm --> not enough info
# - this doesn't make too much sense as a loop... better try all our tricks and exit
# - can I reuse the same for place and MoveToTarget???
smach.Iterator.set_contained_state('', sm, loop_outcomes=['continue'])
return it
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer)
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized':
'SAY_UNABLE_TO_OPEN_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR',
states.Say(
robot,
"I am unable to open the shelf door, "
"can you please open it for me?"),
transitions={'spoken': 'AWAIT_START'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None, manipulate_machine=None):
""" Moves the entities for this challenge to the correct poses"""
# Determine where to perform the challenge
robot_pose = robot.base.get_location()
ENTITY_POSES.sort(key=lambda tup:
(tup[0].frame.p - robot_pose.frame.p).Norm())
cabinet_id = ENTITY_POSES[0][2]
table_id = ENTITY_POSES[0][3]
# Update the world model
robot.ed.update_entity(id="balcony_shelf",
frame_stamped=FrameStamped(
kdl.Frame(kdl.Rotation(),
kdl.Vector(0.0, 3.0,
0.0)),
frame_id="map"))
robot.ed.update_entity(id=cabinet_id,
frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=table_id,
frame_stamped=ENTITY_POSES[0][1])
# Update designators
cabinet.id_ = ENTITY_POSES[0][2]
room.id_ = ENTITY_POSES[0][4]
# Update manipulate machine
manipulate_machine.place_entity_designator.id_ = cabinet_id
manipulate_machine.place_designator._area = ENTITY_POSES[0][5]
manipulate_machine.place_designator.place_location_designator.id = cabinet_id
manipulate_machine.table_designator.id_ = table_id
manipulate_machine.cabinet.id_ = ENTITY_POSES[0][2]
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table,
cb_args=[single_item]),
transitions={'done': 'NAV_TO_START'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"},
cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, cabinet),
transitions={
'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'
})
smach.StateMachine.add("WRITE_PDF_SHELVES",
pdf_writer,
transitions={"done": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=['succeeded',
'failed'], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded')
with range_iterator:
smach.Iterator.set_contained_state(
'SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {
'succeeded': 'AT_END',
'failed': 'Aborted'
})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "manipulation")
def __init__(self, robot, personNameDesLocal):
@smach.cb_interface(outcomes=['spoken'])
def sayIsYourName(userdata=None):
printOk("sayIsYourName")
robot.speech.speak("I heard " + personNameDes.resolve() +
". Is this correct?",
block=True)
return 'spoken'
@smach.cb_interface(outcomes=['spoken'])
def sayCouldNotLearnNameCB(userdata=None):
printOk("sayCouldNotLearnNameCB")
robot.speech.speak(
"Sorry but I could not understand your name. I will just call you "
+ personNameDesLocal.resolve(),
block=False)
return 'spoken'
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
learnNameIterator = smach.Iterator(
outcomes=['iterator_success', 'iterator_failed'],
it=lambda: range(0, 3),
it_label='counter',
input_keys=[],
output_keys=[],
exhausted_outcome='iterator_failed')
with learnNameIterator:
# for some reason the container needs to have the same outcomes as the iterator, plus a "continue" state
learnNameContainer = smach.StateMachine(outcomes=[
'iterator_success', 'iterator_failed',
'inner_container_failed'
])
with learnNameContainer:
smach.StateMachine.add('ASK_PERSON_NAME',
test_states.AskPersonName(
robot,
personNameDesLocal.writeable),
transitions={
'succeeded': 'SAY_IS_YOUR_NAME',
'failed': 'SAY_HEAR_FAILED'
})
smach.StateMachine.add(
'SAY_IS_YOUR_NAME',
smach.CBState(sayIsYourName),
transitions={'spoken': 'HEAR_YES_NO_1'})
smach.StateMachine.add('HEAR_YES_NO_1',
states_interaction.HearYesNo(robot),
transitions={
'heard_yes': 'iterator_success',
'heard_no': 'SAY_NAME_WRONG',
'heard_failed':
'SAY_HEAR_FAILED'
})
smach.StateMachine.add(
'SAY_NAME_WRONG',
states.Say(
robot,
["Sorry, I understood wrong.", "Oh I'm sorry."],
block=False),
transitions={'spoken': 'inner_container_failed'})
smach.StateMachine.add(
'SAY_HEAR_FAILED',
states.Say(robot, [
"I did not understand your name.",
"I didn't listen correctly."
],
block=False),
transitions={'spoken': 'inner_container_failed'})
smach.Iterator.set_contained_state(
'LEARN_NAME_CONTAINER',
learnNameContainer,
loop_outcomes=['inner_container_failed'])
# break_outcomes=['iterator_success'])
smach.StateMachine.add('LEARN_NAME_ITERATOR',
learnNameIterator,
transitions={
'iterator_success':
'container_success',
'iterator_failed':
'SAY_COULD_NOT_LEARN_NAME'
})
smach.StateMachine.add('SAY_COULD_NOT_LEARN_NAME',
smach.CBState(sayCouldNotLearnNameCB),
transitions={'spoken': 'container_failed'})
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 __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=["Done", "Aborted"])
hmi_result_des = ds.VariableDesignator(resolve_type=HMIResult)
information_point_id_designator = ds.FuncDesignator(ds.AttrDesignator(
hmi_result_des, "semantics", resolve_type=unicode),
str,
resolve_type=str)
information_point_designator = ds.EdEntityDesignator(
robot, id_designator=information_point_id_designator)
with self:
single_item = InformMachine(robot)
if START_ROBUST:
smach.StateMachine.add("START_CHALLENGE",
states.StartChallengeRobust(
robot, INITIAL_POSE_ID),
transitions={
"Done": "ASK_WHERE_TO_GO",
"Aborted": "Aborted",
"Failed": "Aborted"
})
smach.StateMachine.add(
"ASK_WHERE_TO_GO",
states.Say(
robot,
"Near which furniture object should I go to start guiding people?"
),
transitions={"spoken": "WAIT_WHERE_TO_GO"})
smach.StateMachine.add(
"WAIT_WHERE_TO_GO",
states.HearOptionsExtra(
robot=robot,
spec_designator=ds.Designator(
initial_value=START_GRAMMAR),
speech_result_designator=hmi_result_des.writeable),
transitions={
"heard": "ASK_CONFIRMATION",
"no_result": "ASK_WHERE_TO_GO"
}) # ToDo: add fallbacks #option: STORE_STARTING_POSE
smach.StateMachine.add(
"ASK_CONFIRMATION",
states.Say(robot, [
"I hear that you would like me to start the tours at"
" the {place}, is this correct?"
],
place=information_point_id_designator,
block=True),
transitions={"spoken": "CONFIRM_LOCATION"})
smach.StateMachine.add("CONFIRM_LOCATION",
states.HearOptions(
robot=robot, options=["yes", "no"]),
transitions={
"yes": "MOVE_OUT_OF_MY_WAY",
"no": "ASK_WHERE_TO_GO",
"no_result": "ASK_WHERE_TO_GO"
})
smach.StateMachine.add(
"MOVE_OUT_OF_MY_WAY",
states.Say(robot,
"Please move your ass so I can get going!"),
transitions={"spoken": "TC_MOVE_TIME"})
smach.StateMachine.add("TC_MOVE_TIME",
states.WaitTime(robot=robot,
waittime=3),
transitions={
"waited": "NAV_TO_START",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START",
states.NavigateToSymbolic(
robot=robot,
entity_designator_area_name_map={
information_point_designator: "in_front_of"
},
entity_lookat_designator=information_point_designator),
transitions={
"arrived": "TURN_AROUND",
"unreachable": "WAIT_NAV_BACKUP",
"goal_not_defined": "Aborted"
}) # If this happens: never mind
smach.StateMachine.add("WAIT_NAV_BACKUP",
states.WaitTime(robot, 3.0),
transitions={
"waited": "NAV_TO_START_BACKUP",
"preempted": "Aborted"
})
smach.StateMachine.add(
"NAV_TO_START_BACKUP",
states.NavigateToSymbolic(
robot=robot,
entity_designator_area_name_map={
information_point_designator: "near"
},
entity_lookat_designator=information_point_designator),
transitions={
"arrived": "TURN_AROUND",
"unreachable":
"SAY_CANNOT_REACH_WAYPOINT", # Current pose backup
"goal_not_defined": "Aborted"
}) # If this happens: never mind
@smach.cb_interface(outcomes=["done"])
def _turn_around(userdata=None):
""" Turns the robot approximately 180 degrees around """
v_th = 0.5
robot.base.force_drive(vx=0.0,
vy=0.0,
vth=v_th,
timeout=math.pi / v_th)
return "done"
smach.StateMachine.add(
"TURN_AROUND",
smach.CBState(_turn_around),
transitions={"done": "STORE_STARTING_POSE"})
smach.StateMachine.add(
"SAY_CANNOT_REACH_WAYPOINT",
states.Say(
robot, "I am not able to reach the starting point."
"I'll use this as starting point"),
transitions={"spoken": "STORE_STARTING_POSE"})
else:
smach.StateMachine.add("INITIALIZE",
states.Initialize(robot),
transitions={
"initialized":
"STORE_STARTING_POSE",
"abort": "Aborted"
})
## This is purely for a back up scenario until the range iterator
@smach.cb_interface(outcomes=["succeeded"])
def store_pose(userdata=None):
base_loc = robot.base.get_location()
base_pose = base_loc.frame
location_id = INFORMATION_POINT_ID
robot.ed.update_entity(id=location_id,
frame_stamped=FrameStamped(
base_pose, "/map"),
type="waypoint")
return "succeeded"
smach.StateMachine.add("STORE_STARTING_POSE",
smach.CBState(store_pose),
transitions={"succeeded": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=["succeeded",
"failed"], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(1000),
it_label="index",
exhausted_outcome="succeeded")
with range_iterator:
smach.Iterator.set_contained_state(
"SINGLE_ITEM",
single_item,
loop_outcomes=["succeeded", "failed"])
smach.StateMachine.add("RANGE_ITERATOR", range_iterator, {
"succeeded": "AT_END",
"failed": "Aborted"
})
# End setup iterator
smach.StateMachine.add("AT_END",
states.Say(robot, "Goodbye"),
transitions={"spoken": "Done"})
def construct_sm():
broadcaster = TFBroadCaster()
tf_cast_loop = rospy.Timer(rospy.Duration(0.1), broadcaster.loop)
sm = smach.StateMachine(outcomes=['success', 'failure'])
sm.userdata.nums = range(5)
sm.userdata.tf_x = 0
sm.userdata.tf_y = 0
sm.userdata.tf_z = 0
move_base_client = actionlib.SimpleActionClient('/move_base',
MoveBaseAction)
rospy.loginfo('wait for move_base action server')
move_base_client.wait_for_server()
with sm:
@smach.cb_interface(outcomes=['success'])
def start_cb(ud):
rospy.loginfo('start the test')
move_arm_neutral()
return 'success'
smach.StateMachine.add('START',
smach.CBState(start_cb),
transitions={'success': 'ITERATOR'})
iterator = smach.Iterator(outcomes=['success', 'failure'],
input_keys=['tf_x', 'tf_y', 'tf_z'],
output_keys=[],
it=lambda: range(0, len(sm.userdata.nums)),
it_label='index',
exhausted_outcome='success')
with iterator:
container_sm = smach.StateMachine(
input_keys=['tf_x', 'tf_y', 'tf_z', 'index'],
outcomes=['continue', 'failure'])
with container_sm:
### PUT OBJECT IN THE SHELF
@smach.cb_interface(outcomes=['success'])
def put_object_cb(ud):
delete_object('apple')
x = round(np.random.uniform(2.0, 2.4), 1)
put_object('apple', x, 4.75, 0.5)
return 'success'
smach.StateMachine.add('PUTOBJECT',
smach.CBState(put_object_cb),
transitions={'success': 'NAVOBSTACLE'})
### MOVE TO THE CENTER OF THE SHELF
smach.StateMachine.add('NAVOBSTACLE',
UpdateObstacle(0.5),
transitions={'success': 'Move2Shelf'})
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = 2.2
goal.target_pose.pose.position.y = 4.1
goal.target_pose.pose.position.z = 0
quaternion = tf.transformations.quaternion_from_euler(
0, 0, 1.57)
goal.target_pose.pose.orientation.x = quaternion[0]
goal.target_pose.pose.orientation.y = quaternion[1]
goal.target_pose.pose.orientation.z = quaternion[2]
goal.target_pose.pose.orientation.w = quaternion[3]
smach.StateMachine.add('Move2Shelf',
SimpleActionState('/move_base',
MoveBaseAction,
goal=goal),
transitions={
'succeeded': 'FindApple',
'preempted': 'failure',
'aborted': 'failure'
})
### FIND THE APPLE
smach.StateMachine.add('FindApple',
FindApple(),
transitions={'success': 'BROADCASTTF'})
### REGISTER AND BROADCAST THE TF
@smach.cb_interface(input_keys=['tf_x', 'tf_y', 'tf_z'],
outcomes=['success'])
def broadcast_tf_cb(ud):
while not rospy.is_shutdown():
result = move_arm_neutral()
if result is True:
break
#broadcast head_rgbd_sensor to apple position
broadcaster.add(ud.tf_x, ud.tf_y, ud.tf_z, 0.5, 0, 0,
'apple', 'head_rgbd_sensor_rgb_frame')
approach_x = ud.tf_x
tate = 0.1 * math.tan(0.5)
approach_y = ud.tf_y + tate
approach_z = ud.tf_z - 0.1
broadcaster.add(approach_x, approach_y, approach_z, 0.5, 0,
0, 'approach_p',
'head_rgbd_sensor_rgb_frame')
hand_to_map = get_relative_coordinate('map', 'approach_p')
broadcaster.add(hand_to_map.translation.x,
hand_to_map.translation.y,
hand_to_map.translation.z, -1.57, -1.57, 0,
'map_approach_p', 'map')
return 'success'
smach.StateMachine.add(
'BROADCASTTF',
smach.CBState(broadcast_tf_cb),
transitions={'success': 'GRASPOBSTACLE'})
### UPDATE OBSTACLE RECONFIGURE FOR AVOID HAND OBSTACLE
smach.StateMachine.add('GRASPOBSTACLE',
UpdateObstacle(0),
transitions={'success': 'MOVE2CENTER'})
@smach.cb_interface(outcomes=['success'])
def move_to_center_cb(ud):
while not rospy.is_shutdown():
relative_p = get_relative_coordinate2(
"hand_palm_link", "map_approach_p")
duration = (rospy.Time.now() -
relative_p.header.stamp).to_sec()
if duration < 1:
break
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "base_footprint"
goal.target_pose.pose.position.x = 0
goal.target_pose.pose.position.y = -relative_p.transform.translation.y
goal.target_pose.pose.orientation = quaternion_from_euler(
0, 0, 0)
navclient.send_goal(goal)
navclient.wait_for_result()
state = navclient.get_state()
return 'success'
### MOVE TO THE CENTER OF THE OBJECT
smach.StateMachine.add('MOVE2CENTER',
smach.CBState(move_to_center_cb),
transitions={'success': 'GRASPOBJECT'})
### GRASP THE OBJECT
@smach.cb_interface(outcomes=['success'])
def grasp_object_cb(ud):
add_collision_scene(frame_id='map_approach_p')
result = move_hand(1)
while not rospy.is_shutdown():
# trans = get_relative_coordinate("map", "approach_p")
trans = get_relative_coordinate(
"map", "map_approach_p")
result = move_wholebody_ik(trans.translation.x,
trans.translation.y,
trans.translation.z, -90, 0,
-90, 'map')
if result == True:
break
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "base_footprint"
goal.target_pose.pose.position.x = 0.15
goal.target_pose.pose.position.y = 0
goal.target_pose.pose.position.z = 0
goal.target_pose.pose.orientation = quaternion_from_euler(
0, 0, 0)
# goal.target_pose.header.frame_id = "hand_palm_link"
# goal.target_pose.pose.position.x = 0.15
# goal.target_pose.pose.position.y = 0
# goal.target_pose.pose.position.z = 0
# goal.target_pose.pose.orientation = quaternion_from_euler(0, 0, 0)
navclient.send_goal(goal)
navclient.wait_for_result()
state = navclient.get_state()
move_hand(0)
move_base_vel(-1, 0, 0)
remove_collision_scene()
while not rospy.is_shutdown():
result = move_arm_init()
if result is True:
break
broadcaster.clear()
return 'success'
smach.StateMachine.add('GRASPOBJECT',
smach.CBState(grasp_object_cb),
transitions={'success': 'CHECKGRASP'})
### CHECK GRASP THE OBJECT OR NOT
smach.StateMachine.add('CHECKGRASP',
CheckGrasp(),
transitions={
'success': 'ORIGINOBSTACLE',
'failure': 'Move2Shelf'
})
### UPDATE OBSTACLE FOR THE NAVIGATION
smach.StateMachine.add('ORIGINOBSTACLE',
UpdateObstacle(0.5),
transitions={'success': 'Move2Origin'})
### MOVE TO THE ORIGIN 0,0,0 IN MAP REFERENCE
goal = MoveBaseGoal()
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.header.frame_id = 'map'
goal.target_pose.pose.position.x = 0
goal.target_pose.pose.position.y = 0
goal.target_pose.pose.position.z = 0
quaternion = tf.transformations.quaternion_from_euler(0, 0, 0)
goal.target_pose.pose.orientation.x = quaternion[0]
goal.target_pose.pose.orientation.y = quaternion[1]
goal.target_pose.pose.orientation.z = quaternion[2]
goal.target_pose.pose.orientation.w = quaternion[3]
smach.StateMachine.add('Move2Origin',
SimpleActionState('/move_base',
MoveBaseAction,
goal=goal),
transitions={
'succeeded': 'OPENGRIPPER',
'preempted': 'failure',
'aborted': 'failure'
})
### OPENGRIPPER
@smach.cb_interface(outcomes=['success'])
def open_gripper_cb(ud):
result = move_hand(1)
return 'success'
smach.StateMachine.add('OPENGRIPPER',
smach.CBState(open_gripper_cb),
transitions={'success': 'DELETEOBJECT'})
### DETELE OBJECT READY FOR THE NEXT TASK
@smach.cb_interface(outcomes=['success'])
def delete_object_cb(ud):
delete_object('apple')
return 'success'
smach.StateMachine.add('DELETEOBJECT',
smach.CBState(delete_object_cb),
transitions={'success': 'continue'})
smach.Iterator.set_contained_state(
'CONTAINER_STATE',
container_sm,
loop_outcomes=['continue', 'failure'])
smach.StateMachine.add('ITERATOR',
iterator,
transitions={
'success': 'success',
'failure': 'failure'
})
return sm
def setup_statemachine(robot):
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
start_waypoint = ds.EntityByIdDesignator(robot,
id="manipulation_init_pose",
name="start_waypoint")
placed_items = []
with sm:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'INIT_WM',
'abort': 'Aborted'
})
smach.StateMachine.add("INIT_WM",
InitializeWorldModel(robot),
transitions={'done': 'AWAIT_START'})
# smach.StateMachine.add("INSTRUCT_WAIT_FOR_DOOR",
# states.Say(robot, ["Hi there, I will now wait until you remove the cup",
# "I'm waiting for you to remove the cup"], block=False),
# transitions={"spoken": "WAIT_FOR_DOOR"})
#
# smach.StateMachine.add("WAIT_FOR_DOOR",
# states.WaitForDoorOpen(robot, timeout=10),
# transitions={"closed": "DOOR_CLOSED",
# "open": "AWAIT_START"})
#
# smach.StateMachine.add("DOOR_CLOSED",
# states.Say(robot, ["I am waiting for you to remove the cup",
# "I'd start, if you remove the cup from my laser"]),
# transitions={"spoken": "WAIT_FOR_DOOR"})
if USE_SLAM:
drive_state = "RESET_ED_SLAM"
else:
drive_state = "NAV_TO_START"
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': drive_state,
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
if USE_SLAM:
# vth = 1.0
# smach.StateMachine.add("NAV_TO_FIT_POSE",
# ForceDrive(robot, 0, 0, vth, 3.14/vth),
# transitions={'done': 'FIT_ENTITY'})
smach.StateMachine.add("RESET_ED_SLAM",
states.ResetED(robot),
transitions={'done': 'FIT_ENTITY'})
smach.StateMachine.add("FIT_ENTITY",
FitEntity(robot, CABINET),
transitions={
'succeeded': 'NAV_TO_START',
'failed': 'SAY_FITTING_FAILED'
})
smach.StateMachine.add(
"SAY_FITTING_FAILED",
states.Say(robot, [
"Fitting the {0} failed, I will stop now.".format(CABINET)
],
mood="sad"),
transitions={'spoken': 'Aborted'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"}, cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'FORCE_ROTATE',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("FORCE_ROTATE",
ForceRotate(robot, 0.5, 2.0, 30.0),
transitions={
'done': "NAV_TO_START",
'timedout': "INSPECT_SHELVES"
})
# smach.StateMachine.add("RESET_ED",
# states.ResetED(robot),
# transitions={'done' :'INSPECT_SHELVES'})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, OBJECT_SHELVES),
transitions={
'succeeded': 'EXPORT_PDF',
'nothing_found': 'EXPORT_PDF',
'failed': 'EXPORT_PDF'
})
@smach.cb_interface(outcomes=["exported"])
def export_to_pdf(userdata=None):
global DETECTED_OBJECTS_WITH_PROBS
entities = [e[0] for e in DETECTED_OBJECTS_WITH_PROBS]
# Export images (Only best MAX_NUM_ENTITIES_IN_PDF)
# pdf.entities_to_pdf(robot.ed, entities[:MAX_NUM_ENTITIES_IN_PDF], "tech_united_manipulation_challenge")
return "exported"
smach.StateMachine.add('EXPORT_PDF',
smach.CBState(export_to_pdf),
transitions={'exported': 'RANGE_ITERATOR'})
# Begin setup iterator
range_iterator = smach.Iterator(
outcomes=['succeeded', 'failed'], #Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded'
) #The exhausted argument should be set to the preffered state machine outcome
with range_iterator:
single_item = ManipRecogSingleItem(
robot,
ds.VariableDesignator(placed_items, [Entity],
name="placed_items"))
smach.Iterator.set_contained_state(
'SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {
'succeeded': 'AT_END',
'failed': 'Aborted'
})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(sm, "manipulation")
return sm
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer)
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized':
'SAY_UNABLE_TO_OPEN_DOOR',
'abort': 'Aborted'
})
smach.StateMachine.add('SAY_UNABLE_TO_OPEN_DOOR',
states.Say(
robot,
"I am unable to open the shelf door, "
"can you please open it for me?"),
transitions={'spoken': 'AWAIT_START'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={
'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'
})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None, manipulate_machine=None):
""" Moves the entities for this challenge to the correct poses"""
# Determine where to perform the challenge
# Apparently, the indices here come from:
# (cabinet_pose, table_pose, cabinet_amcl, grasp_surface, room, default_place_area)
robot_pose = robot.base.get_location()
WORKSPACES.sort(
key=lambda ws: (ws.place_entity_conf.pose_estimate.frame.p
- robot_pose.frame.p).Norm())
closest_workspace = WORKSPACES[0]
rospy.loginfo(
"Closest workspace: grasp from '{grasp}' and place on '{place}'"
.format(
grasp=closest_workspace.grasp_entity_conf.entity_id,
place=closest_workspace.place_entity_conf.entity_id))
cabinet_id = closest_workspace.place_entity_conf.entity_id
table_id = closest_workspace.grasp_entity_conf.entity_id
# Update the world model by fitting the entities to the frame_stamped's given below.
robot.ed.update_entity(id=cabinet_id,
frame_stamped=closest_workspace.
place_entity_conf.pose_estimate)
robot.ed.update_entity(id=table_id,
frame_stamped=closest_workspace.
grasp_entity_conf.pose_estimate)
# Update designators
cabinet.id_ = closest_workspace.place_entity_conf.entity_id
# Update manipulate machine
manipulate_machine.table_designator.id_ = closest_workspace.grasp_entity_conf.entity_id
manipulate_machine.place_entity_designator.id_ = closest_workspace.place_entity_conf.entity_id
manipulate_machine.place_designator._area = closest_workspace.place_entity_conf.manipulation_volumes[
0]
manipulate_machine.place_designator.place_location_designator.id = closest_workspace.place_entity_conf.entity_id
manipulate_machine.cabinet.id_ = closest_workspace.place_entity_conf.entity_id
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table,
cb_args=[single_item]),
transitions={'done': 'NAV_TO_START'})
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(
robot, {cabinet: "in_front_of"},
cabinet),
transitions={
'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'
})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot, cabinet),
transitions={
'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'
})
smach.StateMachine.add("WRITE_PDF_SHELVES",
pdf_writer,
transitions={"done": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(
outcomes=['succeeded',
'failed'], # Outcomes of the iterator state
input_keys=[],
output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded')
with range_iterator:
smach.Iterator.set_contained_state(
'SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator, {
'succeeded': 'AT_END',
'failed': 'Aborted'
})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "manipulation")
def __init__(self):
super(ExploreHouse,
self).__init__(outcomes=['succeeded', 'aborted', 'preempted'])
# explore a single room
explore_1_room_sm = DoOnExitContainer(
outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=[
'map_image', 'map_resolution', 'map_origin', 'robot_radius',
'segmented_map', 'room_number', 'room_information_in_meter',
'completed_rooms'
],
output_keys=['completed_rooms'])
with explore_1_room_sm:
smach.StateMachine.add(
'HAVE_EXPLORE_PLAN?',
UDIfKey('coverage_path'), # already have a explore plan?
transitions={
'true':
'INSERT_CURRENT_POSE', # then skip to traverse poses
'false': 'GET_ROBOT_POSE'
})
smach.StateMachine.add('GET_ROBOT_POSE',
GetRobotPose(),
transitions={
'succeeded': 'PLAN_ROOM_EXPL',
'aborted': 'aborted'
})
smach.StateMachine.add('PLAN_ROOM_EXPL',
PlanRoomExploration(),
transitions={
'succeeded': 'GOTO_START_POSE',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'GOTO_START_POSE',
GoToPose(
dist_tolerance=cfg.LOOSE_DIST_TOLERANCE, # close enough
angle_tolerance=cfg.INF_ANGLE_TOLERANCE), # ignore yaw
transitions={
'succeeded': 'INSERT_START_POSE',
'preempted': 'preempted',
'aborted': 'INSERT_CURRENT_POSE'
},
remapping={'target_pose': 'start_pose'})
smach.StateMachine.add('INSERT_START_POSE',
UDInsertInList(0),
transitions={
'succeeded': 'TRAVERSE_POSES',
'aborted': 'aborted'
},
remapping={
'element': 'start_pose',
'list': 'coverage_path_pose_stamped'
})
smach.StateMachine.add(
'INSERT_CURRENT_POSE',
PrependCurrentPose(), # otherwise we can retake the remaining
transitions={
'succeeded':
'TRAVERSE_POSES', # plan wherever it is closer from us,
'aborted': 'aborted'
}, # instead of from the beginning
remapping={'path': 'coverage_path_pose_stamped'})
smach.StateMachine.add(
'TRAVERSE_POSES',
FollowWaypoints(),
transitions={
'succeeded': 'ROOM_COMPLETED',
'preempted': 'DEL_TRAVERSED_WPS',
'aborted': 'aborted'
},
remapping={'waypoints': 'coverage_path_pose_stamped'})
smach.StateMachine.add(
'DEL_TRAVERSED_WPS',
DelTraversedWPs(), # deleted the traversed waypoints so we can
transitions={'succeeded': 'preempted'
}, # resume exploration from where we left it
remapping={'waypoints': 'coverage_path_pose_stamped'})
smach.StateMachine.add('ROOM_COMPLETED',
RoomCompleted(),
transitions={'succeeded': 'succeeded'})
DoOnExitContainer.add_finally(
'CLEAR_EXPLORE_PLAN',
UDSetToNone('coverage_path'), # we only keep current
run_on=['succeeded',
'aborted']) # plan when preempted (so we can resume)
# 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',
'room_sequence'
],
output_keys=['completed_rooms'],
it=self.get_room_sequence,
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'])
with self:
smach.StateMachine.add(
'HAVE_SEGMENTED_MAP?',
UDHasKey('segmented_map'), # already have a rooms map?
transitions={
'true': 'HAVE_ROOM_SEQUENCE?', # use it for planning!
'false': 'SEGMENT_ROOMS'
})
smach.StateMachine.add('SEGMENT_ROOMS',
SegmentRooms(),
transitions={
'succeeded': 'GET_ROBOT_POSE',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'HAVE_ROOM_SEQUENCE?',
UDHasKey('room_sequence'), # already have a plan?
transitions={
'true': 'EXPLORE_HOUSE', # follow it!
'false': 'GET_ROBOT_POSE'
})
smach.StateMachine.add('GET_ROBOT_POSE',
GetRobotPose(),
transitions={
'succeeded': 'PLAN_ROOM_SEQ',
'aborted': 'aborted'
})
smach.StateMachine.add('PLAN_ROOM_SEQ',
PlanRoomSequence(),
transitions={
'succeeded': 'EXPLORE_HOUSE',
'aborted': 'aborted',
'preempted': 'preempted'
},
remapping={'input_map': 'map_image'})
smach.StateMachine.add('EXPLORE_HOUSE',
explore_house_it,
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted',
'preempted': 'preempted'
})
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
def bumpBuoy(surge, sway, yaw, timeout):
bump = smach.StateMachine(outcomes=['success', 'abort'])
numIt = 3
with bump:
bump.userdata.back = 180
bump.userdata.speed = 0.25
bump.userdata.backTime = 20
bump.userdata.zero = 0
bump.userdata.bumpTime = 10
it = smach.Iterator(outcomes=['success', 'abort'],
input_keys=[],
output_keys=[],
it=lambda: range(0, int(numIt)),
it_label='index',
exhausted_outcome='success')
with it:
container_sm = smach.StateMachine(
outcomes=['success', 'abort', 'continue'])
container_sm.userdata.moveTime = 6
container_sm.userdata.zero = 0
container_sm.userdata.speed = 0.3
container_sm.userdata.angle = None
container_sm.userdata.timeout = timeout
with container_sm:
smach.StateMachine.add('Move',
Move(surge, sway),
transitions={'success': 'ReLocalize'},
remapping={
'angle': 'zero',
'speed': 'speed',
'moveTime': 'moveTime'
})
smach.StateMachine.add('ReLocalize',
Localize(False, True),
transitions={
'success': 'RotateTo',
'failure': 'abort'
},
remapping={'outAngle': 'angle'})
smach.StateMachine.add('RotateTo',
RotateTo(yaw),
transitions={
'success': 'continue',
'abort': 'abort'
},
remapping={
'timeout': 'timeout',
'angle': 'angle'
})
smach.Iterator.set_contained_state("BumpIt",
container_sm,
loop_outcomes=['continue'])
smach.StateMachine.add("BUMP_IT",
it,
transitions={
'success': 'FinalBump',
'abort': 'abort'
})
smach.StateMachine.add("FinalBump",
Move(surge, sway),
transitions={'success': 'MoveBack'},
remapping={
'angle': 'zero',
'speed': 'speed',
'moveTime': 'bumpTime'
})
smach.StateMachine.add("MoveBack",
Move(surge, sway),
transitions={'success': 'success'},
remapping={
'angle': 'back',
'speed': 'speed',
'moveTime': 'backTime'
})
return bump
def main():
rospy.init_node('example_action_smach_state_machine')
#TEST CONCURRENCE STATES WITH ACTIONS--------------------------------------------------------------------------------------------------
test_con1 = smach.Concurrence(outcomes=['test_done'],
default_outcome = 'test_done',
input_keys = ['test_order'],
output_keys= ['test_output_order'],
child_termination_cb = lambda outcome_map : True,
outcome_cb = lambda outcome_map : 'test_done')
# Test Concurrence with goal and result callbacks that grab data from
# the userdata input key passed into the state machine
with test_con1:
def test_con1_result_cb(userdata, status, result):
if status == GoalStatus.SUCCEEDED:
rospy.loginfo(rospy.get_name() + ": The following data was returned from server %s" % str(result.sequence))
userdata.test_output_order = 6
return 'succeeded'
def test_con1_goal_cb(userdata, goal):
test_goal = ExampleGoal()
test_goal.order = userdata.test_order
return test_goal
smach.Concurrence.add('TestCon1', SimpleActionState('example_server',
ExampleAction,
input_keys=['test_order'],
output_keys=['test_output_order'],
goal_cb=test_con1_goal_cb,
result_cb=test_con1_result_cb))
smach.Concurrence.add('testTimeout',sleep(20))
test_con2 = smach.Concurrence(outcomes=['test_done'],
default_outcome = 'test_done',
input_keys = ['test_order'],
child_termination_cb = lambda outcome_map : True,
outcome_cb = lambda outcome_map : 'test_done')
with test_con2:
def test_con2_result_cb(userdata, status, result):
if status == GoalStatus.SUCCEEDED:
rospy.loginfo(rospy.get_name() + ": The following data was returned from server %s" % str(result.sequence))
return 'succeeded'
def test_con2_goal_cb(userdata, goal):
test_goal = ExampleGoal()
test_goal.order = userdata.test_order
return test_goal
smach.Concurrence.add('TestCon2', SimpleActionState('example_server',
ExampleAction,
input_keys=['test_order'],
goal_cb=test_con2_goal_cb,
result_cb=test_con2_result_cb))
smach.Concurrence.add('testTimeout',sleep(20))
test_con3 = smach.Concurrence(outcomes=['test_done'],
default_outcome = 'test_done',
child_termination_cb = lambda outcome_map : True,
outcome_cb = lambda outcome_map : 'test_done')
with test_con3:
def test_con3_result_cb(userdata, status, result):
if status == GoalStatus.SUCCEEDED:
rospy.loginfo(rospy.get_name() + ": The following data was returned from server %s" % str(result.sequence))
return 'succeeded'
test_goal = ExampleGoal()
test_goal.order = 7
smach.Concurrence.add('TestCon3', SimpleActionState('example_server',
ExampleAction,
goal=test_goal,
result_cb=test_con3_result_cb))
smach.Concurrence.add('testTimeout',sleep(20))
#SETUP ITERATOR STATE MACHINE--------------------------------------------------------------------------
sm_it = smach.StateMachine(outcomes=['outcome6'])
sm_it.userdata.nums = range(25, 88, 3)
sm_it.userdata.even_nums = []
sm_it.userdata.odd_nums = []
with sm_it:
tutorial_it = smach.Iterator(outcomes = ['succeeded','aborted'],
input_keys = ['nums', 'even_nums', 'odd_nums'],
it = lambda: range(0, len(sm_it.userdata.nums)),
output_keys = ['even_nums', 'odd_nums'],
it_label = 'index',
exhausted_outcome = 'succeeded')
with tutorial_it:
container_sm = smach.StateMachine(outcomes = ['succeeded','preempted','aborted','continue'],
input_keys = ['nums', 'index', 'even_nums', 'odd_nums'],
output_keys = ['even_nums', 'odd_nums'])
with container_sm:
#test wether even or odd with a conditional state
smach.StateMachine.add('EVEN_OR_ODD',
smach_ros.ConditionState(cond_cb = lambda ud:ud.nums[ud.index]%2,
input_keys=['nums', 'index']),
{'true':'ODD',
'false':'EVEN' })
#add even state using a callback state
@smach.cb_interface(input_keys=['nums', 'index', 'even_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def even_cb(ud):
rospy.sleep(1)
ud.even_nums.append(ud.nums[ud.index])
return 'succeeded'
smach.StateMachine.add('EVEN', smach.CBState(even_cb),
{'succeeded':'continue'})
#add odd state using a callback state
@smach.cb_interface(input_keys=['nums', 'index', 'odd_nums'],
output_keys=['odd_nums'],
outcomes=['succeeded'])
def odd_cb(ud):
rospy.sleep(1)
ud.odd_nums.append(ud.nums[ud.index])
return 'succeeded'
smach.StateMachine.add('ODD', smach.CBState(odd_cb),
{'succeeded':'continue'})
#close container_sm
smach.Iterator.set_contained_state('CONTAINER_STATE',
container_sm,
loop_outcomes=['continue'])
smach.StateMachine.add('TUTORIAL_IT',tutorial_it,
{'succeeded':'outcome6',
'aborted':'outcome6'})
#STATE MACHINE-----------------------------------------------------------------------------------------
# Create a SMACH state machine
sm_top = smach.StateMachine(outcomes=['outcome5'])
# Create userdata for top level state machine
sm_top.userdata.first_goal = 5
sm_top.userdata.pass_data = 0
# Open the top container
with sm_top:
# Add Concurrent States
smach.StateMachine.add('TestCon1', test_con1,
transitions={'test_done':'TestCon2'},
remapping={'test_order':'first_goal',
'test_output_order':'pass_data'})
smach.StateMachine.add('TestCon2', test_con2,
transitions={'test_done':'TestCon3'},
remapping={'test_order':'pass_data'})
smach.StateMachine.add('TestCon3', test_con3,
transitions={'test_done':'SUB'})
# Create and add a low tier statemachine
sm_sub = smach.StateMachine(outcomes=['outcome4'])
with sm_sub:
smach.StateMachine.add('FOO', example_states.Foo(),
transitions={'outcome1':'BAR',
'outcome2':'outcome4'})
smach.StateMachine.add('BAR', example_states.Bar(),
transitions={'outcome2':'FOO'})
# Add the low tier statemachine
smach.StateMachine.add('SUB', sm_sub,
transitions={'outcome4':'TUT_IT'})
# Add the low tier statemachine
smach.StateMachine.add('TUT_IT', sm_it,
transitions={'outcome6':'TestCon1'})
# Execute SMACH plan
sis = smach_ros.IntrospectionServer('smach_server', sm_top, '/SM_ROOT')
sis.start()
outcome = sm_top.execute()
rospy.spin()
sis.stop()
def __init__(self):
super(PickReachableObjs, self).__init__(
outcomes=['succeeded', 'aborted', 'preempted', 'tray_full'],
input_keys=['object_types'])
# pick a single object sm
pick_1_obj_sm = smach.StateMachine(
outcomes=[
'continue', 'succeeded', 'aborted', 'preempted', 'tray_full'
],
input_keys=['object_types', 'failures'])
pick_1_obj_sm.userdata.max_effort = cfg.GRIPPER_MAX_EFFORT
with pick_1_obj_sm:
smach.StateMachine.add('DETECT_OBJECTS',
ObjectDetection(),
transitions={
'succeeded': 'SELECT_TARGET',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add('SELECT_TARGET',
TargetSelection(),
transitions={
'have_target': 'PICKUP_OBJECT',
'no_targets': 'succeeded'
},
remapping={'target': 'object'})
smach.StateMachine.add('PICKUP_OBJECT',
PickupObject(),
transitions={
'succeeded': 'PLACE_ON_TRAY',
'preempted': 'preempted',
'aborted': 'RECORD_FAILURE'
})
smach.StateMachine.add('PLACE_ON_TRAY',
PlaceInTray(),
transitions={
'succeeded': 'CLEAR_FAILURES',
'preempted': 'preempted',
'aborted': 'CLEAR_GRIPPER',
'tray_full': 'tray_full'
})
smach.StateMachine.add('CLEAR_GRIPPER',
ClearGripper(),
transitions={
'succeeded': 'RECORD_FAILURE',
'preempted': 'aborted',
'aborted': 'aborted'
})
smach.StateMachine.add('RECORD_FAILURE',
RecordFailure(),
transitions={
'succeeded': 'continue',
'max_failures': 'aborted'
})
smach.StateMachine.add('CLEAR_FAILURES',
ClearFailures(),
transitions={'succeeded': 'continue'})
pick_reach_objs_it = smach.Iterator(
outcomes=['succeeded', 'preempted', 'aborted', 'tray_full'],
input_keys=['object_types', 'failures'],
output_keys=[],
it=range(25), # kind of while true
it_label='iteration',
exhausted_outcome='succeeded')
with pick_reach_objs_it:
smach.Iterator.set_contained_state('',
pick_1_obj_sm,
loop_outcomes=['continue'])
self.userdata.failures = {
} # keep track of pick failures for each object
with self:
smach.StateMachine.add('PICKUP_OBJECTS',
pick_reach_objs_it,
transitions={
'succeeded': 'FOLD_ARM',
'preempted': 'preempted',
'aborted': 'aborted'
})
smach.StateMachine.add('FOLD_ARM',
FoldArm(),
transitions={
'succeeded': 'DETECT_OBJECTS',
'preempted': 'preempted',
'aborted': 'aborted'
})
smach.StateMachine.add('DETECT_OBJECTS',
ObjectDetection(),
transitions={
'succeeded': 'SELECT_TARGET',
'aborted': 'aborted',
'preempted': 'preempted'
})
smach.StateMachine.add(
'SELECT_TARGET',
TargetSelection(
), # just used to check if there are objects left
transitions={
'have_target': 'PICKUP_OBJECTS', # restart picking if so
'no_targets': 'succeeded'
})
DoOnExitContainer.add_finally('CLEAR_P_SCENE',
ClearPlanningScene())
DoOnExitContainer.add_finally('FINAL_FOLD_ARM', FoldArm())
smach.StateMachine.add('GetDetectedCubes',
GetDetectedCubes(),
remapping={'object_names': 'object_names'},
transitions={
'succeeded': 'StackCubes',
'retry': 'ObjectDetection'
})
# Stack cubes sub state machine; iterates over the detected cubes and stack them over the one most in front of the arm
sc_it = smach.Iterator(
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=[
'base_cube_pose', 'base_cube_name', 'other_cubes', 'cube_height',
'support_surf'
],
output_keys=[],
it=lambda: sm.userdata.
other_cubes, # must be a lambda because we destroy the list
it_label='object_name',
exhausted_outcome='succeeded')
with sc_it:
sc_sm = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'aborted', 'continue'],
input_keys=[
'base_cube_pose', 'object_name', 'support_surf', 'cube_height'
],
output_keys=[])
with sc_sm:
smach.StateMachine.add('PickupObject',
PickupObject(),
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# start_waypoint = ds.EntityByIdDesignator(robot, id="manipulation_init_pose", name="start_waypoint")
pdf_writer = WritePdf(robot=robot)
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={'initialized': 'AWAIT_START',
'abort': 'Aborted'})
smach.StateMachine.add("AWAIT_START",
states.AskContinue(robot),
transitions={'continue': "MOVE_TABLE",
'no_response': 'AWAIT_START'})
@smach.cb_interface(outcomes=["done"])
def move_table(userdata=None):
""" 'Locks' a locking designator """
# For now, don't do anything
return "done"
# Move away the cabinet
robot.ed.update_entity(id="cabinet",
frame_stamped=FrameStamped(frame=kdl.Frame(kdl.Rotation(),
kdl.Vector(12.0, 0, 0)),
frame_id="map"))
# Determine where to perform the challenge
robot_pose = robot.base.get_location()
ENTITY_POSES.sort(key=lambda tup: (tup[0].frame.p - robot_pose.frame.p).Norm())
# Update the world model
robot.ed.update_entity(id=CABINET, frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=TABLE, frame_stamped=ENTITY_POSES[0][1])
return "done"
smach.StateMachine.add("MOVE_TABLE",
smach.CBState(move_table),
transitions={'done': 'NAV_TO_START'})
cabinet = ds.EntityByIdDesignator(robot, id=CABINET)
room = ds.EntityByIdDesignator(robot, id=ROOM)
smach.StateMachine.add("NAV_TO_START",
states.NavigateToSymbolic(robot,
{cabinet: "in_front_of"},
cabinet),
transitions={'arrived': 'INSPECT_SHELVES',
'unreachable': 'INSPECT_SHELVES',
'goal_not_defined': 'INSPECT_SHELVES'})
smach.StateMachine.add("INSPECT_SHELVES",
InspectShelves(robot),
transitions={'succeeded': 'WRITE_PDF_SHELVES',
'nothing_found': 'WRITE_PDF_SHELVES',
'failed': 'WRITE_PDF_SHELVES'})
smach.StateMachine.add("WRITE_PDF_SHELVES", pdf_writer, transitions={"done": "RANGE_ITERATOR"})
# Begin setup iterator
# The exhausted argument should be set to the prefered state machine outcome
range_iterator = smach.Iterator(outcomes=['succeeded', 'failed'], # Outcomes of the iterator state
input_keys=[], output_keys=[],
it=lambda: range(5),
it_label='index',
exhausted_outcome='succeeded')
with range_iterator:
single_item = ManipulateMachine(robot, pdf_writer=pdf_writer) # ToDo: add more pdf stuff
smach.Iterator.set_contained_state('SINGLE_ITEM',
single_item,
loop_outcomes=['succeeded', 'failed'])
smach.StateMachine.add('RANGE_ITERATOR', range_iterator,
{'succeeded': 'AT_END',
'failed': 'Aborted'})
# End setup iterator
smach.StateMachine.add('AT_END',
states.Say(robot, "Goodbye"),
transitions={'spoken': 'Done'})
ds.analyse_designators(self, "manipulation")
