def main():
rospy.init_node('recognize_obect_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
# smach.StateMachine.add(
# 'prepare_object',
# prepare_detect_object(),
# transitions={'succeeded': 'detect_object','aborted' : 'aborted_info'})
# it call the learn state
smach.StateMachine.add(
'detect_object',
object_detect_sm('Barritas'),
transitions={'succeeded': 'succeeded','aborted' : 'aborted'})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer(
'detect_object_introspection', sm, '/DFI_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def main():
rospy.init_node('recognize_obect_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
# smach.StateMachine.add(
# 'prepare_object',
# prepare_detect_object(),
# transitions={'succeeded': 'detect_object','aborted' : 'aborted_info'})
# it call the learn state
smach.StateMachine.add('detect_object',
object_detect_sm('pringles'),
transitions={
'succeeded': 'succeeded',
'aborted': 'aborted'
})
# This is for the smach_viewer so we can see what is happening, rosrun smach_viewer smach_viewer.py it's cool!
sis = smach_ros.IntrospectionServer('detect_object_introspection', sm,
'/DFI_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'aborted', 'preempted'],
input_keys=['object_name'],
output_keys=['standard_error','object_position','object_detected_name'])
with self:
self.userdata.standard_error = ''
smach.StateMachine.add(
'Look_down',
move_head_form(head_left_right="center", head_up_down="down"),
transitions={'succeeded': 'sleep_state',
'aborted': 'aborted',
'preempted': 'preempted'})
# Only for rh2c
smach.StateMachine.add(
'sleep_state',
Sleeper(5),
transitions={'succeeded': 'detect_object', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'detect_object',
object_detect_sm(),
transitions={'succeeded': 'process_object', 'aborted': 'aborted',
'preempted': 'preempted'})
smach.StateMachine.add(
'process_object',
process_object(),
transitions={'succeeded': 'succeeded', 'aborted': 'aborted',
'preempted': 'preempted'})
def call_find_object(object_name,world): #TODO
tosay = "I'm going to search for " + object_name
speak = speaker(tosay)
speak.execute()
rospy.logwarn('call_find_object '+object_name)
#############################################################################
if SKILLS :
if (time.time()-TIME_INIT) > 270:
return "succeeded"
out = 'aborted'
tries = 0
current_position = world.get_current_position()
if current_position in ROOMS:
room = rospy.get_param('/robocup_params/room/' + current_position)
#while(out=='aborted' and tries<3):
for table in room :
if out == 'succeeded' and tries == 3:
break
call_go_to(table)
sm = object_detect_sm()#
out = sm.execute() # #PROVABLY WE WILL HAVE TO CHECK IF THERE IS A TABLE NEARBY BEFORE STARTING TO SEARCH
object_position = sm.userdata.object_pose #
tries = tries+1#
if out=='aborted':
tosay = "I couldn't find the " + object_name + " you asked for. It isn't here. I'm going to the referee to inform"
speak = speaker(tosay)
speak.execute()
rospy.logwarn('FAIL IN FINDING ' + object_name)
time.sleep(SLEEP_TIME)
call_go_to('referee',world)
tosay = "I couldn't find the " + object_name + " you asked for. It isn't there. I'm afraid that sentence was from category 3"
speak = speaker(tosay)
speak.execute()
return "aborted"
#############################################################################
time.sleep(SLEEP_TIME)
return "succeeded"
