def main():
rospy.init_node('recognize_object_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
smach.StateMachine.add(
'prepare_object_recognize',
prepare_recognize_object(),
transitions={'succeeded': 'object_recognition','aborted': 'aborted'})
smach.StateMachine.add(
'object_recognition',
recognize_object(),
transitions={'succeeded': 'object_print','aborted': 'aborted'})
smach.StateMachine.add(
'object_print',
recognize_object_print(),
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, '/DOI_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'], input_keys=['object_array'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name=None
self.userdata.standard_error='OK'
self.userdata.object_name = ''
self.userdata.object_index = 0
# Process order
smach.StateMachine.add(
'decide_next_object',
decide_next_object(),
transitions={'succeeded': 'go_to_object', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to poi where the object can stay
smach.StateMachine.add(
'go_to_object',
nav_to_poi(),
remapping={"nav_to_poi_name": "object_loc"},
transitions={'succeeded': 'object_detection', 'aborted': 'aborted'})
# Object Detection
smach.StateMachine.add(
'object_detection',
recognize_object(),
transitions={'succeeded': 'grasp_object', 'aborted': 'aborted'})
# Grasp Object
smach.StateMachine.add(
'grasp_object',
text_to_say("Grasping Object"),
transitions={'succeeded': 'go_to_delivery', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to the delivery place
smach.StateMachine.add(
'go_to_delivery',
nav_to_poi(),
remapping = {'nav_to_poi_name': 'delivery_loc'},
transitions={'succeeded': 'deliver_object', 'aborted': 'aborted',
'preempted': 'preempted'})
# Deliver object
smach.StateMachine.add(
'deliver_object',
text_to_say("Delivering Object"),
transitions={'succeeded': 'check_loop', 'aborted': 'aborted',
'preempted': 'preempted'})
# End of loop?
smach.StateMachine.add(
'check_loop',
checkLoop(),
transitions={'succeeded': 'decide_next_object', 'aborted': 'aborted',
'preempted': 'preempted', 'end':'succeeded'})
def __init__(self, object_name = None):
smach.StateMachine.__init__(self, outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_name'],
output_keys=['standard_error', 'objectd'])
with self:
smach.StateMachine.add('PrepareData',
prepareData(object_name),
transitions={'succeeded':'get_object_info_sm', 'aborted':'aborted'})
# Obtain the location where the object can stay
smach.StateMachine.add('get_object_info_sm',
GetObjectInfoSM(),
transitions={'succeeded': 'say_go_to_poi',
'aborted': 'aborted',
'preempted': 'preempted'})
# say that it goes to the poi
smach.StateMachine.add(
'say_go_to_poi',
text_to_say("I'm going to take the object"),
transitions={'succeeded': 'go_to_object', 'aborted': 'aborted',
'preempted': 'preempted'})
# Go to poi where the object can stay
smach.StateMachine.add(
'go_to_object',
nav_to_poi(),
remapping={"nav_to_poi_name": "object_location"},
transitions={'succeeded': 'say_start_recognition', 'aborted': 'aborted'})
# Say start recognition
smach.StateMachine.add(
'say_start_recognition',
text_to_say("I'm going to start object recognition"),
transitions={'succeeded': 'object_detection', 'aborted': 'object_detection',
'preempted': 'preempted'})
# Object Detection
smach.StateMachine.add(
'object_detection',
recognize_object(),
transitions={'succeeded': 'succeeded', 'aborted': 'get_object_info_sm'})
def main():
rospy.init_node('recognize_object_test')
sm = smach.StateMachine(outcomes=['succeeded', 'preempted', 'aborted'])
with sm:
smach.StateMachine.add('prepare_object_recognize',
prepare_recognize_object(),
transitions={
'succeeded': 'object_recognition',
'aborted': 'aborted'
})
smach.StateMachine.add('object_recognition',
recognize_object(),
transitions={
'succeeded': 'object_print',
'aborted': 'aborted'
})
smach.StateMachine.add('object_print',
recognize_object_print(),
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,
'/DOI_ROOT')
sis.start()
sm.execute()
rospy.spin()
sis.stop()
def __init__(self):
smach.StateMachine.__init__(
self,
outcomes=['succeeded', 'preempted', 'aborted'],
input_keys=['object_array'])
with self:
# We must initialize the userdata keys if they are going to be accessed or they won't exist and crash!
self.userdata.nav_to_poi_name = None
self.userdata.standard_error = 'OK'
self.userdata.object_name = ''
self.userdata.object_index = 0
# Process order
smach.StateMachine.add('decide_next_object',
decide_next_object(),
transitions={
'succeeded': 'go_to_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Go to poi where the object can stay
smach.StateMachine.add('go_to_object',
nav_to_poi(),
remapping={"nav_to_poi_name": "object_loc"},
transitions={
'succeeded': 'object_detection',
'aborted': 'aborted'
})
# Object Detection
smach.StateMachine.add('object_detection',
recognize_object(),
transitions={
'succeeded': 'grasp_object',
'aborted': 'aborted'
})
# Grasp Object
smach.StateMachine.add('grasp_object',
text_to_say("Grasping Object"),
transitions={
'succeeded': 'go_to_delivery',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Go to the delivery place
smach.StateMachine.add(
'go_to_delivery',
nav_to_poi(),
remapping={'nav_to_poi_name': 'delivery_loc'},
transitions={
'succeeded': 'deliver_object',
'aborted': 'aborted',
'preempted': 'preempted'
})
# Deliver object
smach.StateMachine.add('deliver_object',
text_to_say("Delivering Object"),
transitions={
'succeeded': 'check_loop',
'aborted': 'aborted',
'preempted': 'preempted'
})
# End of loop?
smach.StateMachine.add('check_loop',
checkLoop(),
transitions={
'succeeded': 'decide_next_object',
'aborted': 'aborted',
'preempted': 'preempted',
'end': 'succeeded'
})
