Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- import rclpy
- from rclpy.node import Node
- from sensor_msgs.msg import LaserScan
- import matplotlib.pyplot as plt
- import numpy as np
- class LidarPlotter(Node):
- def __init__(self):
- super().__init__('lidar_plotter')
- self.subscription = self.create_subscription(
- LaserScan,
- '/scan',
- self.listener_callback,
- 10)
- self.subscription # prevent unused variable warning
- def listener_callback(self, msg):
- angles = np.arange(msg.angle_min, msg.angle_max, msg.angle_increment)
- ranges = np.array(msg.ranges)
- x = ranges * np.cos(angles)
- y = ranges * np.sin(angles)
- plt.scatter(x, y, s=1)
- plt.xlabel('X [m]')
- plt.ylabel('Y [m]')
- plt.title('2D Lidar Point Cloud')
- plt.axis('equal')
- plt.grid(True)
- plt.show()
- def main(args=None):
- rclpy.init(args=args)
- lidar_plotter = LidarPlotter()
- rclpy.spin(lidar_plotter)
- lidar_plotter.destroy_node()
- rclpy.shutdown()
- if __name__ == '__main__':
- main()
Advertisement
Add Comment
Please, Sign In to add comment