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'