def callback(self, msg):
new_msg = PeopleTracker()
new_msg.header = msg.header
for i in range(len(msg.uuids)):
if msg.uuids[i] in self.selected_uuids:
new_msg.uuids.append(msg.uuids[i])
new_msg.poses.append(msg.poses[i])
new_msg.velocities.append(msg.velocities[i])
new_msg.distances.append(msg.distances[i])
new_msg.angles.append(msg.angles[i])
if msg.distances[i] < new_msg.min_distance:
new_msg.min_distance = msg.distances[i]
if msg.angles[i] < new_msg.min_distance_angle:
new_msg.min_distance_angle = msg.angles[i]
self.pub.publish(new_msg)
def odom_cb(self, msg):
j = PeopleTracker()
j.header = msg.header
pose1 = msg.pose.pose
pose2 = Pose()
pose2.position.x = 1
pose2.position.y = -1
markerPeople = Marker()
markerPeople.header = msg.header
markerPeople.ns='people_tracker'
markerPeople.id=1
markerPeople.type=markerPeople.SPHERE
scalePeople = Vector3()
scalePeople.x=0.2
scalePeople.y=0.2
scalePeople.z=0.2
markerPeople.scale=scalePeople
colorPeople = ColorRGBA()
colorPeople.r=0.0
colorPeople.g=1.0
colorPeople.b=0.0
colorPeople.a=1.0
markerPeople.color = colorPeople
markerPeople.lifetime.secs=1
markerPeople.lifetime.nsecs=0
markerArrayPeople = MarkerArray()
# if (counter % 500)==0:
# if people_inside:
# people_inside=False
# else:
people_inside=True
if people_inside:
j.poses.append(pose1)
# j.poses.append(pose2)
markerPeople.pose=pose1
markerArrayPeople.markers.append(markerPeople)
#print(j.header.seq)
self.publisher.publish(j)
self.publisher_marker.publish(markerPeople)
self.publisher_marray.publish(markerArrayPeople)
self.counter = self.counter + 1
def person_callback(self, msg):
a = PeopleTracker()
a.header = msg.header
a.min_distance = np.inf
a.min_distance_angle = 0
for t in msg.tracks:
b = np.uint8(t.track_id)
a.uuids.append(str(b))
person = Pose()
person.position = t.pose.pose.position
person.orientation = t.pose.pose.orientation
a.poses.append(person)
v = Vector3()
v.x = t.twist.twist.linear.x
v.y = t.twist.twist.linear.y
v.z = t.twist.twist.linear.z
a.velocities.append(v)
person = PoseStamped()
person.header = msg.header
#person.header.frame_id = self.target_frame
person.pose = t.pose.pose
if msg.header.frame_id != self.target_frame:
try:
ti = self.listener.getLatestCommonTime(
self.target_frame, msg.header.frame_id)
msg.header.stamp = ti
transformed_person = self.listener.transformPose(
self.target_frame, person)
except (tf.Exception, tf.LookupException,
tf.ConnectivityException) as ex:
rospy.logerr("[" + rospy.get_name() + "]: " +
"Can't transform, reason: " + str(ex))
return None
x1 = transformed_person.pose.position.x
y1 = transformed_person.pose.position.y
z1 = transformed_person.pose.position.z
dis = round(
math.sqrt(math.pow(x1, 2) + math.pow(y1, 2) + math.pow(z1, 2)),
3)
a.distances.append(dis)
#ang = math.atan2(math.sqrt(math.pow(x1,2)+math.pow(y1,2)),z1)
ang = math.atan2(y1, x1)
a.angles.append(ang)
if dis < a.min_distance:
a.min_distance = dis
a.min_distance_angle = ang
self.pub.publish(a)
def __init__(self):
person_topic = rospy.get_param("~person_topic",
"/robot4/perception/tracked_persons")
ppl_topic = rospy.get_param("~ppl_topic",
"/robot4/people_tracker/positions")
self.target_frame = rospy.get_param("~target_frame",
"robot4/base_link")
self.listener = tf.TransformListener()
self.pub = rospy.Publisher(ppl_topic, PeopleTracker, queue_size=1)
self.last_msg = PeopleTracker()
rospy.Subscriber(person_topic,
TrackedPersons,
callback=self.person_callback
#queue_size=10
)
publisher = rospy.Publisher('/people_tracker/positions',
PeopleTracker,
queue_size=10)
publisher_marker = rospy.Publisher('/visualization_marker',
Marker,
queue_size=10)
publisher_marray = rospy.Publisher('/people_tracker/marker_array',
MarkerArray,
queue_size=10)
rate = rospy.Rate(30)
counter = 0
people_inside = False
while not rospy.is_shutdown():
j = PeopleTracker()
j.header = Header()
#j.header.stamp = {'secs' : datetime.datetime.now().time().second , 'nsecs' : datetime.datetime.now().time().microsecond}
now = rospy.Time.now()
j.header.stamp.secs = now.secs
j.header.stamp.nsecs = now.nsecs
j.header.frame_id = '/odom'
pose1 = Pose()
pose1.position.x = 0
pose1.position.y = 5
pose2 = Pose()
pose2.position.x = 1
pose2.position.y = -1
markerPeople = Marker()
def callback(msg):
global K_kinect
global pix_kinect
global pix_kinect_array
poses = PoseArray()
poses_kinect = PoseArray()
pixels_kinect = PoseArray()
people_kinect = PeopleTracker()
# print len(msg.poses)
if len(msg.poses)>0:
poses = msg.poses
pix_kinect_array =[]
for prs in range(0,len(msg.poses)):
# print prs
pose = PoseStamped()
pose.pose = poses[prs]
pose.header = Header()
pose.header = msg.header
pose_kinect = PoseStamped()
#print poses[prs]
# listener.waitForTransform('/odom_combined','/k2/depth_frame',rospy.Time.now(),rospy.Duration(0.1))
pose_kinect = listener.transformPose(target_frame,pose)
# print pose_kinect
#poses_kinect.append(pose_kinect.poses)
pt_kinect = numpy.zeros([3, 1])
pt_kinect[0] = pose_kinect.pose.position.x
pt_kinect[1] = -0.4
pt_kinect[2] = pose_kinect.pose.position.z
pt_kinect /= pt_kinect[2]
#print (pt_kinect)
pix_kinect = K_kinect*pt_kinect
#print type(K_kinect)
#print type(pt_kinect)
#print type(pix_kinect)
# print K_kinect.shape
# print pt_kinect.shape
# print pix_kinect.shape
#print K_kinect.shape
#print pt_kinect
#print pix_kinect
pix_kinect_array.append(pix_kinect)
#print (pix_kinect_array)
pix_kinect_pose = Pose()
pix_kinect_pose.position.x = pix_kinect[0]
pix_kinect_pose.position.y = pix_kinect[1]
pix_kinect_pose.position.z = pix_kinect[2]
# print pix_kinect_pose
pixels_kinect.poses.append(pix_kinect_pose)
# new_orientation = tf.transformations.euler_from_quaternion([pose_kinect.pose.orientation.x,pose_kinect.pose.orientation.y,pose_kinect.pose.orientation.z,pose_kinect.pose.orientation.w])
# print (new_orientation[1]/math.pi)*180
# print prs
# print pose_kinect.pose.position
# markerPeople = Marker()
# markerPeople.header = Header()
# markerPeople.header = msg.header
# markerPeople.ns='people_tracker'
# markerPeople.id=1
# markerPeople.type=markerPeople.SPHERE
# scalePeople = Vector3()
# scalePeople.x=0.2
# scalePeople.y=0.2
# scalePeople.z=0.2
# markerPeople.scale=scalePeople
# colorPeople = ColorRGBA()
# colorPeople.r=0.0
# colorPeople.g=1.0
# colorPeople.b=1.0
# colorPeople.a=1.0
# markerPeople.color = colorPeople
# markerPeople.lifetime.secs=1
# markerPeople.lifetime.nsecs=0
# markerArrayPeople = MarkerArray()
# markerPeople.pose=pose_kinect.pose
# markerArrayPeople.markers.append(markerPeople)
# people_kinect.header = Header()
# people_kinect.header = msg.header
# people_kinect.header.frame_id = '/k2/depth_frame'
# people_kinect.poses = poses_kinect
# people_kinect_msg.publish(people_kinect)
pixels_kinect.header = Header()
pixels_kinect.header = msg.header
pixels_kinect.header.frame_id = target_frame
pixels_kinect_msg.publish(pixels_kinect)
def person_callback(self, msg):
a = PeopleTracker()
#print("msg tracks: ")
#print(msg.tracks)
#for t in msg.header:
# for s in a.header:
# s.seq = t.seq
# s.stamp = t.stamp
# s.frame_id = t.frame_id
# print("Frame ID: ")
# print(s.frame_id)
a.header = msg.header
print("header: ")
print(a.header)
for t in msg.tracks:
b = np.uint8(t.track_id)
a.uuids.append(str(b)) #= b#'track_%08d' %(t.track_id)
print("track_id: ")
print(t.track_id)
print("uuids: ")
print(a.uuids)
print("poses: ")
#print(t.pose)
person = Pose()
#person.header = a.header
person.position = t.pose.pose.position
person.orientation = t.pose.pose.orientation
a.poses.append(person) # = t.pose
print(a.poses)
#for t1 in t.pose:
# for s in a.poses:
# for t2 in t1.pose:
# for t3 in t2.position:
# for s1 in s.position:
# s1.x = t3.x
# s1.y = t3.y
# s1.z = t3.z
# for t3 in t2.orientation:
# for s1 in s.orientation:
# s1.x = t3.x
# s1.y = t3.y
# s1.z = t3.z
# s1.w = t3.w
v = Vector3()
v.x = t.twist.twist.linear.x
v.y = t.twist.twist.linear.y
v.z = t.twist.twist.linear.z
a.velocities.append(v) # = t.twist
print("velocities: ")
print(a.velocities)
#for s in a.velocities:
# for t1 in t.twist:
# for t2 in t1.linear:
# s.x = t2.x
# s.y = t2.y
# s.z = t2.z
person = PoseStamped()
person.header = msg.header
print(type(t.pose.pose))
person.pose = t.pose.pose
print("person pose: ")
print(type(person))
#print(person.pose)
if msg.header.frame_id != self.target_frame:
try:
ti = self.listener.getLatestCommonTime(
self.target_frame, msg.header.frame_id)
msg.header.stamp = ti
transformed_person = self.listener.transformPose(
self.target_frame, person)
except (tf.Exception, tf.LookupException,
tf.ConnectivityException) as ex:
return None
x1 = transformed_person.pose.position.x
y1 = transformed_person.pose.position.y
z1 = transformed_person.pose.position.z
print("x: ")
print(x1)
print("y: ")
print(y1)
print("z: ")
print(z1)
dis = [
math.sqrt(math.pow(x1, 2) + math.pow(y1, 2) + math.pow(z1, 2))
] # = math.sqrt(math.pow(x,2)+math.pow(y,2)+math.pow(z,2))
a.distances.append(dis[0])
print("distances: ")
print(a.distances)
ang = [
math.atan2(math.sqrt(math.pow(x1, 2) + math.pow(y1, 2)), z1)
] # = math.atan2(math.sqrt(math.pow(x,2)+math.pow(y,2)),z)
a.angles.append(ang[0])
print("angles: ")
print(a.angles)
#self.pub.publish(a)
#print("I just published")
#break
#a.distances.append(math.sqrt(math.pow(x,2)+math.pow(y,2)+math.pow(z,2)))
#a.angles.append(math.atan2(math.sqrt(math.pow(x,2)+math.pow(y,2)),z))
a.min_distance = min(a.distances)
print("min distance: ")
print(a.min_distance)
i = 0
for d in a.distances:
if d == a.min_distance:
break
else:
i = i + 1
a.min_distance_angle = a.angles[i]
print("min distance angle: ")
print(a.min_distance_angle)
print("I am publishing People Tracker object a.")
self.pub.publish(a)