def execute(self, userdata):
userdata.old_status=userdata.old_status
userdata.feadback=userdata.feadback
rospy.logwarn("-------olf status : "+str(userdata.old_status)+ "mew satusss : "+str(userdata.feadback))
msg=person()
# it means that is not the first time
if userdata.old_status == userdata.feadback :
if (userdata.old_status==OCLUDED) and (rospy.get_rostime().secs-self.time_voice_init.secs)>TIME_SPEACK_OCLUDED :
self.time_voice_init=rospy.get_rostime()
userdata.tts_text=TIME_OCLUDED_SAY
return 'liftime'
if (userdata.old_status==OKI) and (rospy.get_rostime().secs-self.time_voice_init.secs)>TIME_SPEACK_OK :
self.time_voice_init=rospy.get_rostime()
userdata.tts_text=TIME_OK_SAY
return 'liftime'
if (userdata.old_status==NEAR) and (rospy.get_rostime().secs-self.time_voice_init.secs)>TIME_SPEACK_NEAR :
self.time_voice_init=rospy.get_rostime()
userdata.tts_text=TIME_NEAR_SAY
return 'liftime'
else :
self.time_voice_init=rospy.get_rostime()
userdata.old_status=userdata.feadback
userdata.old_status = userdata.feadback
return 'no_liftime'
return 'no_liftime'
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 face_cb(self, data):
faces = data
Face = FaceDetection()
per = person()
if faces.faces:
for Face in faces.faces:
per.targetId = 1
per.x = Face.position.z
per.y = -Face.position.x
per.status = 4
self.persons.peopleSet.append(per)
self.follow_person_pub.publish(self.persons)
def face_cb(self,data):
faces=data
Face=FaceDetection()
per=person()
if faces.faces :
for Face in faces.faces :
per.targetId=1
per.x=Face.position.z
per.y=-Face.position.x
per.status=4
self.persons.peopleSet.append(per)
self.follow_person_pub.publish(self.persons)
def execute(self, userdata):
userdata.in_learn_person=None
person_detect=userdata.tracking_msg
per_aux=person()
#per_follow=[]
for person_aux in person_detect.peopleSet :
if (-Y_CALIBRARTION<person_aux.y<Y_CALIBRARTION) and person_aux.status>=3 :
userdata.in_learn_person=person_aux
rospy.loginfo(OKGREEN+"i have learned the person whit ID : "
+ str(userdata.in_learn_person)+ENDC)
return 'succeeded'
break
userdata.in_learn_person=None
return 'aborted'
def execute(self, userdata):
userdata.in_learn_person = None
person_detect = userdata.tracking_msg
per_aux = person()
#per_follow=[]
for person_aux in person_detect.peopleSet:
if (-Y_CALIBRARTION < person_aux.y <
Y_CALIBRARTION) and person_aux.status >= 3:
userdata.in_learn_person = person_aux
rospy.loginfo(OKGREEN +
"i have learned the person whit ID : " +
str(userdata.in_learn_person) + ENDC)
return 'succeeded'
break
userdata.in_learn_person = None
return 'aborted'
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
