def __init__(self): self.name = str(raw_input('Name of the face :')) rospy.loginfo("Initializing tracker_people_service") self.follow_person_pub = rospy.Publisher( '/people_tracker_node/peopleSet', personArray) self.face_subs = rospy.Subscriber("/pal_face/faces", FaceDetections, self.face_cb) self.persons = personArray()
def run(self): """Publishing usersaid when face recognitionL is enabled """ # TODO: add tags, add other fields, take into account loaded grammar to put other text in the recognized sentence PersonArray = personArray() pers = person() pers.x = 0 pers.y = 1 pers.targetId = 1 pers.status = 4 PersonArray.peopleSet.append(pers) pers = person() pers.x = 1 pers.y = 0 pers.targetId = 1 pers.status = 4 PersonArray.peopleSet.append(pers) while not rospy.is_shutdown(): self.person_pub.publish(PersonArray) rospy.sleep(0.1) # publish a 10 Hz
def run(self): """Publishing usersaid when face recognitionL is enabled """ # TODO: add tags, add other fields, take into account loaded grammar to put other text in the recognized sentence PersonArray=personArray() pers=person() pers.x=0 pers.y=1 pers.targetId=1 pers.status=4 PersonArray.peopleSet.append(pers) pers=person() pers.x=1 pers.y=0 pers.targetId=1 pers.status=4 PersonArray.peopleSet.append(pers) while not rospy.is_shutdown(): self.person_pub.publish(PersonArray) rospy.sleep(0.1)# publish a 10 Hz
def create_tracker_msgs(self): current_msg = copy.deepcopy(self.last_faces_msg) per = person() # person msg person_arr = personArray() pose_stamped_first_face = None # rospy.loginfo("waiting for transform...") latest_common_tf_time = self.tf_listener.getLatestCommonTime("base_link", current_msg.header.frame_id) # self.tf_listener.waitForTransform("odom", current_msg.header.frame_id, latest_common_tf_time, rospy.Duration(1.0)) # Not needed # rospy.loginfo("got a transform!") if len(current_msg.faces) > 0: # should always be filled, but just in case target_id = 1 for face in current_msg.faces: # Temporal PoseStamped for transforming into odom frame the 3d pose of the face detection face_ps = PoseStamped() face_ps.header.frame_id = current_msg.header.frame_id face_ps.header.stamp = latest_common_tf_time # rospy.Time.now() #latest_common_tf_time face_ps.pose.position = face.position face_ps.pose.orientation.w = 1.0 current_robot_ps = PoseStamped() current_robot_ps.header.frame_id = "base_link" current_robot_ps.header.stamp = rospy.Time.now() current_robot_ps.pose.position = Point(0.0, 0.0, 0.0) current_robot_ps.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0) # transform the pose of the face detection to base_link to compute on that frame transform_ok = False while not transform_ok: # this is ugly as is polling a lot to TF... but works try: transformed_baselink_pose = self.tf_listener.transformPose("base_link", face_ps) transform_ok = True except tf.ExtrapolationException: rospy.logwarn("Exception on transforming transformed_pose... trying again.") face_ps.header.stamp = rospy.Time.now() # Compute the orientation of the tracking goal in base_link transformed_baselink_pose.pose.orientation = compute_orientation( transformed_baselink_pose, current_robot_ps ) transformed_baselink_pose.pose.position.z = 0.0 # transform the pose to odom for convenience (follow planner needs odom) # transform_ok = False # while not transform_ok: # try: # transformed_odom_pose = self.tf_listener.transformPose("odom", transformed_baselink_pose) # transform_ok = True # except tf.ExtrapolationException: # rospy.logwarn("Exception on transforming transformed_pose... trying again.") # transformed_baselink_pose.header.stamp = rospy.Time.now() if not pose_stamped_first_face: # pose_stamped_first_face = transformed_odom_pose pose_stamped_first_face = transformed_baselink_pose per.targetId = target_id target_id += 1 per.x = transformed_baselink_pose.pose.position.x per.y = transformed_baselink_pose.pose.position.y per.status = 4 person_arr.peopleSet.append(per) return person_arr, pose_stamped_first_face else: rospy.logerr("Message without faces in publisher, shouldn't happen") return None, None
def __init__(self): self.name=str(raw_input('Name of the face :')) rospy.loginfo("Initializing tracker_people_service") self.follow_person_pub=rospy.Publisher('/people_tracker_node/peopleSet',personArray) self.face_subs=rospy.Subscriber("/pal_face/faces", FaceDetections, self.face_cb) self.persons=personArray()