Ejemplo n.º 1
0
def callback(msg):
    start = time.time()
    cov = np.array(covariance, dtype=np.float64).reshape(6, 6)
    test.merge(cloud(msg), transformation(tfl, "/map", msg.header), cov)
    poses.append([pose_stamped.header.stamp.to_time(),
                  pose_stamped.pose.position.x,
                  pose_stamped.pose.position.y])
    profile.append(time.time() - start)
    test.export8u('%s/atlaas.jpg'%robot_name)
Ejemplo n.º 2
0
def callback(msg):
    start = time.time()
    cov = np.array(covariance, dtype=np.float64).reshape(6, 6)
    test.merge(cloud(msg), transformation(tfl, "/map", msg.header), cov)
    poses.append([
        pose_stamped.header.stamp.to_time(), pose_stamped.pose.position.x,
        pose_stamped.pose.position.y
    ])
    profile.append(time.time() - start)
    test.export8u('%s/atlaas.jpg' % robot_name)
Ejemplo n.º 3
0
def callback(msg):
    global d_noise, poses
    start = time.time()
    T = transformation(tfl, "/map", msg.header)
    poses.append([time.time(), T[0:2, 3][0], T[0:2, 3][1]])
    d_noise += 0.02
    T[0:2, 3] += d_noise
    test.merge(cloud(msg), T, None)
    profile.append(time.time() - start)
    test.export8u("%s/atlaas.jpg" % robot_name)
Ejemplo n.º 4
0
def callback(msg):
    start = time.time()
    test.merge(cloud(msg), transformation(tfl, "/map", msg.header), None, False)
    profile.append(time.time() - start)
    test.export8u('%s/atlaas.jpg'%robot_name)
Ejemplo n.º 5
0
def callback(msg):
    start = time.time()
    test.merge(cloud(msg), transformation(tfl, "/map", msg.header), None,
               False)
    profile.append(time.time() - start)
    test.export8u('%s/atlaas.jpg' % robot_name)