class PersonEstimate(object):
def __init__(self, msg):
self.pos = msg
self.reliability = 0.1
self.k = Kalman()
def update(self, msg):
last = self.pos
self.pos = msg
self.reliability = max(self.reliability, msg.reliability)
ivel = subtract(self.pos.pos, last.pos)
time = (self.pos.header.stamp - last.header.stamp).to_sec()
if time == 0:
return
scale(ivel, 1.0 / time)
self.k.update([ivel.x, ivel.y, ivel.z])
def age(self):
return self.pos.header.stamp
def id(self):
return self.pos.object_id
def velocity(self):
k = self.k.values()
if k == None:
return Vector3()
v = Vector3(k[0], k[1], k[2])
return v
def publish_markers(self, pub):
gen.scale = [.1, .3, 0]
gen.color = [1, 1, 1, 1]
vel = self.velocity()
#scale(vel, 15)
m = gen.marker(points=[self.pos.pos, add(self.pos.pos, vel)])
m.header = self.pos.header
pub.publish(m)
def get_person(self):
p = Person()
p.name = self.id()
p.position = self.pos.pos
p.velocity = self.velocity()
p.reliability = self.reliability
p.tags.append(self.pos.name)
return self.pos.header.frame_id, p
class PersonEstimate:
def __init__(self, msg):
self.pos = msg
self.reliability = 0.1
self.k = Kalman()
def update(self, msg):
last = self.pos
self.pos = msg
self.reliability = max(self.reliability, msg.reliability)
ivel = subtract(self.pos.pos, last.pos)
time = (self.pos.header.stamp - last.header.stamp).to_sec()
scale(ivel, 1.0/time)
self.k.update([ivel.x, ivel.y, ivel.z])
def age(self):
return self.pos.header.stamp
def id(self):
return self.pos.object_id
def velocity(self):
k = self.k.values()
if k==None:
return Vector3()
v = Vector3(k[0],k[1],k[2])
return v
def publish_markers(self, pub):
gen.scale = [.1, .3, 0]
gen.color = [1,1,1,1]
vel = self.velocity()
#scale(vel, 15)
m = gen.marker(points=[self.pos.pos, add(self.pos.pos, vel)])
m.header = self.pos.header
pub.publish(m)
def get_person(self):
p = Person()
p.name = self.id()
p.position = self.pos.pos
p.velocity = self.velocity()
p.reliability = self.reliability
return self.pos.header.frame_id, p
class BoatEstimate(object):
def __init__(self, msg, stamp):
self.det_boat = msg
self.stamp = stamp
self.k = Kalman()
def update(self, msg, stamp):
last = self.det_boat
last_stamp = self.stamp
self.det_boat = msg
self.stamp = stamp
ivel = subtract(self.det_boat.pose.position, last.pose.position)
if ((self.stamp - last_stamp).to_sec() != 0.0):
time = (self.stamp - last_stamp).to_sec()
scale(ivel, 1.0 / time)
self.k.update([ivel.x, ivel.y, ivel.z])
def id(self):
return self.det_boat.id
def velocity(self):
k = self.k.values()
if k == None:
return Vector3()
v = Vector3(k[0], k[1], k[2])
return v
def get_boat(self):
b = Boat()
b.id = self.id()
b.lidar_index = self.det_boat.lidar_index
b.pose = self.det_boat.pose
b.velocity = self.velocity()
b.size = self.det_boat.size
return b
