使用python-open3d给rosbag点云加载包围盒框(2)

import rospy
import open3d as o3d
import numpy as np
from sensor_msgs.msg import PointCloud2
from sensor_msgs import point_cloud2

global d
d = None

def callback_pcl(data):
    global d
    d=data

def Point_cloud():
    global d
    ss=point_cloud2.read_points(d, field_names=("x", "y", "z"), skip_nans=True)
    ss=list(ss)
    points=np.array(ss)
    return points
     
if __name__ == '__main__':
    rospy.init_node('points',anonymous=False)
    rospy.Subscriber('/rslidar_points', PointCloud2, callback_pcl)
    
    vis = o3d.visualization.Visualizer()
    vis.create_window()
    points3d = o3d.geometry.PointCloud()
    
    #初始化包围盒
    x=o3d.geometry.AxisAlignedBoundingBox()
    x.color = (1,0,0)
    
    render_option = vis.get_render_option()
    render_option.point_size = 1
    render_option.background_color = np.asarray([0, 0, 0])
    vis.add_geometry(points3d)
    
    #加载包围盒框到窗口里
    vis.add_geometry(x)
    
    to_reset = True
    
    rate = rospy.Rate(10)
    
    while not rospy.is_shutdown():
    #订阅的话题是否有数据
        if not d is None:
            points=Point_cloud()
            points3d.points = o3d.utility.Vector3dVector(points)
            vis.update_geometry(points3d)
            
            #aabb方式获取点云包围盒并赋值给x
            aa1=points3d.get_axis_aligned_bounding_box()
            x.max_bound=aa1.max_bound
            x.min_bound=aa1.min_bound
            #更新x包围盒框
            vis.update_geometry(x)
            
            if to_reset:
                vis.reset_view_point(True)
                to_reset = False
            vis.poll_events()
            vis.update_renderer()
            
        else:
            print("not points_data")
            time.sleep(2)
        rate.sleep()