Example #1
0
    def execute(self, userdata):
        # go to a specified location where the user is at
        approach_user_point = self.ogm.approach_user(self.tiago.user_point)
        self.tiago.approach_user_point = approach_user_point

        # go to approach point, orientation will be off
        self.tiago.goto_location(
            Pose(approach_user_point,
                 Util.quaternion_at_point(approach_user_point)))
        # orient to face table user point
        self.tiago.goto_location(
            Util.turn_towards_point(
                Point(self.tiago.user_point.x, self.tiago.user_point.y, 0.0)))

        self.tiago.talk('Hello, what would you like me to do ?')

        search_n_fetch_queries = self.extract_tasks_with_dialogue()

        if len(search_n_fetch_queries):
            print(search_n_fetch_queries)
            rospy.set_param('/search_n_fetch_queries', search_n_fetch_queries)
            self.tiago.talk('I will be right back, with your requests.')
            return 'start_done'
        else:
            self.tiago.talk(
                'I did not get any requests from you. I will go back to sleep now.'
            )
            return 'start_abort'
Example #2
0
 def go_to_user_location(self):
     # go to approach point, orientation will be off
     self.tiago.goto_location(
         Pose(self.tiago.approach_user_point,
              Util.quaternion_at_point(self.tiago.approach_user_point)))
     # orient to face table user point
     self.tiago.goto_location(
         Util.turn_towards_point(
             Point(self.tiago.user_point.x, self.tiago.user_point.y, 0.0)))
Example #3
0
    def approach_object(self, current_furniture, object):
        # calculate object approach point and navigate to it
        approach_object_point = self.ogm.approach_object(
            current_furniture, object)
        self.tiago.goto_location(
            Pose(approach_object_point,
                 Util.quaternion_at_point(approach_object_point)))

        # orient to face object
        self.tiago.goto_location(Util.turn_towards_point(object.centroid))
Example #4
0
    def execute(self, userdata):
        # get search and fetch params
        search_n_fetch_queries = rospy.get_param('/search_n_fetch_queries')

        for index in range(0, len(self.tiago.furniture)):
            current_furniture = self.tiago.furniture[index]
            current_furniture['objects'] = []
            rospy.loginfo('Going to furniture %s' % current_furniture['name'])

            # initial approach to nearest free spot. orientation will be wrong
            approach_point = self.ogm.approach_furniture(current_furniture)
            self.tiago.goto_location(Pose(approach_point, Util.quaternion_at_point(approach_point)))

            # orient to face table center
            self.tiago.goto_location(Util.turn_towards_point(Point(approach_point.x, current_furniture['top_right'].y, 0.0)))

            self.tiago.talk('I am about to scan this furniture for objects.')

            self.tiago.play('lower_head')
            self.look_for_objects(current_furniture)

            center_y = (current_furniture['bottom_left'].y + current_furniture['top_right'].y)/2
            # turn head to cover left and right areas of the furniture
            current_pose = self.tiago.get_robot_pose()
            self.tiago.look_at(Util.map_to_base(Point(current_furniture['top_left'].x, center_y, 0.0), current_pose))
            self.look_for_objects(current_furniture)

            self.tiago.look_at(Util.map_to_base(Point(current_furniture['top_right'].x, center_y, 0.0), current_pose))
            self.look_for_objects(current_furniture)
            # reset head back to normal
            self.tiago.play('lower_head')

            if len(current_furniture['objects']):
                search_n_fetch_queries, requests = Util.create_snf_requests(current_furniture['objects'], search_n_fetch_queries, current_furniture)
                self.search_n_fetch_requests.extend(requests)

            # exit explore loop when queries are over
            if not len(search_n_fetch_queries):
                break

        rospy.set_param('/search_n_fetch_queries', search_n_fetch_queries)
        self.tiago.snf_requests = self.search_n_fetch_requests
        # transition to pick object or go back to user directly
        if len(filter(lambda q: q['intent'] == 'fetch', self.tiago.snf_requests)):    # if there are fetch queries
            return 'manipulate_object'
        else:
            return 'report_back'    # only search queries to report about