lemueltra

ros2laserscan

Jul 17th, 2024
201
0
Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 1.09 KB | None | 0 0
  1. import rclpy
  2. from rclpy.node import Node
  3. from sensor_msgs.msg import LaserScan
  4. import matplotlib.pyplot as plt
  5. import numpy as np
  6.  
  7. class LidarPlotter(Node):
  8. def __init__(self):
  9. super().__init__('lidar_plotter')
  10. self.subscription = self.create_subscription(
  11. LaserScan,
  12. '/scan',
  13. self.listener_callback,
  14. 10)
  15. self.subscription # prevent unused variable warning
  16.  
  17. def listener_callback(self, msg):
  18. angles = np.arange(msg.angle_min, msg.angle_max, msg.angle_increment)
  19. ranges = np.array(msg.ranges)
  20. x = ranges * np.cos(angles)
  21. y = ranges * np.sin(angles)
  22.  
  23. plt.scatter(x, y, s=1)
  24. plt.xlabel('X [m]')
  25. plt.ylabel('Y [m]')
  26. plt.title('2D Lidar Point Cloud')
  27. plt.axis('equal')
  28. plt.grid(True)
  29. plt.show()
  30.  
  31. def main(args=None):
  32. rclpy.init(args=args)
  33. lidar_plotter = LidarPlotter()
  34. rclpy.spin(lidar_plotter)
  35. lidar_plotter.destroy_node()
  36. rclpy.shutdown()
  37.  
  38. if __name__ == '__main__':
  39. main()
  40.  
Advertisement
Add Comment
Please, Sign In to add comment