tracker.py
Go to the documentation of this file.
00001 #!/usr/bin/python
00002 
00003 import roslib; roslib.load_manifest('people_velocity_tracker')
00004 import rospy
00005 from geometry_msgs.msg import Point, Vector3
00006 import math
00007 from people_msgs.msg import PositionMeasurementArray, Person, People
00008 from easy_markers.generator import MarkerGenerator, Marker
00009 from kalman_filter import Kalman
00010 
00011 def distance(leg1, leg2):
00012     return math.sqrt(math.pow(leg1.x - leg2.x, 2) +
00013                      math.pow(leg1.y - leg2.y, 2) +
00014                      math.pow(leg1.z - leg2.z, 2))
00015 
00016 def average(leg1, leg2):
00017     return Point((leg1.x + leg2.x) / 2,
00018                  (leg1.y + leg2.y) / 2,
00019                  (leg1.z + leg2.z) / 2)
00020 
00021 def add(v1, v2):
00022     return Vector3(v1.x + v2.x, v1.y + v2.y, v1.z + v2.z)
00023 
00024 def subtract(v1, v2):
00025     return Vector3(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z)
00026 
00027 def scale(v, s):
00028     v.x *= s
00029     v.y *= s
00030     v.z *= s
00031 
00032 def printv(v):
00033     print "%.2f %.2f %.2f"%(v.x, v.y, v.z),
00034 
00035 gen = MarkerGenerator()
00036 gen.type = Marker.ARROW
00037 gen.ns = 'velocities'
00038 gen.lifetime = .5
00039 
00040 
00041 class PersonEstimate(object):
00042     def __init__(self, msg):
00043         self.pos = msg
00044         self.reliability = 0.1
00045         self.k = Kalman()
00046 
00047     def update(self, msg):
00048         last = self.pos
00049         self.pos = msg
00050         self.reliability = max(self.reliability, msg.reliability)
00051 
00052         ivel = subtract(self.pos.pos, last.pos)
00053         time = (self.pos.header.stamp - last.header.stamp).to_sec()
00054         scale(ivel, 1.0 / time)
00055 
00056         self.k.update([ivel.x, ivel.y, ivel.z])
00057 
00058     def age(self):
00059         return self.pos.header.stamp
00060 
00061     def id(self):
00062         return self.pos.object_id
00063 
00064     def velocity(self):
00065         k = self.k.values()
00066         if k == None:
00067             return Vector3()
00068         v = Vector3(k[0], k[1], k[2])
00069         return v
00070 
00071     def publish_markers(self, pub):
00072         gen.scale = [.1, .3, 0]
00073         gen.color = [1, 1, 1, 1]
00074         vel = self.velocity()
00075         #scale(vel, 15)
00076         m = gen.marker(points=[self.pos.pos, add(self.pos.pos, vel)])
00077         m.header = self.pos.header
00078         pub.publish(m)
00079 
00080     def get_person(self):
00081         p = Person()
00082         p.name = self.id()
00083         p.position = self.pos.pos
00084         p.velocity = self.velocity()
00085         p.reliability = self.reliability
00086         return self.pos.header.frame_id, p
00087 
00088 
00089 class VelocityTracker(object):
00090     def __init__(self):
00091         self.people = {}
00092         self.TIMEOUT = rospy.Duration(rospy.get_param('~timeout', 1.0))
00093         self.sub = rospy.Subscriber('/people_tracker_measurements',
00094                                     PositionMeasurementArray,
00095                                     self.pm_cb)
00096         self.mpub = rospy.Publisher('/visualization_marker',
00097                                     Marker,
00098                                     queue_size=10)
00099         self.ppub = rospy.Publisher('/people',
00100                                     People,
00101                                     queue_size=10)
00102 
00103     def pm_cb(self, msg):
00104         for pm in msg.people:
00105             if pm.object_id in self.people:
00106                 self.people[pm.object_id].update(pm)
00107             else:
00108                 p = PersonEstimate(pm)
00109                 self.people[pm.object_id] = p
00110 
00111     def spin(self):
00112         rate = rospy.Rate(10)
00113         while not rospy.is_shutdown():
00114             # Remove People Older Than timeout param
00115             now = rospy.Time.now()
00116             for p in self.people.values():
00117                 if now - p.age() > self.TIMEOUT:
00118                     del self.people[p.id()]
00119             self.publish()
00120             rate.sleep()
00121 
00122     def publish(self):
00123         gen.counter = 0
00124         pl = People()
00125         pl.header.frame_id = None
00126 
00127         for p in self.people.values():
00128             p.publish_markers(self.mpub)
00129             frame, person = p.get_person()
00130             pl.header.frame_id = frame
00131             pl.people.append(person)
00132 
00133         self.ppub.publish(pl)
00134 
00135 rospy.init_node("people_velocity_tracker")
00136 vt = VelocityTracker()
00137 vt.spin()


people_velocity_tracker
Author(s): David V. Lu!!
autogenerated on Thu Apr 13 2017 02:41:54