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