ros，open3d单线激光雷达数据可视化

YJXI

已于 2024-06-26 08:55:45 修改

阅读量298

点赞数 2

文章标签： ros python

于 2024-06-25 18:10:43 首次发布

本文链接：https://blog.csdn.net/qq_38269799/article/details/139965892

版权

之前一直尝试将可视化写在ros的回调函数中，visualization一直会阻塞进程，换一种方式可以顺利实现可视化

import rospy
import open3d as o3d
import numpy as np
from sensor_msgs.msg import LaserScan

global scan_data
scan_data = None

def callback_laserscan(data):
    global scan_data
    scan_data = data

if __name__ == '__main__':
    rospy.init_node('laser_scan_reader')
    rospy.Subscriber('/scan', LaserScan, callback_laserscan)

    vis = o3d.visualization.Visualizer()
    vis.create_window()

    points3d = o3d.geometry.PointCloud()
    vis.add_geometry(points3d)

    to_reset = True
    rate = rospy.Rate(10)

    while not rospy.is_shutdown():
        print(scan_data)
        if scan_data is not None:
            ranges = scan_data.ranges
            angles = np.linspace(scan_data.angle_min, scan_data.angle_max, len(ranges))
            points = np.zeros((len(ranges), 3))
            points[:, 0] = ranges * np.cos(angles)
            points[:, 1] = ranges * np.sin(angles)

            points3d.points = o3d.utility.Vector3dVector(points)

            vis.update_geometry(points3d)

            if to_reset:
                vis.reset_view_point(True)
                to_reset = False

            vis.poll_events()
            vis.update_renderer()

        else:
            print("No laser scan data")
            rospy.sleep(2)

        rate.sleep()

    vis.destroy_window()