ros_node.py 文件源码

def handle_radar_msg(msg, dont_fuse):
    assert msg._md5sum == '6a2de2f790cb8bb0e149d45d297462f8'

    publish_rviz_topics = True

    with g_fusion_lock:
        # do we have any estimation?
        if g_fusion.last_state_mean is not None:
            pose = g_fusion.lidar_observation_function(g_fusion.last_state_mean)

            observations = RadarObservation.from_msg(msg, RADAR_TO_LIDAR, 0.9115)

            # find nearest observation to current object position estimation
            distance_threshold = 4.4
            nearest = None
            nearest_dist = 1e9
            for o in observations:
                dist = [o.x - pose[0], o.y - pose[1], o.z - pose[2]]
                dist = np.sqrt(np.array(dist).dot(dist))

                if dist < nearest_dist and dist < distance_threshold:
                    nearest_dist = dist
                    nearest = o

            if nearest is not None:
                # FUSION
                if not dont_fuse:
                    g_fusion.filter(nearest)

                if publish_rviz_topics:
                    header = Header()
                    header.frame_id = '/velodyne'
                    header.stamp = rospy.Time.now()
                    point = np.array([[nearest.x, nearest.y, nearest.z]], dtype=np.float32)
                    rospy.Publisher(name='obj_radar_nearest',
                      data_class=PointCloud2,
                      queue_size=1).publish(pc2.create_cloud_xyz32(header, point))

                    #pose = g_fusion.lidar_observation_function(g_fusion.last_state_mean)

                    #br = ros_tf.TransformBroadcaster()
                    #br.sendTransform(tuple(pose), (0,0,0,1), rospy.Time.now(), 'car_fuse_centroid', 'velodyne')

#                     if last_known_box_size is not None:
#                         # bounding box
#                         marker = Marker()
#                         marker.header.frame_id = "car_fuse_centroid"
#                         marker.header.stamp = rospy.Time.now()
#                         marker.type = Marker.CUBE
#                         marker.action = Marker.ADD
#                         marker.scale.x = last_known_box_size[2]
#                         marker.scale.y = last_known_box_size[1]
#                         marker.scale.z = last_known_box_size[0]
#                         marker.color = ColorRGBA(r=1., g=1., b=0., a=0.5)
#                         marker.lifetime = rospy.Duration()
#                         pub = rospy.Publisher("car_fuse_bbox", Marker, queue_size=10)
#                         pub.publish(marker)