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.)
Example #3
0
    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')
Example #4
0
    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()
Example #8
0
    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.)
Example #12
0
 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')
Example #14
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
Example #17
0
 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', {})
Example #18
0
 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')
Example #19
0
 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)
Example #20
0
    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
Example #21
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()
Example #25
0
    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()
Example #29
0
 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)