00001
00002
00003 import roslib; roslib.load_manifest('people_velocity_tracker')
00004 import rospy, rosbag
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 *
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, (leg1.y+leg2.y)/2, (leg1.z+leg2.z)/2)
00018
00019 def add(v1, v2):
00020 return Vector3( v1.x + v2.x, v1.y + v2.y, v1.z + v2.z )
00021
00022 def subtract(v1, v2):
00023 return Vector3( v1.x - v2.x, v1.y - v2.y, v1.z - v2.z )
00024
00025 def scale(v, s):
00026 v.x *= s
00027 v.y *= s
00028 v.z *= s
00029
00030 def printv(v):
00031 print "%.2f %.2f %.2f"%(v.x, v.y, v.z),
00032
00033 gen = MarkerGenerator()
00034 gen.type = Marker.ARROW
00035 gen.ns = 'velocities'
00036 gen.lifetime = .5
00037
00038 class PersonEstimate:
00039 def __init__(self, msg):
00040 self.pos = msg
00041 self.reliability = 0.1
00042 self.k = Kalman()
00043
00044 def update(self, msg):
00045 last = self.pos
00046 self.pos = msg
00047 self.reliability = max(self.reliability, msg.reliability)
00048
00049 ivel = subtract(self.pos.pos, last.pos)
00050 time = (self.pos.header.stamp - last.header.stamp).to_sec()
00051 scale(ivel, 1.0/time)
00052
00053 self.k.update([ivel.x, ivel.y, ivel.z])
00054
00055 def age(self):
00056 return self.pos.header.stamp
00057
00058 def id(self):
00059 return self.pos.object_id
00060
00061 def velocity(self):
00062 k = self.k.values()
00063 if k==None:
00064 return Vector3()
00065 v = Vector3(k[0],k[1],k[2])
00066 return v
00067
00068 def publish_markers(self, pub):
00069 gen.scale = [.1, .3, 0]
00070 gen.color = [1,1,1,1]
00071 vel = self.velocity()
00072
00073 m = gen.marker(points=[self.pos.pos, add(self.pos.pos, vel)])
00074 m.header = self.pos.header
00075 pub.publish(m)
00076
00077 def get_person(self):
00078 p = Person()
00079 p.name = self.id()
00080 p.position = self.pos.pos
00081 p.velocity = self.velocity()
00082 p.reliability = self.reliability
00083 return self.pos.header.frame_id, p
00084
00085 class VelocityTracker:
00086 def __init__(self):
00087 self.people = {}
00088 self.TIMEOUT = rospy.Duration( rospy.get_param('~timeout', 1.0) )
00089 self.sub = rospy.Subscriber('/people_tracker_measurements', PositionMeasurementArray, self.pm_cb)
00090 self.mpub = rospy.Publisher('/visualization_marker', Marker)
00091 self.ppub = rospy.Publisher('/people', People)
00092
00093 def pm_cb(self, msg):
00094 for pm in msg.people:
00095 if pm.object_id in self.people:
00096 self.people[pm.object_id].update(pm)
00097 else:
00098 p = PersonEstimate(pm)
00099 self.people[pm.object_id] = p
00100
00101 def spin(self):
00102 rate = rospy.Rate(10)
00103 while not rospy.is_shutdown():
00104
00105 now = rospy.Time.now()
00106 for p in self.people.values():
00107 if now - p.age() > self.TIMEOUT:
00108 del self.people[p.id()]
00109 self.publish()
00110 rate.sleep()
00111
00112 def publish(self):
00113 gen.counter = 0
00114 pl = People()
00115 pl.header.frame_id = None
00116
00117 for p in self.people.values():
00118 p.publish_markers(self.mpub)
00119 frame, person = p.get_person()
00120 pl.header.frame_id = frame
00121 pl.people.append( person )
00122
00123 self.ppub.publish(pl)
00124
00125 rospy.init_node("people_velocity_tracker")
00126 vt = VelocityTracker()
00127 vt.spin()