radar_test.py 文件源码

def process_radar_tracks(msg):
    assert msg._md5sum == '6a2de2f790cb8bb0e149d45d297462f8'

    tracks = RadarObservation.from_msg(msg)

    num_tracks = len(tracks)

    acc=[]
    cloud = np.zeros([num_tracks, 3], dtype=np.float32)
    for i, track in enumerate(tracks):
        cloud[i] = [track.x, track.y, track.z] - np.array(RADAR_TO_LIDAR)

        if np.abs(track.y) < 2:
            #acc.append(track.x)
            #acc.append(track.power)
            print track.vx*3.7, track.vy*3.7
            #print x, y, z

    #print acc #msg.header.stamp

    header = Header()
    header.stamp = msg.header.stamp
    header.frame_id = 'velodyne'
    cloud_msg = pc2.create_cloud_xyz32(header, cloud)
    cloud_msg.width = 1
    cloud_msg.height = num_tracks
    rospy.Publisher('radar_points', PointCloud2, queue_size=1).publish(cloud_msg)