def __align_base_with_pose(self, pose_base_link):
'''Moves the base so that the elbow is aligned with the goal pose.
Keyword arguments:
pose_base_link -- a 'geometry_msgs/PoseStamped' message representing
the goal pose in the base link frame
'''
aligned_base_pose = PoseStamped()
aligned_base_pose.header.frame_id = 'base_link'
aligned_base_pose.header.stamp = rospy.Time.now()
aligned_base_pose.pose.position.x = 0.
aligned_base_pose.pose.position.y = pose_base_link.pose.position.y - self.base_elbow_offset
aligned_base_pose.pose.position.z = 0.
aligned_base_pose.pose.orientation.x = 0.
aligned_base_pose.pose.orientation.y = 0.
aligned_base_pose.pose.orientation.z = 0.
aligned_base_pose.pose.orientation.w = 1.
move_base_goal = MoveBaseGoal()
move_base_goal.goal_type = MoveBaseGoal.POSE
move_base_goal.pose = aligned_base_pose
self.move_base_client.send_goal(move_base_goal)
self.move_base_client.wait_for_result()
self.move_base_client.get_result()
def execute(self, userdata):
unknown_people = self.kb_interface.get_all_attributes('unknown')
people_identifiers = []
for item in unknown_people:
if not item.is_negative:
for param in item.values:
if param.key == 'person':
people_identifiers.append(param.value)
print(people_identifiers)
if not people_identifiers:
print('[move_to_person] No people found; aborting operation')
return 'failed_after_retrying'
person_to_interview = people_identifiers[0]
person_info = self.kb_interface.get_obj_instance(
person_to_interview, Person._type)
person_pose = MoveToPerson.subtract_distance_from_pose(
person_info.pose, 1)
move_base_goal = MoveBaseGoal()
move_base_goal.goal_type = MoveBaseGoal.POSE
move_base_goal.pose = person_pose
rospy.loginfo('[move_to_person] Going to person ' +
person_to_interview + ' at pose ' +
str(person_pose.pose.position.x) + ' ' +
str(person_pose.pose.position.y))
self.move_base_client.send_goal(move_base_goal)
self.move_base_client.wait_for_result(
rospy.Duration.from_sec(int(self.move_base_timeout)))
result = self.move_base_client.get_result()
if result and result.success:
return 'succeeded'
if self.retry_count == self.number_of_retries:
rospy.loginfo('[move_to_person] Could not go to %s',
person_to_interview)
self.say('[move_to_person] Could not go to %s; aborting operation',
person_to_interview)
return 'failed_after_retrying'
rospy.logerr('[move_to_person] Could not go to %s; retrying',
person_to_interview)
self.retry_count += 1
return 'failed'
def execute(self, userdata):
if self.save_sm_state:
self.save_current_state()
# if the current location of the robot was not specified
# in the request, we query this information from the knowledge base
original_location = ''
request = rosplan_srvs.GetAttributeServiceRequest()
request.predicate_name = 'robot_at'
result = self.attribute_fetching_client(request)
for item in result.attributes:
if not item.is_negative:
for param in item.values:
if param.key == 'bot' and param.value != self.robot_name:
break
if param.key == 'wp':
original_location = param.value
break
break
for destination_location in self.destination_locations:
if destination_location.lower().find('table') != -1:
self.say('Moving to table')
table_pose = self.get_table_pose()
if table_pose is None:
rospy.loginfo('Could not get table pose')
self.say('Could not move to table')
return 'failed'
move_base_goal = MoveBaseGoal()
move_base_goal.goal_type = MoveBaseGoal.POSE
move_base_goal.pose = table_pose
self.move_base_client.send_goal(move_base_goal)
self.move_base_client.wait_for_result()
result = self.move_base_client.get_result()
if result:
return 'succeeded'
else:
dispatch_msg = self.get_dispatch_msg(original_location,
destination_location)
rospy.loginfo('Sending the base to %s' % destination_location)
self.say('Going to ' + destination_location)
self.action_dispatch_pub.publish(dispatch_msg)
self.executing = True
self.succeeded = False
start_time = time.time()
duration = 0.
while self.executing and duration < self.timeout:
rospy.sleep(0.1)
duration = time.time() - start_time
if self.succeeded:
rospy.loginfo('Successfully reached %s' % destination_location)
original_location = destination_location
else:
rospy.logerr('Could not reach %s' % destination_location)
self.say('Could not reach ' + destination_location)
if self.retry_count == self.number_of_retries:
self.say('Aborting operation')
return 'failed_after_retrying'
self.retry_count += 1
return 'failed'
return 'succeeded'