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