def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'dispatch_plan', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'dispatch_plan') self.planner_interface = PlannerInterface()
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'report', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed'], input_keys=['command']) self.timeout = kwargs.get('timeout', 120.)
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'verify_person', save_sm_state=save_sm_state, outcomes=[ 'no_empty_spot', 'new_person', 'already_logged_person', 'known_person', 'face_not_seen' ], output_keys=['destination_locations']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'verify_person') self.number_of_retries = kwargs.get('number_of_retries', 0) self.total_locations = kwargs.get('total_locations', 0) self.person_recognition_threshold = kwargs.get( 'person_recognition_threshold', 0) self.retry_count = 0 self.timeout = kwargs.get('timeout', 10.) self.bye = ['bye', 'by', 'buy', 'bi'] occupied_locations = self.kb_interface.get_obj_instance( 'occupied_locations', DomainFormula._type) if occupied_locations is None: self.kb_interface.insert_obj_instance('occupied_locations', DomainFormula()) # wait for listen action server self.listen_client = actionlib.SimpleActionClient( 'listen_server', ListenAction) listen_wait_result = self.listen_client.wait_for_server( timeout=rospy.Duration(self.timeout)) if not listen_wait_result: raise RuntimeError('Failed to wait for "listen_server" action')
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__( self, 'receive_information', save_sm_state=save_sm_state, input_keys=[], output_keys=['target_entity'], outcomes=['succeeded', 'failed', 'failed_after_retrying']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'receive_information') self.number_of_retries = kwargs.get('number_of_retries', 0) self.timeout = kwargs.get('timeout', 25) self.threshold = kwargs.get('threshold', 0.68) # Load the rasa model model_directory = get_package_path('rasa_nlu_models', 'common', 'where_is_this', 'nlu') self.interpreter = Interpreter.load(model_directory) # wait for listen action server self.listen_client = actionlib.SimpleActionClient( "listen_server", ListenAction) listen_wait_result = self.listen_client.wait_for_server( timeout=rospy.Duration(self.timeout)) if not listen_wait_result: raise RuntimeError('failed to wait for "listen_server" action') # Speech Matcher self.speech_matcher = SpeechMatching()
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__( self, 'select_storage_location', save_sm_state=save_sm_state, input_keys=['grasped_object'], output_keys=['destination_locations', 'storage_location'], outcomes=['succeeded']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'select_storage_location') self.storage_locations_file = kwargs.get('storage_locations_file', 'storage_location.yaml') self.available_trays = kwargs.get('available_trays', ['tray_1', 'tray_2']) self.tray_locations = kwargs.get('tray_locations', { 'tray_1': 'long_table_a_trays', 'tray_2': 'long_table_a_trays' }) self.available_bins = kwargs.get('available_bins', ['bin_a', 'bin_b']) self.bin_locations = kwargs.get('bin_locations', { 'bin_a': 'bin_a', 'bin_b': 'bin_b' }) self.default_storage_location = kwargs.get('default_storage_location', 'bin') storage_locations_path = get_package_path('mdr_wrs_tidy_up', 'config', self.storage_locations_file) if not self.storage_locations_file: raise ValueError( '[{0}] Argument "storage_locations_file" cannot be empty'. format(self.state_name)) self.storage_locations_map = load_yaml_file(storage_locations_path)
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'check_mates_to_find', save_sm_state=save_sm_state, outcomes=['mates_left', 'no_mates_left']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'check_mates_to_find')
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__( self, '_receive_object', save_sm_state=save_sm_state, outcomes=[ 'succeeded', 'failed', 'failed_after_retrying', 'person_not_found' ], input_keys=['person_name', 'object_to_transport'], output_keys=['grasped_object']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'get_person_pose') self.action_server = kwargs.get('action_server', 'receive_object_server') self.timeout = kwargs.get('timeout', 120.) self.context_aware = kwargs.get('context_aware', True) self.reception_detection = kwargs.get('reception_detection', True) self.posture_ratio_ranges = kwargs.get('posture_ratio_ranges', {}) self.person_name = kwargs.get('person_name', None) self.object_to_transport = kwargs.get('object_to_transport', None) self.object_category = kwargs.get('object_category', None) self.gripper_controller_pkg = kwargs.get('gripper_controller_pkg', 'mdr_gripper_controller') self.number_of_retries = kwargs.get('number_of_retries', 0) self.retry_count = 0 self.client = actionlib.SimpleActionClient(self.action_server, ReceiveObjectAction) self.client.wait_for_server(rospy.Duration(10.)) gripper_controller_class = getattr( import_module(self.gripper_controller_pkg), 'GripperController') self.gripper_controller = gripper_controller_class()
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__( self, 'get_user_data', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed', 'failed_after_retrying'], input_keys=['destination_locations']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'get_user_data') self.number_of_retries = kwargs.get('number_of_retries', 0) self.retry_count = 0 self.timeout = kwargs.get('timeout', 120.) self._sheet_id = kwargs.get('sheet_id', '') self._worksheet_name = kwargs.get('worksheet_name', 'responses') self._num_known_entries = None self._loop_rate_s = kwargs.get('loop_rate_s', 2.) data = self._load_spreadsheet() if data: self._num_known_entries = data.shape[0] rospy.loginfo( "[get_user_data] Initialized the user data query with {0} prior known users" .format(self._num_known_entries)) else: rospy.logerr( "[get_user_data] Could not initialize user data sheet!")
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__( self, 'describe_location', save_sm_state=save_sm_state, input_keys=['target_entity'], outcomes=['succeeded', 'failed', 'failed_after_retrying']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'describe_location') self.number_of_retries = kwargs.get('number_of_retries', 0) self.pose_timeout = kwargs.get('pose_timeout', 5.) self.pose_topic_name = kwargs.get('pose_topic_name', '/amcl_pose') self.retry_count = 0 self.current_pose = None self.current_top_pos = None rospy.loginfo( '[describe_location] Waiting for topological_position server') self.topological_position_client = rospy.ServiceProxy( 'topological_position', TopologicalPosition) rospy.loginfo('[describe_location] topological_position server up') rospy.loginfo( '[describe_location] Waiting for topological_path_plan server') self.topological_path_client = rospy.ServiceProxy( 'topological_path_plan', TopologicalPath) rospy.loginfo('[describe_location] topological_path_plan server up')
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'store_groceries_goal_selector', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'store_groceries_goal_selector') self.number_of_shelves = kwargs.get('number_of_shelves', -1) self.planner_interface = PlannerInterface()
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'say_sentence', save_sm_state=save_sm_state, outcomes=['succeeded']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'say_sentence') self.sentences = list(kwargs.get('sentences', list())) self.sleep_time = kwargs.get('sleep_time', -1.)
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'describe_person', save_sm_state=save_sm_state, input_keys=['person_name'], outcomes=['succeeded']) self.bridge = CvBridge() self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'describe_person')
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__( self, 'store_person', save_sm_state=save_sm_state, outcomes=['succeeded', 'retake_picture', 'failed']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'store_person') self.person_msg_id = kwargs.get('person_msg_id', 'person_0')
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'say', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed']) self.timeout = rospy.Duration.from_sec(kwargs.get('timeout', 10.)) self.sentence = kwargs.get('sentence', '') self.sound_topic = kwargs.get('sound_topic', '/say') self.waiting_time_sec = kwargs.get('waiting_time_sec', 5.)
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'find_people', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed', 'failed_after_retrying']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'find_people') self.number_of_retries = kwargs.get('number_of_retries', 0) self.retry_count = 0 self.timeout = 120.
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'open_door', save_sm_state=save_sm_state, outcomes=['succeeded', 'waiting'], output_keys=['command']) self.timeout = rospy.Duration.from_sec(kwargs.get('timeout', 10.)) self.start_time = rospy.Time.now() self.restart_state = False self.asked_for_door_opening = False
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'identify_posture', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed'], input_keys=['person_list'], output_keys=['posture', 'person_pose']) self.sm_id = kwargs.get('sm_id', 'mdr_demo_context_aware_hand_over') self.posture_height_width_ratio_ranges = kwargs.get( 'posture_height_width_ratio_ranges', {})
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'select_object_for_picking', save_sm_state=save_sm_state, input_keys=['detected_objects'], output_keys=['selected_object'], outcomes=['succeeded']) self.tf_listener = tf.TransformListener() self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'select_object_for_picking')
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'wait_for_name', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed']) self.timeout = rospy.Duration.from_sec(kwargs.get('timeout', 10.)) self.speech_topic = kwargs.get('speech_topic', '/recognized_speech') self.robot_name = kwargs.get('name', 'bot') self.cmd_received = False self.speech_sub = rospy.Subscriber(self.speech_topic, String, self.get_speech_cmd)
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'place', save_sm_state=save_sm_state, input_keys=['grasped_object'], outcomes=['pick_new_object', 'finished', 'failed', 'failed_after_retrying']) self.sm_id = kwargs.get('sm_id', 'mdr_store_groceries') self.state_name = kwargs.get('state_name', 'place') self.timeout = kwargs.get('timeout', 120.) self.number_of_retries = kwargs.get('number_of_retries', 0) self.retry_count = 0
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'listen', save_sm_state=save_sm_state, output_keys=['listen_result'], outcomes=['received_command', 'failed']) self.listen_server = kwargs.get('listen_server', '/mdr_actions/listen_server') self.listen_client = actionlib.SimpleActionClient( self.listen_server, ListenAction) self.listen_client.wait_for_server()
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__( self, 'perceive_plane', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed', 'failed_after_retrying']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'perceive_planes') self.timeout = kwargs.get('timeout', 120.) self.plane_prefix = kwargs.get('plane_prefix', 0) self.number_of_retries = kwargs.get('number_of_retries', 0) self.retry_count = 0
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'detect_person', save_sm_state=save_sm_state, outcomes=['succeeded', 'failed', 'failed_after_retrying'], output_keys=['person_list']) self.sm_id = kwargs.get('sm_id', 'mdr_demo_context_aware_hand_over') self.action_server = kwargs.get('action_server', 'find_people_server') self.timeout = kwargs.get('timeout', 120.) self.number_of_retries = kwargs.get('number_of_retries', 0) self.retry_count = 0 self.client = actionlib.SimpleActionClient(self.action_server, FindPeopleAction) self.client.wait_for_server(rospy.Duration(10.))
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'find_objects', save_sm_state=save_sm_state, outcomes=[ 'succeeded', 'no_objects', 'failed', 'failed_after_retrying' ], input_keys=[ 'environment_objects', 'floor_objects_cleared', 'table_objects_cleared', 'object_location', 'destination_locations' ], output_keys=[ 'detected_objects', 'floor_objects_cleared', 'table_objects_cleared' ]) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'find_objects') self.object_detection_timeout_s = kwargs.get( 'object_detection_timeout', 5.) self.object_detection_server_name = kwargs.get( 'object_detection_server_name', '/mas_perception/detect_objects') self.cloud_obstacle_detection_topic = kwargs.get( 'cloud_obstacle_detection_topic', '/mas_perception/cloud_obstacle_detection/obstacle_objects') self.cloud_obstacle_cache_reset_topic = kwargs.get( 'cloud_obstacle_cache_reset_topic', '/mas_perception/cloud_obstacle_detection/reset_cache') self.max_allowed_obj_height_cm = kwargs.get( 'max_allowed_obj_height_cm', 0.15) self.min_allowed_dist_to_leg = kwargs.get('min_allowed_dist_to_leg', 0.15) self.tables_to_clean_up = kwargs.get('tables_to_clean_up', ['long_table_b', 'tall_table']) self.number_of_retries = kwargs.get('number_of_retries', 0) self.large_object_size_threshold = kwargs.get( 'large_object_size_threshold', 0.15) self.large_object_height_threshold = kwargs.get( 'large_object_height_threshold', 0.075) self.large_object_aspect_ratio_threshold = kwargs.get( 'large_object_aspect_ratio_threshold', 0.4) self.filtered_poses_topic = kwargs.get('filtered_poses_topic', 'filtered_object_poses') self.filtered_poses_pub = rospy.Publisher(self.filtered_poses_topic, PoseArray, queue_size=1) self.__init_ros_components()
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__( self, 'throw', save_sm_state=save_sm_state, input_keys=['grasped_object'], outcomes=['succeeded', 'failed', 'failed_after_retrying']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'throw') self.throwing_target_name = kwargs.get('throwing_target_name', '') self.timeout = kwargs.get('timeout', 120.) self.number_of_retries = kwargs.get('number_of_retries', 0) self.retry_count = 0
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__( self, 'place', save_sm_state=save_sm_state, input_keys=['grasped_object'], outcomes=['succeeded', 'failed', 'failed_after_retrying']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'place_based_on_category') self.placing_surface_prefix = kwargs.get('placing_surface_prefix', '') self.timeout = kwargs.get('timeout', 120.) self.number_of_retries = kwargs.get('number_of_retries', 0) self.retry_count = 0
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'describe_people', save_sm_state=save_sm_state, outcomes=[ 'succeeded', 'failed', 'no_image_received', 'failed_after_retrying' ]) self.timeout = rospy.Duration.from_sec(kwargs.get('timeout', 10.)) self.no_detection_waiting_time = kwargs.get( 'no_detection_waiting_time', 5.) self.image_topic = kwargs.get('image_topic', '/image') self.detect_person_server = kwargs.get( 'detect_person_server', '/mdr_actions/detect_person_server') self.recognize_emotion_server = kwargs.get( 'recognize_emotion_server', '/mdr_actions/recognize_emotion_server') self.recognize_gender_server = kwargs.get( 'recognize_gender_server', '/mdr_actions/gender_recognition_server') self.sound_topic = kwargs.get('sound_topic', '/say') self.number_of_retries = kwargs.get('number_of_retries', 0) self.retry_count = 0 self.image_sub = rospy.Subscriber(self.image_topic, Image, self.get_image) self.image_received = False self.image = None self.detect_person_client = actionlib.SimpleActionClient( self.detect_person_server, DetectPersonAction) rospy.loginfo('Waiting for %s' % self.detect_person_server) self.detect_person_client.wait_for_server() self.recognize_emotion_client = actionlib.SimpleActionClient( self.recognize_emotion_server, RecognizeEmotionAction) rospy.loginfo('Waiting for %s' % self.recognize_emotion_server) self.recognize_emotion_client.wait_for_server() self.recognize_gender_client = actionlib.SimpleActionClient( self.recognize_gender_server, GenderRecognitionAction) rospy.loginfo('Waiting for %s' % self.recognize_gender_server) self.recognize_gender_client.wait_for_server() rospy.loginfo('Starting people description') self.start_time = rospy.Time.now()
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__( self, 'listen', save_sm_state=save_sm_state, input_keys=['listen_result'], outcomes=['start_store_groceries', 'continue_waiting']) self.start_command = kwargs.get('start_command', 'store groceries') self.process_command_server = kwargs.get( 'process_command_server', '/mdr_actions/process_speech_command_server') self.process_command_client = actionlib.SimpleActionClient( self.process_command_server, ProcessSpeechAction) self.process_command_client.wait_for_server()
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'release_object', save_sm_state=save_sm_state, input_keys=['grasped_object', 'environment_objects'], outcomes=['succeeded', 'failed', 'failed_after_retrying']) self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'release_object') self.action_server_name = kwargs.get('action_server_name', 'place_server') self.release_timeout = kwargs.get('release_timeout', 120.) self.number_of_retries = kwargs.get('number_of_retries', 0) self.release_target_name = kwargs.get('release_target_name', 'bin_a') self.retry_count = 0 self.tf_listener = tf.TransformListener()
def __init__(self, save_sm_state=False, **kwargs): ScenarioStateBase.__init__(self, 'detect_person', save_sm_state=save_sm_state, input_keys=[], output_keys=[], outcomes=['succeeded', 'failed', 'failed_after_retrying']) # Get parameters self.sm_id = kwargs.get('sm_id', '') self.state_name = kwargs.get('state_name', 'detect_person') self.number_of_retries = kwargs.get('number_of_retries', 0) self.timeout = kwargs.get('timeout', 30) # not used right now self.person_in_front = False self.person_status_sub = rospy.Subscriber(DetectPerson.DOOR_STATUS_TOPIC, Bool, self.update_person_in_front)