class YoubotTransportationAI():
    def __init__(self):
        self.__sis = None
        self.setup_statemachine()
        #self.setup_introspection()
    def run(self):
        self.__sm.execute()
        if self.__sis is not None:
            self.__sis.stop()
    def get_statemachine(self):
        return self.__sm
       
    def setup_introspection(self):
        self.__sis = smach_ros.IntrospectionServer('debug', self.__sm, '/youbot_transportation_ai')
        self.__sis.start()
        
    def cb_dynamic_reconfigure(self, config, level):
        rospy.set_param("youbot_transportation_ai_re/misc/arm_step_range", config.arm_step_range)
        return config
    
    def setup_dynamic_reconfigure(self):
        srv = dynamic_reconfigure.server.Server(ManipulationAI_ReConfig, cb_dynamic_reconfigure)

    def setup_statemachine(self):
        
       # setup_dynamic_reconfigure()
        
        self.__sm = smach.StateMachine(outcomes=['succeeded', 'failed'],
                                input_keys=['task_spec_in_sm'],
                                output_keys=[])
        self.__grasp_ai = YoubotGraspAI()
        self.__grasp_sm = self.__grasp_ai.get_statemachine()
        self.__deposit_ai = DepositObject()
        self.__deposit_sm = self.__deposit_ai.get_statemachine()
        self.__nav_ai = YoubotNavigationAI()
        self.__nav_sm = self.__nav_ai.get_statemachine()
                                
        with self.__sm:
            self.__sm.userdata.task_spec_nav = None
            self.__sm.userdata.task_spec_manip = None
            self.__sm.userdata.task_spec_trans = None
            self.__sm.userdata.selected_service_area = None
            self.__sm.userdata.destination = None
            self.__sm.userdata.service_areas = None
            self.__sm.userdata.grasped_objects = None
            self.__sm.userdata.object_counter = 0
            self.__sm.userdata.alignment = 'line'
            #self.__sm.userdata.task_spec = TaskSpecificationMan('S1',['S40_40_B','F20_20_B','R20','R20','M20_100'])

            with self.__sm:
                
                smach.StateMachine.add('Initialization', Initialization(),
                                       transitions={'initialized': 'SelectPlaceSrc'},
                                        remapping={'service_areas_out': 'service_areas',
                                                   'task_spec_in': 'task_spec_in_sm',
                                                   'task_spec_out': 'task_spec_trans'})
                                       
                smach.StateMachine.add('SelectPlaceSrc', SelectPlaceSrc(), 
                                       transitions={'selected':'ConstructSrcNavMsg'},
                                       remapping={'task_spec_in': 'task_spec_trans',
                                                  'task_spec_out': 'task_spec_trans',
                                                  'selected_src_out': 'selected_service_area'})
                                       
                smach.StateMachine.add('ConstructSrcNavMsg', ConstructNavMsg(),
                                       transitions={'constructed': 'DriveToSrc'},
                                        remapping= {'destination_in': 'selected_service_area',
                                                    'service_areas_in': 'service_areas',
                                                    'task_spec_nav_out': 'task_spec_nav'})
                
                smach.StateMachine.add('DriveToSrc', self.__nav_sm,
                                       transitions={'succeeded': 'AlignToSrcServiceArea'},
                                        remapping={'task_specification_nav_in': 'task_spec_nav'})
                
                smach.StateMachine.add('AlignToSrcServiceArea', Align(),
                                       transitions={'succeeded': 'ConstructManipMsg',
                                                    'failed': 'ConstructManipMsg'})
            
                smach.StateMachine.add('ConstructManipMsg', ConstructManipMsg(),
                                       transitions={'constructed': 'GraspObject'},
                                        remapping={'task_specification_manip_out': 'task_spec_manip',
                                                   'selected_src_in': 'selected_service_area',
                                                   'task_spec_in': 'task_spec_trans'})                
                
                smach.StateMachine.add('GraspObject', self.__grasp_sm,
                                       transitions={'succeeded': 'SelectPlaceDst'},
                                        remapping={'task_spec_in': 'task_spec_manip',
                                                   'selected_object_name_out_sm': 'grasped_objects'})              

                smach.StateMachine.add('SelectPlaceDst', SelectPlaceDst(),
                                       transitions={'selected': 'ConstructDstNavMsg'},
                                       remapping={'grasped_object_in': 'grasped_objects',
                                                  'task_spec_trans_in': 'task_spec_trans',
                                                  'selected_service_area_out': 'destination'})

                smach.StateMachine.add('ConstructDstNavMsg', ConstructNavMsg(),
                                       transitions={'constructed': 'DriveToDst'},
                                        remapping= {'service_areas_in': 'service_areas',
                                                    'task_spec_nav_out': 'task_spec_nav',
                                                    'destination_in': 'destination'})
                
                smach.StateMachine.add('DriveToDst', self.__nav_sm,
                                       transitions={'succeeded': 'AlignToDstServiceArea'},
                                        remapping={'task_specification_nav_in': 'task_spec_nav'})
                                        
                smach.StateMachine.add('AlignToDstServiceArea', Align(),
                                       transitions={'succeeded': 'DepositObject',
                                                    'failed': 'DepositObject'})
                
                smach.StateMachine.add('DepositObject', self.__deposit_sm,
                                       transitions={'succeeded': 'UpdateTaskSpec'},
                                        remapping={'object_counter_in_sm': 'object_counter',
                                                   'alignment_in_sm': 'alignment',
                                                   'service_area_name_in_sm': 'destination'})
                                       
                smach.StateMachine.add('UpdateTaskSpec', UpdateTaskSpec(),
                                       transitions={'Object remaining': 'SelectPlaceSrc',
                                                    'finished': 'succeeded'},
                                        remapping={'grasped_objects_in': 'grasped_objects',
                                                   'task_spec_trans_io': 'task_spec_trans'})                       
Esempio n. 2
0
class YoubotManipulationAI():
    def __init__(self):
        self.__sis = None
        self.setup_statemachine()
        #self.setup_introspection()
    def run(self):
        self.__sm.execute()
        if self.__sis is not None:
            self.__sis.stop()
    def get_statemachine(self):
        return self.__sm
    def setup_introspection(self):
        self.__sis = smach_ros.IntrospectionServer('debug', self.__sm, '/youbot_manipulation_ai_re')
        self.__sis.start()
    def setup_statemachine(self):
        self.__nav_ai = YoubotNavigationAI()
        self.__nav_sm = self.__nav_ai.get_statemachine()
        self.__grasp_ai = YoubotGraspAI()
        self.__grasp_sm = self.__grasp_ai.get_statemachine()
        self.__deposit_ai = DepositObject()
        self.__deposit_sm = self.__deposit_ai.get_statemachine()
        self.__sm = smach.StateMachine(outcomes=['succeeded', 'failed'],
                            input_keys=['task_specification_manip_in'])
        with self.__sm:
            self.__sm.userdata.task_specification = None
            self.__sm.userdata.task_spec_tmp = None
            self.__sm.userdata.current_object = None
            self.__sm.userdata.dest_object_counter = 0
            self.__sm.userdata.alignment = None
            self.__sm.userdata.marker_init = None
            self.__sm.userdata.marker_src = None
            self.__sm.userdata.marker_dst = None
            self.__sm.userdata.marker_fin = None
            self.__sm.userdata.orientation_init = None
            self.__sm.userdata.orientation_src = None
            self.__sm.userdata.orientation_dst = None
            self.__sm.userdata.orientation_fin = None
            
            smach.StateMachine.add('initialization', Initialization(), 
                                   transitions={'done': 'gen_taskspec_nav_initial'}, 
                                   remapping={'task_specification_io': 'task_specification_manip_in',
                                              'task_specification_out': 'task_specification',
                                              'dest_object_counter_out': 'dest_object_counter',
                                              'alignment_out': 'alignment',
                                              'marker_init_out': 'marker_init',
                                              'marker_src_out': 'marker_src',
                                              'marker_dst_out': 'marker_dst',
                                              'marker_fin_out': 'marker_fin',
                                              'orientation_init_out': 'orientation_init',
                                              'orientation_src_out': 'orientation_src',
                                              'orientation_dst_out': 'orientation_dst',
                                              'orientation_fin_out': 'orientation_fin'})
                                              
            smach.StateMachine.add('gen_taskspec_nav_initial', GenTaskspecNavigation(),
                                   transitions={'generated': 'drive_to_initial_place',
                                                'failed': 'failed'},
                                   remapping={'marker_in': 'marker_init',
                                              'orientation_in': 'orientation_init',
                                              'task_spec_out': 'task_spec_tmp'})
            
            smach.StateMachine.add('drive_to_initial_place', self.__nav_sm,
                                   transitions={'succeeded': 'gen_taskspec_nav_source',
                                                'failed': 'gen_taskspec_nav_source'},
                                   remapping={'task_specification_nav_in': 'task_spec_tmp'})
                                   
            smach.StateMachine.add('gen_taskspec_nav_source', GenTaskspecNavigation(),
                                   transitions={'generated': 'drive_to_source_place',
                                                'failed': 'failed'},
                                   remapping={'marker_in': 'marker_src',
                                              'orientation_in': 'orientation_src',
                                              'task_spec_out': 'task_spec_tmp'})
            
            smach.StateMachine.add('drive_to_source_place', self.__nav_sm,
                                   transitions={'succeeded': 'align_robot_source',
                                                'failed': 'gen_taskspec_nav_source'},
                                   remapping={'task_specification_nav_in': 'task_spec_tmp'})
                                   
            smach.StateMachine.add('align_robot_source', AlignRobot(),
                                   transitions={'succeeded': 'gen_taskspec_grasp',
                                                'failed': 'gen_taskspec_grasp'},
                                   remapping={})
                                   
            smach.StateMachine.add('gen_taskspec_grasp', GenTaskspecGrasp(),
                                   transitions={'generated': 'grasp_object',
                                                'failed': 'failed'},
                                   remapping={'task_specification_in': 'task_specification',
                                              'task_spec_out': 'task_spec_tmp'}) 
                                   
            smach.StateMachine.add('grasp_object', self.__grasp_sm,
                                   transitions={'succeeded': 'gen_taskspec_nav_dest',
                                                'failed': 'gen_taskspec_nav_dest'},
                                   remapping={'task_spec_in': 'task_spec_tmp',
                                              'selected_object_name_out_sm': 'current_object'}) 
           
            smach.StateMachine.add('gen_taskspec_nav_dest', GenTaskspecNavigation(),
                                   transitions={'generated': 'drive_to_dest_place',
                                                'failed': 'failed'},
                                   remapping={'marker_in': 'marker_dst',
                                              'orientation_in': 'orientation_dst',
                                              'task_spec_out': 'task_spec_tmp'}) 
           
            smach.StateMachine.add('drive_to_dest_place', self.__nav_sm,
                                   transitions={'succeeded': 'align_robot_dest',
                                                'failed': 'align_robot_dest'},
                                   remapping={'task_specification_nav_in': 'task_spec_tmp'})
           
            smach.StateMachine.add('align_robot_dest', AlignRobot(),
                                   transitions={'succeeded': 'deposit_object',
                                                'failed': 'deposit_object'},
                                   remapping={})
           
            smach.StateMachine.add('deposit_object', self.__deposit_sm,
                                   transitions={'succeeded': 'task_spec_update',
                                                'failed': 'task_spec_update'},
                                   remapping={'object_counter_in_sm': 'dest_object_counter',
                                              'alignment_in_sm': 'alignment',
                                              'service_area_name_in_sm': 'marker_dst'})
                                              
            smach.StateMachine.add('task_spec_update', TaskSpecUpdate(),
                                   transitions={'nextObject': 'gen_taskspec_nav_source',
                                                'finished': 'gen_taskspec_nav_final'},
                                   remapping={'task_specification_io': 'task_specification',
                                              'current_object_in': 'current_object'})
                                              
            smach.StateMachine.add('gen_taskspec_nav_final', GenTaskspecNavigation(),
                                   transitions={'generated': 'drive_to_final_place',
                                                'failed': 'failed'},
                                   remapping={'marker_in': 'marker_fin',
                                              'orientation_in': 'orientation_fin',
                                              'task_spec_out': 'task_spec_tmp'})
            
            smach.StateMachine.add('drive_to_final_place', self.__nav_sm,
                                   transitions={'succeeded': 'succeeded',
                                                'failed': 'failed'},
                                   remapping={'task_specification_nav_in': 'task_spec_tmp'})