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 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
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()
markerPeople.header = Header()
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)