def __init__(self, robot, grab_designator=None):
"""
Constructor
:param robot: robot object
:param grab_designator: EdEntityDesignator designating the item to grab. If not provided, a default one is
constructed (grabs the closest object in the volume of the surface)
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
# Create designators
self.empty_arm_designator = ds.UnoccupiedArmDesignator(
robot, {
"required_trajectories": ["prepare_grasp"],
"required_goals": ["carrying_pose"],
"required_gripper_types": [arms.GripperTypes.GRASPING]
},
name="empty_arm_designator")
self.grab_designator = ds.LockToId(robot=robot,
to_be_locked=grab_designator)
with self:
@smach.cb_interface(outcomes=["locked"])
def lock(userdata=None):
""" 'Locks' a locking designator """
# This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
self.grab_designator.lock()
if self.grab_designator.resolve():
rospy.loginfo("Current_item is now locked to {0}".format(
self.grab_designator.resolve().id))
return "locked"
smach.StateMachine.add("LOCK_ITEM",
smach.CBState(lock),
transitions={'locked': 'GRAB_ITEM'})
smach.StateMachine.add("GRAB_ITEM",
states.Grab(robot, self.grab_designator,
self.empty_arm_designator),
transitions={
'done': 'UNLOCK_ITEM_SUCCEED',
'failed': 'UNLOCK_ITEM_FAIL'
})
@smach.cb_interface(outcomes=["unlocked"])
def unlock(userdata=None):
""" 'Unlocks' a locking designator """
self.grab_designator.unlock()
return "unlocked"
smach.StateMachine.add("UNLOCK_ITEM_SUCCEED",
smach.CBState(unlock),
transitions={'unlocked': 'succeeded'})
smach.StateMachine.add("UNLOCK_ITEM_FAIL",
smach.CBState(unlock),
transitions={'unlocked': 'failed'})
def testcase2():
import smach
toplevel = smach.StateMachine(outcomes=['Done', 'Aborted'])
with toplevel:
@smach.cb_interface(outcomes=["succeeded", 'error'])
def execute(userdata):
return "succeeded"
smach.StateMachine.add('TEST1',
smach.CBState(execute),
transitions={'succeeded':'SUBLEVEL1',
'error' :"Aborted"})
sublevel1 = smach.StateMachine(outcomes=['Finished', 'Failed'])
with sublevel1:
smach.StateMachine.add('SUBTEST1',
smach.CBState(execute),
transitions={'succeeded':'SUBTEST2',
'error' :'Failed'})
smach.StateMachine.add('SUBTEST2',
smach.CBState(execute),
transitions={'succeeded':'Finished',
'error' :'Failed'})
smach.StateMachine.add('SUBLEVEL1',
sublevel1,
transitions={'Finished' :'Done',
'Failed' :'Aborted'})
visualize(toplevel, "testcase2", save_dot=True)
def testcase3():
import smach
testcase3 = smach.StateMachine(outcomes=['Done'])
with testcase3:
@smach.cb_interface(outcomes=["succeeded"])
def execute(userdata=None):
return "succeeded"
smach.StateMachine.add('TEST1',
smach.CBState(execute),
transitions={'succeeded': 'SUBLEVEL1'})
sublevel1 = smach.StateMachine(outcomes=['Finished'])
with sublevel1:
smach.StateMachine.add('SUBTEST1',
smach.CBState(execute),
transitions={'succeeded': 'SUBTEST2'})
smach.StateMachine.add('SUBTEST2',
smach.CBState(execute),
transitions={'succeeded': 'Finished'})
smach.StateMachine.add('SUBLEVEL1',
sublevel1,
transitions={'Finished': 'Done'})
visualize(testcase3, "testcase3", save_dot=True)
def __init__(self):
button_topic = rospy.get_param('~button_topic', '/drone/joy/buttons[4]')
button_topic_class, button_real_topic, self.button_eval_func = rostopic.get_topic_class(rospy.resolve_name(button_topic))
self.sub_button = rospy.Subscriber(button_real_topic, button_topic_class, self.button_cb)
self.last_button = None
self.button_pressed = False
self.button_released = False
self.land_action_ns = rospy.get_param('~land_action_ns', '/drone/land_action')
robot_odom_topic = rospy.get_param('~robot_odom_topic', '/drone/odom')
self.sub_odom = rospy.Subscriber(robot_odom_topic, Odometry, self.robot_odom_cb)
self.robot_current_pose = PoseStamped()
self.state_name_topic = rospy.get_param('~state_name_topic', '~state')
self.pub_state_name = rospy.Publisher(self.state_name_topic, String, queue_size = 10, latch = True)
robot_state_topic = rospy.get_param('~robot_state_topic', '/drone/flight_state')
self.last_known_flight_state = FlightState.Landed
self.sub_flight_state = rospy.Subscriber(robot_state_topic, FlightState, self.flight_state_cb)
self.flight_state_event = threading.Event()
landing_spot = rospy.get_param('landing_spot', {'x': float('nan'), 'y': float('nan'), 'z': float('nan'), 'tolerance': float('nan')})
self.landing_spot, self.landing_tolerance = self.get_landing_spot(**landing_spot)
if math.isnan(self.landing_spot.Norm()):
rospy.logwarn('Landing spot is not specified, allow landing anywhere')
self.sm = smach.StateMachine(outcomes = ['FINISH'])
with self.sm:
# smach.StateMachine.add('WAIT_USER',
# smach.CBState(self.check_button, cb_kwargs = {'context': self}),
# transitions = {'pressed': 'FOLLOW_POINTING',
# 'preempted': 'FINISH'})
smach.StateMachine.add('WAIT_USER',
smach.CBState(self.wait_for_flying, cb_kwargs = {'context': self}),
transitions = {'succeeded': 'FOLLOW_POINTING',
'aborted': 'FINISH',
'preempted': 'WAIT_USER'})
smach.StateMachine.add('FOLLOW_POINTING',
smach.CBState(self.wait_for_landing, cb_kwargs = {'context': self}),
transitions = {'succeeded': 'LAND',
'aborted': 'FOLLOW_POINTING',
'preempted': 'FINISH'})
smach.StateMachine.add('LAND',
smach_ros.SimpleActionState(self.land_action_ns, EmptyAction),
transitions = {'succeeded': 'WAIT_USER',
'preempted': 'FINISH',
'aborted': 'WAIT_USER'})
self.sm.register_start_cb(self.state_transition_cb, cb_args = [self.sm])
self.sm.register_transition_cb(self.state_transition_cb, cb_args = [self.sm])
self.sis = smach_ros.IntrospectionServer('smach_server', self.sm, '/SM_JOY')
def __init__(self, robot, order_type):
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
beverage_dest_desig = EdEntityDesignator(robot) #.id is overwritten by instruct_barman
with self:
@smach.cb_interface(outcomes=['spoken'])
def instruct_barman(userdata=None):
try:
order = ORDERS[order_type]
beverage_dest_desig.id = order['location']
robot.speech.speak("Barman, please put a {name} in my basket for table {location}".format(**order))
except KeyError:
rospy.logerr("No beverage in ORDERS")
return 'spoken'
smach.StateMachine.add( 'INSTRUCT_BARMAN',
smach.CBState(instruct_barman),
transitions={'spoken' :'AWAIT_PUT_ORDER_CONFIRMATION'})
smach.StateMachine.add( 'AWAIT_PUT_ORDER_CONFIRMATION',
states.WaitTime(robot, 8),
transitions={ 'waited' :'GOTO_ORDER_DESTINATION_1',
'preempted' :'failed'})
smach.StateMachine.add( 'GOTO_ORDER_DESTINATION_1', states.NavigateToWaypoint(robot, beverage_dest_desig, radius = WAYPOINT_RADIUS),
transitions={ 'arrived' :'SAY_TAKE_ORDER',
'unreachable' :'GOTO_ORDER_DESTINATION_2',
'goal_not_defined' :'GOTO_ORDER_DESTINATION_2'})
smach.StateMachine.add( 'GOTO_ORDER_DESTINATION_2', states.NavigateToWaypoint(robot, beverage_dest_desig, radius = WAYPOINT_RADIUS),
transitions={ 'arrived' :'SAY_TAKE_ORDER',
'unreachable' :'failed',
'goal_not_defined' :'failed'})
@smach.cb_interface(outcomes=['spoken'])
def instruct_guest(userdata=None):
try:
order = ORDERS[order_type]
robot.speech.speak("Dear guest at table {location}, you can get your {name} from my basket.".format(**order))
except KeyError:
rospy.logerr("No beverage in ORDERS")
return 'spoken'
smach.StateMachine.add( 'SAY_TAKE_ORDER',
smach.CBState(instruct_guest),
transitions={'spoken' :'AWAIT_TAKE_ORDER_CONFIRMATION'})
smach.StateMachine.add( 'AWAIT_TAKE_ORDER_CONFIRMATION',
states.WaitTime(robot, 5),
transitions={ 'waited' :'SAY_ENJOY_ORDER',
'preempted' :'failed'})
smach.StateMachine.add( 'SAY_ENJOY_ORDER',
states.Say(robot, ["Enjoy your {}".format(order_type)], block=False),
transitions={ 'spoken' :'succeeded'})
def __init__(self, robot, grab_designator=None):
""" Constructor
:param robot: robot object
:param grab_designator: EdEntityDesignator designating the item to grab. If not provided, a default one is
constructed (grabs the closest object in the volume of the surface)
"""
smach.StateMachine.__init__(self, outcomes=["succeeded", "failed"])
# Create designators
self.empty_arm_designator = ds.UnoccupiedArmDesignator(robot.arms, robot.leftArm, name="empty_arm_designator")
self.grab_designator = ds.LockToId(robot=robot, to_be_locked=grab_designator)
with self:
@smach.cb_interface(outcomes=["locked"])
def lock(userdata=None):
""" 'Locks' a locking designator """
# This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
self.grab_designator.lock()
if self.grab_designator.resolve():
rospy.loginfo("Current_item is now locked to {0}".format(self.grab_designator.resolve().id))
return "locked"
smach.StateMachine.add("LOCK_ITEM",
smach.CBState(lock),
transitions={'locked': 'ANNOUNCE_ITEM'})
smach.StateMachine.add("ANNOUNCE_ITEM",
states.Say(robot, EntityDescriptionDesignator(self.grab_designator,
name="current_item_desc"),
block=False),
transitions={'spoken': 'GRAB_ITEM'})
smach.StateMachine.add("GRAB_ITEM",
states.Grab(robot, self.grab_designator, self.empty_arm_designator),
transitions={'done': 'UNLOCK_ITEM_SUCCEED',
'failed': 'UNLOCK_ITEM_FAIL'})
@smach.cb_interface(outcomes=["unlocked"])
def lock(userdata=None):
""" 'Locks' a locking designator """
# This determines that self.current_item cannot not resolve to a new value until it is unlocked again.
self.grab_designator.unlock()
return "unlocked"
smach.StateMachine.add("UNLOCK_ITEM_SUCCEED",
smach.CBState(lock),
transitions={'unlocked': 'succeeded'})
smach.StateMachine.add("UNLOCK_ITEM_FAIL",
smach.CBState(lock),
transitions={'unlocked': 'failed'})
def get_smach_sm():
sm = smach.StateMachine(outcomes=['succeeded', 'aborted', 'preempted'])
sm.userdata.counter = 0
sm.userdata.cardboard_1 = (None, None)
sm.userdata.cardboard_2 = (None, None)
with sm:
smach.StateMachine.add(
'UNDOCK',
smach_ros.SimpleActionState('undock',
UndockAction,
goal=UndockGoal(rotate_in_place=True)),
{
'aborted': 'GO_TO_CARDBOARD_1'
}) # Why does this action always abort when it works every time???
smach.StateMachine.add(
'GO_TO_CARDBOARD_1',
smach_ros.SimpleActionState('move_base',
MoveBaseAction,
goal=tag_1_goal), {
'succeeded': 'TAKE_IMAGE_1',
'aborted': 'GO_TO_DOCK'
})
smach.StateMachine.add('TAKE_IMAGE_1',
smach.CBState(cardboard_imaging_cb),
transitions={'succeeded': 'GO_TO_CARDBOARD_2'},
remapping={'classification': 'cardboard_1'})
smach.StateMachine.add(
'GO_TO_CARDBOARD_2',
smach_ros.SimpleActionState('move_base',
MoveBaseAction,
goal=tag_2_goal), {
'succeeded': 'TAKE_IMAGE_2',
'aborted': 'GO_TO_DOCK'
})
smach.StateMachine.add('TAKE_IMAGE_2',
smach.CBState(cardboard_imaging_cb),
transitions={'succeeded': 'GO_TO_DOCK'},
remapping={'classification': 'cardboard_2'})
smach.StateMachine.add(
'GO_TO_DOCK',
smach_ros.SimpleActionState('move_base',
MoveBaseAction,
goal=dock_goal), {'succeeded': 'DOCK'})
smach.StateMachine.add('DOCK',
smach_ros.SimpleActionState('dock', DockAction),
{'succeeded': 'succeeded'})
return sm
def get_pub_head_registration(self):
@smach.cb_interface(input_keys=['cheek_pose'],
output_keys=[],
outcomes=['succeeded'])
def pub_head_registration(ud):
cheek_pose_base_link = self.tf_list.transformPose(
"/base_link", ud.cheek_pose)
# find the center of the ellipse given a cheek click
cheek_transformation = np.mat(
tf_trans.euler_matrix(2.6 * np.pi / 6, 0, 0, 'szyx'))
cheek_transformation[0:3, 3] = np.mat([-0.08, -0.04, 0]).T
cheek_pose = PoseConverter.to_homo_mat(cheek_pose_base_link)
#b_B_c[0:3,0:3] = np.eye(3)
norm_xy = cheek_pose[0:2, 2] / np.linalg.norm(cheek_pose[0:2, 2])
head_rot = np.arctan2(norm_xy[1], norm_xy[0])
cheek_pose[0:3,
0:3] = tf_trans.euler_matrix(0, 0, head_rot,
'sxyz')[0:3, 0:3]
self.cheek_pub.publish(
PoseConverter.to_pose_stamped_msg("/base_link", cheek_pose))
ell_center = cheek_pose * cheek_transformation
self.ell_center_pub.publish(
PoseConverter.to_pose_stamped_msg("/base_link", ell_center))
# create an ellipsoid msg and command it
ep = EllipsoidParams()
ep.e_frame.transform = PoseConverter.to_tf_msg(ell_center)
ep.height = 0.924
ep.E = 0.086
self.ep_pub.publish(ep)
return 'succeeded'
return smach.CBState(pub_head_registration)
def testcase1():
import smach
sm = smach.StateMachine(outcomes=['Done', 'Aborted'])
with sm:
@smach.cb_interface(outcomes=["succeeded"])
def execute(userdata):
return "succeeded"
smach.StateMachine.add('TEST1',
smach.CBState(execute),
transitions={'succeeded':'TEST2'})
smach.StateMachine.add('TEST2',
smach.CBState(execute),
transitions={'succeeded':'Done'})
visualize(sm, "testcase1")
def __init__(self, robot):
smach.StateMachine.__init__(self,
outcomes=["Done", "Aborted", "Failed"])
self.robot = robot
with self:
smach.StateMachine.add("RESET_SPINDLE_HEAD_UP",
states.ResetSpindleHeadUp(robot),
transitions={'done': 'RESET_REASONER'})
@smach.cb_interface(outcomes=['done'])
def reset_reasoner(*args, **kwargs):
robot.reasoner.reset()
return 'done'
smach.StateMachine.add("RESET_REASONER",
smach.CBState(reset_reasoner),
transitions={"done": "LOOK"})
smach.StateMachine.add("LOOK",
states.LookAtItem(
robot, ["face_recognition"],
states.LookAtItem.face_in_front_query),
transitions={
'Done': 'Done',
'Aborted': 'Aborted',
'Failed': 'Failed'
})
def get_trajectory_generator(self):
@smach.cb_interface(input_keys=['start_click', 'end_click'],
output_keys=['start_traj_frame', 'end_traj_frame'],
outcomes=['succeeded'])
def generate_trajectory(ud):
b_B_s = PoseConverter.to_homo_mat(ud.start_click)
b_B_e = PoseConverter.to_homo_mat(ud.end_click)
s_B_e = (b_B_s**-1) * b_B_e
b_normal = b_B_s[:3, 2] / np.linalg.norm(b_B_s[:3, 2])
s_vel = np.mat([s_B_e[0, 3], s_B_e[1, 3], 0]).T
s_vel = s_vel / np.linalg.norm(s_vel)
b_vel = b_B_s[:3, :3].T * s_vel
b_ortho = np.mat(np.cross(b_normal.T, b_vel.T)).T
b_ortho = b_ortho / np.linalg.norm(b_ortho)
b_R_traj = np.vstack([b_vel.T, b_ortho.T, b_normal.T])
b_p_start = b_B_s[:3, 3]
b_p_end = b_B_e[:3, 3]
b_p_end = 3 #TODO TODO
self.start_frame_pub.publish(
PoseConverter.to_pose_stamped_msg(
ud.start_click.header.frame_id, (b_p_start, b_R_traj)))
self.end_frame_pub.publish(
PoseConverter.to_pose_stamped_msg(
ud.start_click.header.frame_id, (b_p_end, b_R_traj)))
ud.start_traj_frame = (b_p_start, b_R_traj)
ud.end_traj_frame = (b_p_end, b_R_traj)
return 'succeeded'
return smach.CBState(generate_trajectory)
def __init__(self, robot, room_grammar, roomw, cleanup_locationsw):
smach.StateMachine.__init__(self, outcomes=["done"])
"""
Ask the operator which room has to be cleaned
"""
hmi_result_des = ds.VariableDesignator(resolve_type=hmi.HMIResult,
name="hmi_result_des")
room_name_des = ds.FuncDesignator(ds.AttrDesignator(
hmi_result_des, "semantics", resolve_type=unicode),
str,
resolve_type=str)
@smach.cb_interface(outcomes=['done'])
def write_room(ud, des_read, des_write):
# type: (object, ds.Designator, ds.Designator) -> str
assert (ds.is_writeable(des_write))
assert (des_write.resolve_type == des_read.resolve_type)
des_write.write(des_read.resolve())
return 'done'
with self:
smach.StateMachine.add("ASK_WHICH_ROOM",
Say(robot,
"Which room should I clean for you?",
block=True),
transitions={"spoken": "HEAR_ROOM"})
smach.StateMachine.add("HEAR_ROOM",
HearOptionsExtra(
robot, room_grammar,
ds.writeable(hmi_result_des)),
transitions={
"heard": "SAY_HEARD_CORRECT",
"no_result": "ASK_WHICH_ROOM"
})
smach.StateMachine.add(
"SAY_HEARD_CORRECT",
Say(robot,
"I understood that the {room} should be cleaned, is this correct?",
room=room_name_des,
block=True),
transitions={"spoken": "HEAR_CORRECT"})
smach.StateMachine.add("HEAR_CORRECT",
AskYesNo(robot),
transitions={
"yes": "FILL_LOCATIONS",
"no": "ASK_WHICH_ROOM",
"no_result": "ASK_WHICH_ROOM"
})
smach.StateMachine.add("FILL_LOCATIONS",
RoomToCleanUpLocations(
challenge_knowledge, room_name_des,
cleanup_locationsw),
transitions={"done": "WRITE_ROOM"})
smach.StateMachine.add('WRITE_ROOM',
smach.CBState(
write_room,
cb_args=[room_name_des, roomw]),
transitions={'done': 'done'})
def __init__(self, robot, category_grammar, categoryw):
smach.StateMachine.__init__(self, outcomes=["done"])
hmi_result_des = ds.VariableDesignator(resolve_type=hmi.HMIResult,
name="hmi_result_des")
category_des = ds.FuncDesignator(ds.AttrDesignator(
hmi_result_des, "semantics", resolve_type=unicode),
str,
resolve_type=str)
@smach.cb_interface(outcomes=['done'])
def write_category(ud, des_read, des_write):
# type: (object, ds.Designator, ds.Designator) -> str
assert (ds.is_writeable(des_write))
assert (des_write.resolve_type == des_read.resolve_type)
des_write.write(des_read.resolve())
return 'done'
with self:
smach.StateMachine.add(
"ASK_WHERE_TO_DROP",
Say(robot,
"Please look at the object in my gripper and tell me"
"which category it is. If it should be thrown away,"
"call it trash",
block=True),
transitions={"spoken": "HEAR_LOCATION"})
smach.StateMachine.add("HEAR_LOCATION",
HearOptionsExtra(
robot, category_grammar,
ds.writeable(hmi_result_des)),
transitions={
"heard": "SAY_HEARD_CORRECT",
"no_result": "ASK_WHERE_TO_DROP"
})
smach.StateMachine.add(
"SAY_HEARD_CORRECT",
Say(robot,
"I understood that the object is of category {category}, is this correct?",
category=category_des,
block=True),
transitions={"spoken": "HEAR_CORRECT"})
smach.StateMachine.add("HEAR_CORRECT",
AskYesNo(robot),
transitions={
"yes": "WRITE_CATEGORY",
"no": "ASK_WHERE_TO_DROP",
"no_result": "ASK_WHERE_TO_DROP"
})
smach.StateMachine.add('WRITE_CATEGORY',
smach.CBState(
write_category,
cb_args=[category_des, categoryw]),
transitions={'done': 'done'})
def main():
rospy.init_node("pepper_imitation_game_node")
top_sm = smach.Concurrence(outcomes = ['game_success', 'game_error', 'game_canceled'],
default_outcome = 'game_error', child_termination_cb = game_termination_cb, outcome_cb = game_outcome_cb)
with top_sm:
main_sm = smach.StateMachine(outcomes = ['main_success', 'main_failed', 'main_preempted'])
with main_sm:
smach.StateMachine.add('SAY_HI', smach.CBState(say_hi), transitions = {'finished' : 'WAIT_USER_INPUT'})
smach.StateMachine.add('WAIT_USER_INPUT', WaitUserInput(), transitions={ 'start':'INIT_GAME', 'stop':'main_success', 'preempted': 'GAME_STOPPED', 'waiting':'WAIT_USER_INPUT'})
smach.StateMachine.add('INIT_GAME', smach.CBState(init_game), transitions = {'finished' : 'GAME_ITERATION'})
smach.StateMachine.add('GAME_ITERATION', GameIteration(), transitions={ 'continue':'SYNC_MUSIC', 'game_over':'END_SESSION', 'preempted': 'GAME_STOPPED'},
remapping={'previous_imitation_succeeded':'game_state_result',
'synchro_time':'game_state_synchro_time',
'next_pose':'game_state_pose'})
smach.StateMachine.add('SYNC_MUSIC', smach_ros.MonitorState("pepper_imitation/audio_player_progress", \
std_msgs.msg.Float32, synchronize_song, input_keys = ['synchro_time']), transitions = {'valid':'SYNC_MUSIC', 'invalid':'SEND_POSE', 'preempted': 'GAME_STOPPED'},
remapping = {'synchro_time':'game_state_synchro_time'})
smach.StateMachine.add('SEND_POSE', smach.CBState(send_pose), transitions = {'finished' : 'CHECK_POSE'}, remapping = {'pose':'game_state_pose'})
smach.StateMachine.add('CHECK_POSE', CheckPoseState(), transitions={'waiting':'CHECK_POSE', 'finished':'GIVE_FEEDBACK', 'no_skeleton':'GET_SKELETON', 'preempted': 'GAME_STOPPED'}, remapping = {'detection_succeeded':'game_state_result'})
smach.StateMachine.add('GIVE_FEEDBACK', smach.CBState(give_feedback), transitions = {'finished' : 'GAME_ITERATION'}, remapping = {'positive_feedback':'game_state_result'})
smach.StateMachine.add('END_SESSION', smach.CBState(end_session), transitions = {'finished' : 'WAIT_USER_INPUT'})
smach.StateMachine.add('GAME_STOPPED', smach.CBState(game_stopped), transitions = {'finished' : 'main_preempted'})
skeleton_sm = smach.StateMachine(outcomes = ['skeleton_found', 'timeout', 'preempted'])
with skeleton_sm:
smach.StateMachine.add('WAIT_SKELETON_INIT', smach.CBState(wait_skeleton_init), transitions = {'finished' : 'WAIT_SKELETON'})
smach.StateMachine.add('WAIT_SKELETON', WaitSkeletonState(), transitions={ 'skeleton_found':'SKELETON_FOUND', 'skeleton_not_found':'SKELETON_NOT_FOUND', 'waiting':'WAIT_SKELETON', 'preempted': 'preempted' })
smach.StateMachine.add('SKELETON_FOUND', smach.CBState(skeleton_found), transitions = {'finished' : 'skeleton_found'})
smach.StateMachine.add('SKELETON_NOT_FOUND', smach.CBState(skeleton_not_found), transitions = {'finished' : 'timeout'})
smach.StateMachine.add("GET_SKELETON", skeleton_sm, transitions = { 'skeleton_found':'INIT_GAME', 'timeout':'main_failed', 'preempted':'GAME_STOPPED' })
smach.Concurrence.add('GAME', main_sm)
smach.Concurrence.add('USER_EXIT_GAME', smach_ros.MonitorState("/pepper_imitation/cmd_user", pepper_imitation.msg.UserCommand, check_user_exit))
introspection_server = smach_ros.IntrospectionServer('pepper_imitation_game_state', top_sm, '/PEPPER_IMITATION_ROOT')
introspection_server.start()
outcome = top_sm.execute();
rospy.spin()
introspection_server.stop();
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'failed'])
self.robot = robot
@smach.cb_interface(outcomes=['done'])
def look_at_standing_person(userdata=None):
robot.head.look_at_standing_person()
return 'done'
with self:
smach.StateMachine.add('LOOK_AT_OPERATOR',
smach.CBState(look_at_standing_person),
transitions={'done': 'SAY_LOOK_AT_ME'})
smach.StateMachine.add(
'SAY_LOOK_AT_ME',
states.Say(
robot,
"Please stand one meter in front of me and look at me while I \
learn to recognize your face.",
block=True),
transitions={'spoken': 'LEARN_PERSON'})
smach.StateMachine.add(
'LEARN_PERSON',
states.LearnPerson(
robot, name_designator=ds.VariableDesignator("operator")),
transitions={
'succeeded_learning': 'SAY_OPERATOR_LEARNED',
'failed_learning': 'SAY_FAILED_LEARNING',
'timeout_learning': 'SAY_FAILED_LEARNING'
})
smach.StateMachine.add(
'SAY_FAILED_LEARNING',
states.Say(
robot,
"I could not learn my operator's face. Let me try again.",
block=True),
transitions={'spoken': 'failed'})
smach.StateMachine.add(
'SAY_OPERATOR_LEARNED',
states.Say(
robot,
"Now i know what you look like. Please go mix with the crowd."
),
transitions={'spoken': 'succeeded'})
def build_row_nav(self):
row_goal = navigate_rowGoal()
row_goal.desired_offset_from_row = 0
row_goal.distance_scale = -0.35
row_goal.forward_velcoity = 0.8
row_goal.headland_timeout = 5
row_goal.P = 0.3
row_nav = build_row_nav_sm(row_goal, 2)
#length_in, length_out, width, turn_radius , direction_l,vel_fw,vel_turn, fix_offset):
uturn_right = build_u_turn_sm(7, 3, 9.5, 3, False, 0.4, 0.4, 0)
uturn_left = build_u_turn_sm(7, 3, 8.1, 3, True, 0.4, 0.4, 0)
main_sm = smach.StateMachine(["succeeded", "aborted", "preempted"])
main_sm.userdata.next_turn = "left"
with main_sm:
smach.StateMachine.add(
"NAVIGATE_IN_ROW",
row_nav,
transitions={"succeeded": "TURN_SELECTOR"},
)
smach.StateMachine.add("TURN_SELECTOR",
smach.CBState(self.on_turn_selection,
outcomes=["left", "right"],
input_keys=["next_turn_in"],
output_keys=["next_turn_out"
]),
transitions={
"left": "MAKE_TURN_LEFT",
"right": "MAKE_TURN_RIGHT"
},
remapping={
"next_turn_in": "next_turn",
"next_turn_out": "next_turn"
})
smach.StateMachine.add(
"MAKE_TURN_RIGHT",
uturn_right,
transitions={"succeeded": "NAVIGATE_IN_ROW"})
smach.StateMachine.add(
"MAKE_TURN_LEFT",
uturn_left,
transitions={"succeeded": "NAVIGATE_IN_ROW"})
return main_sm
def get_nav_approach_pose(self):
@smach.cb_interface(input_keys=['head_click_pose'],
output_keys=['nav_pose_ps'],
outcomes=['succeeded', 'tf_failure'])
def make_approach_pose(ud):
frame_B_head = util.pose_msg_to_mat(ud.head_click_pose)
base_B_frame = self.get_transform(
"base_link", ud.head_click_pose.header.frame_id)
if base_B_frame is None:
return 'tf_failure'
base_B_head = base_B_frame * frame_B_head
norm_z = np.array([base_B_head[0, 2], base_B_head[1, 2], 0])
norm_z /= np.linalg.norm(norm_z)
base_B_head[:3, :3] = np.mat([[-norm_z[0], norm_z[1], 0],
[-norm_z[1], -norm_z[0], 0],
[0, 0, 1]])
# offset in the x-direction by the given parameter
offset_B_base = self.get_transform(self.base_offset_frame,
"base_link")
if offset_B_base is None:
return 'tf_failure'
nav_pose = base_B_head * offset_B_base
print nav_pose
nav_pose[:4, 3] = nav_pose * np.mat(
[[-self.nav_approach_dist, 0, 0, 1]]).T
print nav_pose
now = rospy.Time.now()
nav_pose_ps = util.pose_mat_to_stamped_msg('base_link', nav_pose,
now)
self.tf_listener.waitForTransform("/map",
"base_link",
now,
timeout=rospy.Duration(20))
nav_pose_map_ps = self.tf_listener.transformPose(
"/map", nav_pose_ps)
#self.nav_pub.publish(util.pose_mat_to_stamped_msg('base_link', base_B_head, rospy.Time.now()))
print base_B_head, base_B_head.T * base_B_head
self.nav_pub.publish(nav_pose_map_ps)
ud.nav_pose_ps = nav_pose_map_ps
return 'succeeded'
return smach.CBState(make_approach_pose)
def __init__(self, robot, personNameDesLocal):
@smach.cb_interface(outcomes=['spoken'])
def saySearchingOperatorCB(userdata=None):
printOk("saySearchingOperatorCB")
robot.speech.speak("I am searching for " +
personNameDesLocal.resolve() + "!",
block=False)
return 'spoken'
smach.StateMachine.__init__(
self, outcomes=['container_success', 'container_failed'])
with self:
smach.StateMachine.add(
'NAV_TO_WAYPOINT',
states.NavigateToWaypoint(
robot,
EdEntityDesignator(robot,
id=challenge_knowledge.wp_test_nav)),
transitions={
'arrived': 'SAY_SEARCHING_OPERATOR',
'unreachable': 'SAY_FAILED_WAYPOINT',
'goal_not_defined': 'SAY_FAILED_WAYPOINT'
})
smach.StateMachine.add(
'SAY_FAILED_WAYPOINT',
states.Say(robot, "Failed reaching the waypoint.", block=True),
transitions={'spoken': 'SAY_SEARCHING_OPERATOR'})
smach.StateMachine.add('SAY_SEARCHING_OPERATOR',
smach.CBState(saySearchingOperatorCB),
transitions={'spoken': 'SAY_UNFINSHED'})
smach.StateMachine.add('SAY_UNFINSHED',
states.Say(robot,
"This part is not finished yet.",
block=False),
transitions={'spoken': 'container_success'})
def get_sm_basic(self):
sm = smach.StateMachine(
outcomes=['succeeded', 'preempted', 'shutdown'])
with sm:
smach.StateMachine.add('INPUT_START_CLICK',
ClickMonitor(),
transitions={
'click': 'PUBLISH_CONTROL_FRAME',
'shutdown': 'shutdown'
},
remapping={'click_pose': 'start_click'
}) # output (PoseStamped)
@smach.cb_interface(input_keys=['start_click'],
outcomes=['succeeded', 'failed'])
def publish_control_frame(ud):
if ud.start_click.pose.position.x == -10000.0:
return 'failed'
pose = ud.start_click
pose.header.stamp = rospy.Time(0)
click_torso = self.tf_listener.transformPose(
"/torso_lift_link", pose)
self.tf_pub.update_pose(click_torso)
rospy.sleep(1)
self.arm.set_tip_frame("/contact_control_frame")
#self.arm.set_motion_gains(p_trans=[300, 300, 100])
self.arm.update_gains()
return 'succeeded'
smach.StateMachine.add('PUBLISH_CONTROL_FRAME',
smach.CBState(publish_control_frame),
transitions={
'succeeded': 'INPUT_START_CLICK',
'failed': 'INPUT_START_CLICK'
})
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)
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):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
# - - - - - - - - - - - - - - - - - - - Callback States - - - - - - - - - - - - - - - - - - -
@smach.cb_interface(outcomes=['spoken'])
def sayHelloCB(userdata=None):
printOk("sayHelloCB")
robot.speech.speak("Hello " + personNameDes.resolve() + "!",
block=False)
return 'spoken'
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
with self:
smach.StateMachine.add('INITIALIZE',
states.Initialize(robot),
transitions={
'initialized': 'INIT_WM',
'abort': 'Aborted'
})
smach.StateMachine.add(
"INIT_WM",
states.InitializeWorldModel(robot),
transitions={'done': 'ENTER_ROOM_CONTAINER'})
# smach.StateMachine.add( 'SELECT_NEXT_CONTAINER',
# test_states.SelectNextContainer(robot, containerResultDes),
# transitions={ 'go_to_enter_room':'ENTER_ROOM_CONTAINER',
# 'go_to_wait_person':'WAIT_PERSON_CONTAINER',
# 'go_to_pick_up':'PICK_UP_CONTAINER',
# 'go_to_recognize_people':'RECOGNIZE_PEOPLE_CONTAINER',
# 'go_to_search_people':'SEARCH_PEOPLE_CONTAINER',
# 'go_to_end_challenge':'END_CHALLENGE'})
# Navigation test
smach.StateMachine.add('ENTER_ROOM_CONTAINER',
EnterRoomContainer(robot),
transitions={
'container_success':
'WAIT_PERSON_CONTAINER',
'container_failed':
'WAIT_PERSON_CONTAINER'
})
# Human Interaction Test
smach.StateMachine.add('WAIT_PERSON_CONTAINER',
WaitPersonContainer(robot),
transitions={
'container_success':
'LEARN_NAME_CONTAINER',
'container_failed':
'WAIT_PERSON_CONTAINER'
})
# Speech Test
smach.StateMachine.add('LEARN_NAME_CONTAINER',
LearnNameContainer(robot, personNameDes),
transitions={
'container_failed': 'SAY_HELLO',
'container_success': 'SAY_HELLO'
})
smach.StateMachine.add(
'SAY_HELLO',
smach.CBState(sayHelloCB),
transitions={'spoken': 'LEARN_FACE_CONTAINER'})
# Face Learning Test
smach.StateMachine.add('LEARN_FACE_CONTAINER',
LearnFaceContainer(robot, personNameDes),
transitions={
'container_success':
'RECOGNIZE_PEOPLE_CONTAINER',
'container_failed':
'RECOGNIZE_PEOPLE_CONTAINER'
})
# Face Recognition Test
smach.StateMachine.add('RECOGNIZE_PEOPLE_CONTAINER',
RecognizePeopleContainer(robot),
transitions={
'container_success':
'PICK_UP_CONTAINER',
'container_failed': 'PICK_UP_CONTAINER'
})
# Manipulation Test
smach.StateMachine.add('PICK_UP_CONTAINER',
PickUpContainer(robot, objectsIDsDes),
transitions={
'container_success':
'SEARCH_PEOPLE_CONTAINER',
'container_failed':
'SEARCH_PEOPLE_CONTAINER'
})
# Face Recogniton Test
smach.StateMachine.add('SEARCH_PEOPLE_CONTAINER',
SearchPeopleContainer(robot, personNameDes),
transitions={
'container_success': 'END_CHALLENGE',
'container_failed': 'END_CHALLENGE'
})
smach.StateMachine.add('END_CHALLENGE',
states.Say(
robot,
"My work here is done, goodbye!"),
transitions={'spoken': 'Done'})
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 create_sm():
sm = smach.StateMachine(outcomes=['success', 'failure'])
sm.userdata.pose = [0.0, 0.0, 0.0]
sm.userdata.start_time = []
sm.userdata.stop_time = []
with sm:
##########
#START: BASIC FUNCTION
##########
@smach.cb_interface(outcomes=['success', 'failure'])
def start_cb(userdata):
try:
rospy.sleep(10)
print("First state.")
return 'success'
except:
return 'failure'
smach.StateMachine.add(
'START',
smach.CBState(start_cb),
transitions={
'success':
'PLAY1', #'SETPOSE2', #'SETPOSE', #'FOLLOWPERSON', #'MOVEARM',
'failure': 'failure'
})
smach.StateMachine.add(
'PLAY1',
RobotPlay(
path=
"/home/roboworks/erasersedu_ws/src/carry_luggage/sounds/R2D2a.wav"
),
transitions={
'success': 'Head for',
'failure': 'failure'
}) #R2-D2(Start)
smach.StateMachine.add('Head for',
RobotSay(message="I will go there soon",
delay=8),
transitions={
'success': 'SETPOSE2',
'failure': 'failure'
})
#Move rel example
@smach.cb_interface(outcomes=['success', 'failure'],
input_keys=['pose'],
output_keys=['pose'])
def set_pose_cb(userdata):
try:
userdata.pose = [-0.853, 0.393,
-2.935] #[-0.683, 0.393, -2.935]
return 'success'
except:
return 'failure'
smach.StateMachine.add('SETPOSE2',
smach.CBState(set_pose_cb),
transitions={
'success': 'MOVEBASE2',
'failure': 'failure'
})
smach.StateMachine.add('MOVEBASE2',
MoveBase(mode='abs'),
transitions={
'success': 'SAY',
'timeout': 'failure',
'failure': 'failure'
}) #MOVEARM
##Speech##
smach.StateMachine.add('SAY',
RobotSay(message="Hello, I am Turtlebot.",
delay=3),
transitions={
'success': 'help?',
'failure': 'failure'
})
smach.StateMachine.add('help?',
RobotYesNo(message="Shall I help you?"),
transitions={
'yes': 'carry_luggage?',
'no': 'PLAY2',
'failure': 'CONFIRMATION'
})
smach.StateMachine.add(
'carry_luggage?',
RobotYesNo(message="Shall I carry your luggage?"),
transitions={
'yes': 'MOVEARM',
'no': 'New_task',
'failure': 'CONFIRMATION'
})
smach.StateMachine.add(
'New_task',
RobotSay(message="Sorry.I have no other features yet."),
transitions={
'success': 'help?',
'failure': 'failure'
})
smach.StateMachine.add('CONFIRMATION',
RobotYesNo(message="Can you hear me?"),
transitions={
'yes': 'help?',
'no': 'CONFIRMATION',
'failure': 'failure'
})
##Speech##
##ARM##
smach.StateMachine.add('MOVEARM',
MoveArm(target='vertical',
pose=[0.0, 0.0, 0.0, 0.0, 0.0],
delay=3),
transitions={
'success': 'draw_out1',
'timeout': 'failure',
'failure': 'failure'
}) #MOVEARM2
smach.StateMachine.add('draw_out1',
MoveArm(target='other',
pose=[0.0, 0.0, 0.79, 1.4, 0.655],
delay=8),
transitions={
'success': 'draw_out2',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add('draw_out2',
MoveArm(target='other',
pose=[-0.4, 2.0, 0.79, 1.4, 0.655],
delay=8),
transitions={
'success': 'draw_out3',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add('draw_out3',
MoveArm(target='other',
pose=[-0.4, 2.0, 0.79, -1.43, 0.655],
delay=10),
transitions={
'success': 'draw_out4',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add('draw_out4',
MoveArm(target='other',
pose=[-0.4, 2.0, 0.79, 1.4, 0.655],
delay=6),
transitions={
'success': 'draw_out5',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add('draw_out5',
MoveArm(target='other',
pose=[-0.4, 1.0, 0.79, 1.4, 0.655],
delay=6),
transitions={
'success': 'MOVEARM2',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add('MOVEARM2',
MoveArm(target='front2_test',
pose=[0.0, 0.0, 0.0, 0.0, 0.0],
delay=9),
transitions={
'success': 'SAY2',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add(
'SAY2',
RobotSay(message="Please give your luggage to me"),
transitions={
'success': 'MOVEARM3',
'failure': 'failure'
})
smach.StateMachine.add('MOVEARM3',
MoveArm(target='front_test',
pose=[0.0, 0.0, 0.0, 0.0, 0.0],
delay=10),
transitions={
'success': 'MOVEARM4',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add('MOVEARM4',
MoveArm(target='carrying',
pose=[0.0, 0.0, 0.0, 0.0, 0.0],
delay=6),
transitions={
'success': 'FOLLOWPERSON',
'timeout': 'failure',
'failure': 'failure'
})
##ARM##
##Follow me##
smach.StateMachine.add('FOLLOWPERSON',
FollowPerson(delay=60),
transitions={
'success': 'MOVEARM5',
'timeout': 'failure',
'failure': 'failure'
})
##hand over,Return##
smach.StateMachine.add('MOVEARM5',
MoveArm(target='front3_test',
pose=[0.0, 0.0, 0.0, 0.0, 0.0],
delay=7),
transitions={
'success': 'Take',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add('Take',
RobotSay(message="Please take your luggage",
delay=7),
transitions={
'success': 'put_away1',
'failure': 'failure'
})
smach.StateMachine.add('put_away1',
MoveArm(target='front2',
pose=[0.0, 0.0, 0.0, 1.0, 0.0],
delay=8),
transitions={
'success': 'put_away2',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add('put_away2',
MoveArm(target='other',
pose=[0.0, 2.0, 0.79, 1.0, 0.655],
delay=9),
transitions={
'success': 'MOVEARM7',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add('MOVEARM7',
MoveArm(target='vertical',
pose=[0.0, 0.0, 0.0, 0.0, 0.0],
delay=10),
transitions={
'success': 'once_again?',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add(
'once_again?',
RobotYesNo(message="What else do you want me to do?"),
transitions={
'yes': 'Return',
'no': 'mission_complete!',
'failure': 'mission_complete!'
})
smach.StateMachine.add('mission_complete!',
RobotSay(message="mission complete!", delay=10),
transitions={
'success': 'mission_complete!_Return',
'failure': 'failure'
})
smach.StateMachine.add(
'mission_complete!_Return',
RobotSay(message="I will return to the original position.",
delay=7),
transitions={
'success': 'SETPOSE3',
'failure': 'failure'
})
##hand over,Return##
##once again##
smach.StateMachine.add('Return',
RobotSay(message="I'll be back."),
transitions={
'success': 'SETPOSE_n',
'failure': 'failure'
}) #I will be back
@smach.cb_interface(outcomes=['success', 'failure'],
input_keys=['pose'],
output_keys=['pose'])
def set_pose_n_cb(userdata):
try:
rospy.sleep(8)
userdata.pose = [
-0.853, 0.393, -2.935
] #In front of locker = [-0.149,-3.372,0.753],chair = [-0.683, 0.393, -2.935]
return 'success'
except:
return 'failure'
smach.StateMachine.add('SETPOSE_n',
smach.CBState(set_pose_cb),
transitions={
'success': 'MOVEBASE_n',
'failure': 'failure'
})
smach.StateMachine.add('MOVEBASE_n',
MoveBase(mode='abs'),
transitions={
'success': 'carry_luggage?',
'timeout': 'failure',
'failure': 'failure'
})
##once again##
#Mive abs example(Place to return)
@smach.cb_interface(outcomes=['success', 'failure'],
input_keys=['pose'],
output_keys=['pose'])
def set_pose2_cb(userdata):
try:
rospy.sleep(8)
userdata.pose = [
-0.683, 0.393, -2.935
] #In front of locker = [-0.149,-3.372,0.753],chair = [-0.683, 0.393, -2.935]
return 'success'
except:
return 'failure'
smach.StateMachine.add('SETPOSE3',
smach.CBState(set_pose2_cb),
transitions={
'success': 'MOVEBASE3',
'failure': 'failure'
})
smach.StateMachine.add('MOVEBASE3',
MoveBase(mode='abs'),
transitions={
'success': 'PLAY2',
'timeout': 'failure',
'failure': 'failure'
})
smach.StateMachine.add(
'PLAY2',
RobotPlay(
path=
"/home/roboworks/erasersedu_ws/src/carry_luggage/sounds/R2D2a.wav"
),
transitions={
'success': 'FINAL',
'failure': 'failure'
}) #R2-D2(End)
@smach.cb_interface(outcomes=['success', 'failure'])
def final_cb(userdata):
try:
print("Last state.")
return 'success'
except:
return 'failure'
smach.StateMachine.add('FINAL',
smach.CBState(final_cb),
transitions={
'success': 'success',
'failure': 'failure'
})
##########
#END: BASIC FUNCTION2
##########
# Create and start the introspection server
sis = smach_ros.IntrospectionServer('server_name', sm, '/SM_ROOT')
sis.start()
return sm
def __init__(self, robot, room_id=ROOM_ID):
"""
Finds the people in the living room and takes pictures of them. Put then in a data struct and put this in
output keys.
Output keys:
* detected_people: same datastruct as was used in find my mates. Ask Rein for a pickled example
:param robot: (Robot) api object
:param room_id: (str) identifies the room in which the people are detected
"""
smach.StateMachine.__init__(self, outcomes=["done"], output_keys=["detected_people"])
with self:
# Move to start location
smach.StateMachine.add("NAVIGATE_TO_START",
states.NavigateToWaypoint(
robot=robot,
waypoint_designator=ds.EntityByIdDesignator(robot, WAYPOINT_ID), radius=0.3),
transitions={"arrived": "DETECT_PEOPLE",
"unreachable": "DETECT_PEOPLE",
"goal_not_defined": "DETECT_PEOPLE"})
@smach.cb_interface(outcomes=["done"], output_keys=["raw_detections"])
def detect_people(user_data):
person_detections = []
#look_angles = np.linspace(-np.pi / 2, np.pi / 2, 8) # From -pi/2 to +pi/2 to scan 180 degrees wide
look_angles = np.linspace(-np.pi / 4, np.pi / 4, 4) # From -pi/2 to +pi/2 to scan 180 degrees wide
head_goals = [kdl_conversions.VectorStamped(x=100 * math.cos(angle),
y=100 * math.sin(angle),
z=1.5,
frame_id="/%s/base_link" % robot.robot_name)
for angle in look_angles]
sentences = deque([
"Hi there mates, where are you, please look at me!",
"I am looking for my mates! Dippi dee doo! Pew pew!",
"You are all looking great today! Keep looking at my camera.",
"I like it when everybody is staring at me!"
])
look_at_me_sentences = deque([
"Please look at me",
"Let me check you out",
"Your eyes are beautiful",
"Try to look pretty, your face will pop up on the screen later!"
])
for _ in range(NUM_LOOKS):
sentences.rotate(1)
robot.speech.speak(sentences[0], block=False)
for head_goal in head_goals:
look_at_me_sentences.rotate(1)
robot.speech.speak(look_at_me_sentences[0], block=False)
robot.head.look_at_point(head_goal)
robot.head.wait_for_motion_done()
now = time.time()
rgb, depth, depth_info = robot.perception.get_rgb_depth_caminfo()
if rgb:
try:
persons = robot.perception.detect_person_3d(rgb, depth, depth_info)
except Exception as e:
rospy.logerr(e)
rospy.sleep(2.0)
else:
for person in persons:
if person.face.roi.width > 0 and person.face.roi.height > 0:
try:
person_detections.append({
"map_ps": robot.tf_listener.transformPoint("map", PointStamped(
header=rgb.header,
point=person.position
)),
"person_detection": person,
"rgb": rgb
})
except Exception as e:
rospy.logerr(
"Failed to transform valid person detection to map frame: {}".format(e))
rospy.loginfo("Took %.2f, we have %d person detections now", time.time() - now,
len(person_detections))
rospy.loginfo("Detected %d persons", len(person_detections))
user_data.raw_detections = person_detections
return 'done'
smach.StateMachine.add('DETECT_PEOPLE',
smach.CBState(detect_people),
transitions={"done": "FILTER_AND_CLUSTER"})
# Filter and cluster images
@smach.cb_interface(
outcomes=["done", "failed"],
input_keys=["raw_detections"],
output_keys=["detected_people"])
def filter_and_cluster_images(user_data):
"""
Filters the raw detections so that only people within the room maintain. Next, it clusters the images
so that only one image per person remains. This is stored in the user data
:param user_data: (smach.UserData)
:return: (str) Done
"""
try:
user_data.detected_people = _filter_and_cluster_images(
robot, user_data.raw_detections, room_id)
return "done"
except:
return "failed"
smach.StateMachine.add('FILTER_AND_CLUSTER',
smach.CBState(filter_and_cluster_images),
transitions={"done": "done",
"failed": "done"}) # ToDo: fallback
def __init__(self, robot):
smach.StateMachine.__init__(self, outcomes=['Done', 'Aborted'])
self.robot = robot
self.position_constraint = PositionConstraint()
self.orientation_constraint = OrientationConstraint()
self.position_constraint.constraint = "x^2+y^2 < 1"
self.requested_location = None
rospy.Subscriber("/location_request", std_msgs.msg.String,
self.requestedLocationcallback)
self.random_nav_designator = RandomNavDesignator(self.robot)
with self:
smach.StateMachine.add("WAIT_A_SEC",
states.WaitTime(robot, waittime=1.0),
transitions={
'waited': "SELECT_ACTION",
'preempted': "Aborted"
})
smach.StateMachine.add("SELECT_ACTION",
SelectAction(),
transitions={
'continue': "DETERMINE_TARGET",
'pause': "SELECT_ACTION",
'stop': "SAY_DONE"
})
@smach.cb_interface(
outcomes=['target_determined', 'no_targets_available'],
input_keys=[],
output_keys=[])
def determine_target(userdata, designator):
entity_id = designator.getRandomGoal()
sentences = [
"Lets go look at the %s", "Lets have a look at the %s",
"Lets go to the %s", "Lets move to the %s",
"I will go to the %s", "I will now move to the %s",
"I will now drive to the %s", "I will look the %s",
"The %s will be my next location", "The %s it is",
"New goal, the %s", "Going to look at the %s",
"Moving to the %s", "Driving to the %s", "On to the %s",
"On the move to the %s", "Going to the %s"
]
robot.speech.speak(random.choice(sentences) % entity_id,
block=False)
return 'target_determined'
smach.StateMachine.add('DETERMINE_TARGET',
smach.CBState(determine_target,
cb_kwargs={
'designator':
self.random_nav_designator
}),
transitions={
'target_determined': 'DRIVE',
'no_targets_available': 'SELECT_ACTION'
})
smach.StateMachine.add('DRIVE',
states.NavigateToObserve(
robot, self.random_nav_designator),
transitions={
"arrived": "SAY_SUCCEEDED",
"unreachable": 'SAY_UNREACHABLE',
"goal_not_defined": 'SELECT_ACTION'
})
smach.StateMachine.add("SAY_SUCCEEDED",
states.Say(robot, [
"I am here",
"Goal succeeded",
"Another goal succeeded",
"Goal reached",
"Another goal reached",
"Target reached",
"Another target reached",
"Destination reached",
"Another destination reached",
"I have arrived",
"I have arrived at my goal",
"I have arrived at my target",
"I have arrived at my destination",
"I am at my goal",
"I am at my target",
"I am at my destination",
"Here I am",
]),
transitions={'spoken': 'SELECT_ACTION'})
smach.StateMachine.add(
"SAY_UNREACHABLE",
states.Say(robot, [
"I can't find a way to my goal, better try something else",
"This goal is unreachable, I better find somewhere else to go",
"I am having a hard time getting there so I will look for a new target"
]),
transitions={'spoken': 'SELECT_ACTION'})
smach.StateMachine.add(
"SAY_DONE",
states.Say(robot, [
"That's all folks", "I'll stay here for a while", "Goodbye"
]),
transitions={'spoken': 'Done'})
def __init__(self):
self.beacons_set = set(rospy.get_param('~beacons_set', [1, 2, 3]))
self.beacons_ns = rospy.get_param('~beacons_ns', '/')
self.beacons_prefix = rospy.get_param('~beacons_prefix', 'beacon')
# Next target
c_next = rospy.get_param('~color_next', {
'index': 255,
'color': {
'r': 0.0,
'g': 1.0,
'b': 1.0,
'a': 0.1
}
})
self.color_next = NeoPixelColor(index=c_next['index'],
color=ColorRGBA(**c_next['color']))
# Wait for confirmation
c_wait = rospy.get_param('~color_wait', {
'index': 255,
'color': {
'r': 1.0,
'g': 0.4,
'b': 0.0,
'a': 0.1
}
})
self.color_wait = NeoPixelColor(index=c_wait['index'],
color=ColorRGBA(**c_wait['color']))
# Confirmed
c_confirm = rospy.get_param('~color_confirm', {
'index': 255,
'color': {
'r': 0.0,
'g': 1.0,
'b': 0.0,
'a': 0.2
}
})
self.color_confirm = NeoPixelColor(
index=c_confirm['index'], color=ColorRGBA(**c_confirm['color']))
# Complete
c_complete = rospy.get_param('~color_complete', {
'index': 255,
'color': {
'r': 0.0,
'g': 1.0,
'b': 1.0,
'a': 0.2
}
})
self.color_complete = NeoPixelColor(
index=c_complete['index'], color=ColorRGBA(**c_complete['color']))
# Clear pixels
self.color_clear = NeoPixelColor(index=255, color=ColorRGBA())
relloc_fsm_state_topic = rospy.get_param('~relloc_fsm_state_topic',
'drone_relloc_fsm/state')
self.sub_relloc_fsm_state = rospy.Subscriber(relloc_fsm_state_topic,
String,
self.relloc_fsm_state_cb)
self.relloc_fsm_event = threading.Event()
self.relloc_fsm_state = None
self.tf_buff = tf2_ros.Buffer()
self.tf_ls = tf2_ros.TransformListener(self.tf_buff)
robot_current_pose_topic = rospy.get_param('~robot_current_pose_topic',
'/optitrack/robot')
self.sub_current_pose = rospy.Subscriber(robot_current_pose_topic,
PoseStamped,
self.robot_current_pose_cb)
self.robot_current_pose = None
self.ignore_z = rospy.get_param('~ignore_z', True)
self.target_threshold_xy = rospy.get_param('~target_threshold_xy',
0.10) # 10cm
self.target_threshold_z = rospy.get_param('~target_threshold_z',
0.20) # 10cm
self.state_name_topic = rospy.get_param('~state_name_topic', '~state')
self.pub_state_name = rospy.Publisher(self.state_name_topic,
String,
queue_size=10,
latch=True)
self.pubs_beacon_color = {
it: rospy.Publisher(self.beacons_ns + self.beacons_prefix +
str(it) + '/color',
NeoPixelColor,
queue_size=10)
for it in self.beacons_set
}
self.sm = smach.StateMachine(outcomes=['FINISH'])
self.sm.userdata.targets_list = self.generate_targets_list(
self.beacons_set)
rospy.loginfo('Targets list: {}'.format(self.sm.userdata.targets_list))
with self.sm:
smach.StateMachine.add('INIT_ALL_CLEAR',
smach.CBState(self.set_all_color,
cb_kwargs={
'context': self,
'color': self.color_clear,
'delay': 1.0
}),
transitions={
'done': 'WAIT_FOR_RELLOC',
'preempted': 'FINISH'
})
smach.StateMachine.add('WAIT_FOR_RELLOC',
smach.CBState(
self.wait_for_relloc_fsm_state,
cb_kwargs={
'context': self,
'expected_state': 'FOLLOW_POINTING'
}),
transitions={
'ready': 'CHOOSE_NEXT',
'preempted': 'FINISH'
})
smach.StateMachine.add('CHOOSE_NEXT',
smach.CBState(self.choose_next_target,
cb_kwargs={'context': self}),
transitions={
'done': 'INDICATE_NEXT',
'empty': 'WAIT_FOR_LANDING',
'preempted': 'FINISH'
})
smach.StateMachine.add('INDICATE_NEXT',
smach.CBState(self.set_color,
cb_kwargs={
'context': self,
'color': self.color_next
}),
transitions={
'done': 'WAIT_TO_ENTER',
'preempted': 'FINISH'
})
smach.StateMachine.add('WAIT_TO_ENTER',
smach.CBState(self.wait_to_enter,
cb_kwargs={'context': self}),
transitions={
'entered': 'INDICATE_WAIT',
'preempted': 'FINISH'
})
smach.StateMachine.add('INDICATE_WAIT',
smach.CBState(self.set_color,
cb_kwargs={
'context': self,
'color': self.color_wait
}),
transitions={
'done': 'WAIT_TO_CONFIRM',
'preempted': 'FINISH'
})
smach.StateMachine.add('WAIT_TO_CONFIRM',
smach.CBState(self.wait_to_confirm,
cb_kwargs={'context': self}),
transitions={
'confirmed': 'INDICATE_CONFIRMED',
'canceled': 'INDICATE_NEXT',
'preempted': 'FINISH'
})
smach.StateMachine.add('INDICATE_CONFIRMED',
smach.CBState(self.set_color,
cb_kwargs={
'context': self,
'color':
self.color_confirm,
'delay': 0.5
}),
transitions={
'done': 'INDICATE_CLEAR',
'preempted': 'FINISH'
})
smach.StateMachine.add('INDICATE_CLEAR',
smach.CBState(self.set_color,
cb_kwargs={
'context': self,
'color': self.color_clear
}),
transitions={
'done': 'CHOOSE_NEXT',
'preempted': 'FINISH'
})
smach.StateMachine.add('WAIT_FOR_LANDING',
smach.CBState(
self.wait_for_relloc_fsm_state,
cb_kwargs={
'context': self,
'expected_state': 'LAND'
}),
transitions={
'ready': 'INDICATE_ALL_COMPLETE',
'preempted': 'FINISH'
})
smach.StateMachine.add('INDICATE_ALL_COMPLETE',
smach.CBState(self.set_all_color,
cb_kwargs={
'context': self,
'color':
self.color_complete,
'delay': 3.0
}),
transitions={
'done': 'INDICATE_ALL_CLEAR',
'preempted': 'FINISH'
})
smach.StateMachine.add('INDICATE_ALL_CLEAR',
smach.CBState(self.set_all_color,
cb_kwargs={
'context': self,
'color': self.color_clear
}),
transitions={
'done': 'WAIT_FOR_RELLOC',
'preempted': 'FINISH'
})
# smach.StateMachine.add('FINISH',
# smach.CBState(self.set_all_color, cb_kwargs = {'context': self, 'color': self.color_clear}),
# transitions = {'done': 'OVER',
# 'preempted': 'OVER'})
self.sm.register_start_cb(self.state_transition_cb, cb_args=[self.sm])
self.sm.register_transition_cb(self.state_transition_cb,
cb_args=[self.sm])
self.sis = smach_ros.IntrospectionServer('smach_server', self.sm,
'/SM_BEACONS')
def __init__(self, robot, operator_distance=1.0, operator_radius=0.5):
# type: (Robot, float, float) -> None
"""
Base Smach state to guide an operator to a designated position
:param robot: (Robot) robot api object
:param operator_distance: (float) check for the operator to be within this range of the robot
:param operator_radius: (float) from the point behind the robot defined by `distance`, the person must be within this radius
"""
smach.StateMachine.__init__(
self, outcomes=["arrived", "unreachable", "goal_not_defined", "lost_operator", "preempted"])
self.robot = robot
self.operator_distance = operator_distance
self.operator_radius = operator_radius
self.execute_plan = ExecutePlanGuidance(robot=self.robot,
operator_distance=self.operator_distance,
operator_radius=self.operator_radius)
with self:
@smach.cb_interface(outcomes=["done"])
def _reset_mentioned_entities(userdata=None):
"""
Resets the entities that have been mentioned so that the robot will mention all entities to all
operators
"""
self.execute_plan.reset_tourguide()
return "done"
smach.StateMachine.add("RESET_MENTIONED_ENTITIES",
smach.CBState(_reset_mentioned_entities),
transitions={"done": "SAY_BEHIND"})
smach.StateMachine.add("SAY_BEHIND",
Say(robot, "Please stand behind me and look at me", block=True),
transitions={"spoken": "WAIT"})
smach.StateMachine.add("WAIT",
WaitTime(robot, waittime=3.0),
transitions={"waited": "GET_PLAN",
"preempted": "preempted"})
smach.StateMachine.add("GET_PLAN", navigation.getPlan(self.robot, self.generate_constraint),
transitions={"unreachable": "unreachable",
"goal_not_defined": "goal_not_defined",
"goal_ok": "EXECUTE_PLAN"})
smach.StateMachine.add("EXECUTE_PLAN", self.execute_plan,
transitions={"arrived": "arrived",
"blocked": "PLAN_BLOCKED",
"preempted": "preempted",
"lost_operator": "WAIT_FOR_OPERATOR"})
smach.StateMachine.add("WAIT_FOR_OPERATOR",
WaitForOperator(robot=self.robot,
distance=self.operator_distance,
radius=self.operator_radius),
transitions={"is_following": "GET_PLAN",
"is_lost": "lost_operator"})
smach.StateMachine.add("PLAN_BLOCKED", navigation.planBlocked(self.robot),
transitions={"blocked": "GET_PLAN",
"free": "EXECUTE_PLAN"})
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, robot):
smach.StateMachine.__init__(self,
outcomes=["Done", "Aborted", "Failed"])
self.robot = robot
ignore_predicate = "ignore"
person_to_look_at_predicate = "person_to_look_at"
person_to_look_at = Conjunction(
Compound(person_to_look_at_predicate, "ObjectID"),
Compound("property_expected", "ObjectID", "position",
Sequence("X", "Y", "Z")))
#Make sure the predicate exists and we don't get stupid existence errors
robot.reasoner.assertz(ignore_predicate, "dummy")
robot.reasoner.query(
Compound("retractall", Compound(ignore_predicate, "X")))
item_query = Conjunction(
Conjunction(
Compound("property_expected", "ObjectID", "class_label",
"person"),
Compound("property_expected", "ObjectID", "position",
Sequence("X", "Y", "IgnoredZ"))),
Compound("=", "Z", "1.65"),
Compound("not", Compound(ignore_predicate, "ObjectID")))
print item_query
with self:
@smach.cb_interface(outcomes=['done'])
def reset_reasoner(*args, **kwargs):
robot.reasoner.reset()
return 'done'
smach.StateMachine.add(
"RESET_REASONER",
smach.CBState(reset_reasoner),
transitions={"done": "WAIT_FOR_POSSIBLE_DETECTION"})
#Wait until there are some objects in the WM that match the query. The object should not be marked to ignore them.
#If we can't find a match, this can mean there are nu objects that have the right class,
# or that each of those are already ignored.
# If all objects of a class are already ignored, we're done and should unignore the ignored objects
smach.StateMachine.add("WAIT_FOR_POSSIBLE_DETECTION",
states.Wait_queried_perception(
robot, ["ppl_detection"],
item_query,
timeout=60),
transitions={
"query_true": "SELECT_ITEM_TO_LOOK_AT",
"timed_out": "RETRACT_IGNORE",
"preempted": "Aborted"
})
#Then, of those matching objects, select one to look at and mark that object as the object we should look at.
smach.StateMachine.add('SELECT_ITEM_TO_LOOK_AT',
states.Select_object(
robot, item_query,
person_to_look_at_predicate),
transitions={
'selected': 'LOOK_AT_POSSIBLE_PERSON',
'no_answers': 'Failed'
})
#Then, finally, look at it.
smach.StateMachine.add('LOOK_AT_POSSIBLE_PERSON',
states.LookAtPoint(robot,
person_to_look_at),
transitions={
'looking': 'LOOK_AT_FACE',
'no_point_found': 'Failed',
'abort': 'Aborted'
})
smach.StateMachine.add("LOOK_AT_FACE",
states.LookAtItem(
robot, ["face_recognition"],
states.LookAtItem.face_in_front_query,
timeout=3),
transitions={
'Done': 'IGNORE_CURRENTLY_LOOKED_AT',
'Aborted': 'Aborted',
'Failed': 'IGNORE_CURRENTLY_LOOKED_AT'
})
#Mark the selected object as that it should be ignored. If there is no such item, retract all ignores
#If we did succesfully ignore the item we just looked at, unmark the object as the object we should look at.
# The result is that there is no item to look at
smach.StateMachine.add('IGNORE_CURRENTLY_LOOKED_AT',
states.Select_object(
robot,
person_to_look_at,
ignore_predicate,
retract_previous=False),
transitions={
'selected': 'RETRACT_ITEM_TO_LOOK_AT',
'no_answers': 'RETRACT_IGNORE'
})
#Un-select the object we were looking at
smach.StateMachine.add(
'RETRACT_ITEM_TO_LOOK_AT',
states.Retract_facts(
robot, Compound(person_to_look_at_predicate, "Item")),
transitions={'retracted': 'WAIT_FOR_POSSIBLE_DETECTION'})
#When we're done, and can't find matches to the WM query, retract all ignores we introduced so the WM is normal again
smach.StateMachine.add('RETRACT_IGNORE',
states.Retract_facts(
robot, Compound(ignore_predicate,
"Item")),
transitions={'retracted': 'Done'})
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"})