ros_node.py 文件源码

def handle_image_msg(msg):
    assert msg._type == 'sensor_msgs/Image'

    with g_fusion_lock:
        g_fusion.filter(EmptyObservation(msg.header.stamp.to_sec()))

        if g_fusion.last_state_mean is not None:
            pose = g_fusion.lidar_observation_function(g_fusion.last_state_mean)

            yaw_q = ros_tf.transformations.quaternion_from_euler(0, 0, pose[3])
            br = ros_tf.TransformBroadcaster()
            br.sendTransform(tuple(pose[:3]), tuple(yaw_q), rospy.Time.now(), 'obj_fuse_centroid', 'velodyne')

            if last_known_box_size is not None:
                # bounding box
                marker = Marker()
                marker.header.frame_id = "obj_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=0., g=1., b=0., a=0.5)
                marker.lifetime = rospy.Duration()
                pub = rospy.Publisher("obj_fuse_bbox", Marker, queue_size=10)
                pub.publish(marker)