def _operator_standing_still_for_x_seconds(self, timeout):
"""Check whether the operator is standing still for X seconds
:param timeout how many seconds must the operator be standing still before returning True
:type timeout float
:returns bool indicating whether the operator has been standing still for longer than timeout seconds"""
if not self._operator:
return False
operator_current_fs = kdl_conversions.FrameStamped(
self._operator._pose, "/map", stamp=rospy.Time.now())
# print "Operator position: %s" % self._operator.pose.position
if not self._last_operator_fs:
self._last_operator_fs = operator_current_fs
else:
# Compare the pose with the last pose and update if difference is larger than x
if (operator_current_fs.frame.p -
self._last_operator_fs.frame.p).Norm() > 0.15:
# Update the last pose
self._last_operator_fs = operator_current_fs
else:
print "Operator is standing still for %f seconds" % (
operator_current_fs.stamp -
self._last_operator_fs.stamp).to_sec()
# Check whether we passed the timeout
if (operator_current_fs.stamp -
self._last_operator_fs.stamp).to_sec() > timeout:
return True
return False
def get_frame_from_vector(x_vector, origin):
unit_z = kdl.Vector(0, 0, 1)
unit_z_cross_diff = (unit_z * x_vector) / (unit_z * x_vector).Norm()
y_vector = x_vector * unit_z_cross_diff
z_vector = x_vector * y_vector
rotation = kdl.Rotation(x_vector, y_vector, z_vector)
translation = origin.vector
frame_stamped = kdl_conversions.FrameStamped(kdl.Frame(rotation, translation), origin.frame_id)
return frame_stamped
def execute(self, userdata=None):
rospy.loginfo("Trying to find {}".format(self._person_label))
self._robot.head.look_at_standing_person()
self._robot.speech.speak(
"{}, please look at me while I am looking for you".format(
self._person_label),
block=False)
start_time = rospy.Time.now()
look_distance = 2.0 # magic number 4
look_angles = [
f * math.pi / d if d != 0 else 0.0 for f in [-1, 1]
for d in [0, 6, 4, 2.3]
] # Magic numbers
head_goals = [
kdl_conversions.VectorStamped(x=look_distance * math.cos(angle),
y=look_distance * math.sin(angle),
z=1.7,
frame_id="/%s/base_link" %
self._robot.robot_name)
for angle in look_angles
]
i = 0
while (rospy.Time.now() - start_time).to_sec() < self._timeout:
if self.preempt_requested():
return 'failed'
self._robot.head.look_at_point(head_goals[i])
i += 1
if i == len(head_goals):
i = 0
self._robot.head.wait_for_motion_done()
raw_detections = self._robot.perception.detect_faces()
best_detection = self._robot.perception.get_best_face_recognition(
raw_detections,
self._person_label,
probability_threshold=self._probability_threshold)
rospy.loginfo("best_detection = {}".format(best_detection))
if not best_detection:
continue
roi = best_detection.roi
try:
person_pos_kdl = self._robot.perception.project_roi(
roi=roi, frame_id="map")
except Exception as e:
rospy.logerr("head.project_roi failed: %s", e)
return 'failed'
person_pos_ros = kdl_conversions.kdl_vector_stamped_to_point_stamped(
person_pos_kdl)
self._face_pos_pub.publish(person_pos_ros)
found_person = self._robot.ed.get_closest_laser_entity(
radius=self._look_distance, center_point=person_pos_kdl)
if found_person:
self._robot.speech.speak("I found {}".format(
self._person_label),
block=False)
self._robot.head.close()
self._robot.ed.update_entity(
id=self._person_label,
frame_stamped=kdl_conversions.FrameStamped(
kdl.Frame(person_pos_kdl.vector), "/map"),
type="waypoint")
return 'found'
else:
rospy.logwarn("Could not find {} in the {}".format(
self._person_label, self.area))
self._robot.head.close()
rospy.sleep(2.0)
return 'failed'