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'})
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'})
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'})