예제 #1
0
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
예제 #2
0
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