diff --git a/examples/unity_lidar/custom.json b/examples/unity_lidar/custom.json new file mode 100644 index 0000000..5ab3223 --- /dev/null +++ b/examples/unity_lidar/custom.json @@ -0,0 +1,20 @@ +{ + "robots": [ + { + "name": "LiDAR2D", + "rpc_pdu_readers": [], + "rpc_pdu_writers": [], + "shm_pdu_readers": [], + "shm_pdu_writers": [ + { + "type": "sensor_msgs/LaserScan", + "org_name": "scan", + "name": "LiDAR2D_scan", + "channel_id": 0, + "pdu_size": 6088, + "write_cycle": 5 + } + ] + } + ] +} \ No newline at end of file diff --git a/examples/unity_lidar/lidar.py b/examples/unity_lidar/lidar.py new file mode 100644 index 0000000..0043762 --- /dev/null +++ b/examples/unity_lidar/lidar.py @@ -0,0 +1,118 @@ +import sys +import threading +import numpy as np +from queue import Queue +from PyQt5.QtWidgets import QApplication, QMainWindow, QVBoxLayout, QWidget +from PyQt5.QtCore import QTimer +from pyqtgraph import PlotWidget, ScatterPlotItem +import pyqtgraph as pg +import hakopy +import hako_pdu +import time + +# データ用キュー +data_queue = Queue(maxsize=1) +pdu_manager = None + +class LidarVisualizer(QMainWindow): + def __init__(self): + super().__init__() + + self.setWindowTitle("Real-time LiDAR Data") + + # Create a widget for the main window + self.main_widget = QWidget() + self.setCentralWidget(self.main_widget) + + # Create a layout for the widget + layout = QVBoxLayout() + self.main_widget.setLayout(layout) + + # Create a plot widget + self.plot_widget = PlotWidget() + self.scatter = pg.ScatterPlotItem() + self.plot_widget.addItem(self.scatter) + self.plot_widget.setXRange(-0.1, 6) + self.plot_widget.setYRange(-2, 2) + self.plot_widget.setLabel('left', "Y (meters)") + self.plot_widget.setLabel('bottom', "X (meters)") + self.plot_widget.setTitle("Real-time LiDAR Data") + layout.addWidget(self.plot_widget) + + # Timer to update the plot + self.timer = QTimer() + self.timer.timeout.connect(self.update_plot) + self.timer.start(100) # Update every 100ms + + def update_plot(self): + global data_queue + do_read_lidar_scan() + if not data_queue.empty(): + # キューからデータを取得 + data = data_queue.get() + ranges_data = data['ranges'] + angle_min = data['angle_min'] + angle_max = data['angle_max'] + + if len(ranges_data) > 0: + angles = np.linspace(angle_min, angle_max, len(ranges_data)) + angles_rad = np.deg2rad(angles) + x = np.array(ranges_data) * np.cos(angles_rad) + y = np.array(ranges_data) * np.sin(angles_rad) + self.scatter.setData(x, y, size=2) # 点の大きさを調整 + +def do_read_lidar_scan(): + if hakopy.simulation_time() <= 1000000: + return + global pdu_manager, data_queue + pdu = pdu_manager.get_pdu("LiDAR2D", 0) + scan = pdu.read() + if scan: + # データの検証を追加 + ranges = scan.get('ranges') + #print(f'ranges: {ranges}') + angle_min_raw = scan.get('angle_min') + angle_max_raw = scan.get('angle_max') + + if ranges is None or angle_min_raw is None or angle_max_raw is None: + print("LiDAR data is incomplete or missing.") + return 0 + + if data_queue.full(): + data_queue.get() # 最古のデータを削除 + data_queue.put({ + 'ranges': ranges, + 'angle_min': np.rad2deg(angle_min_raw), + 'angle_max': np.rad2deg(angle_max_raw) + }) + else: + print("LiDAR data not available.") + return 0 + + +def main(): + if len(sys.argv) != 2: + print(f"Usage: {sys.argv[0]} ") + return 1 + global pdu_manager + asset_name = 'LidarPython' + config_path = sys.argv[1] + delta_time_usec = 20000 + + pdu_manager = hako_pdu.HakoPduManager('/usr/local/lib/hakoniwa/hako_binary/offset', config_path) + hakopy.conductor_start(delta_time_usec, delta_time_usec) + + ret = hakopy.init_for_external() + if not ret: + print(f"ERROR: init_for_external() returns {ret}.") + return 1 + + app = QApplication(sys.argv) + visualizer = LidarVisualizer() + visualizer.show() + sys.exit(app.exec_()) + hakopy.conductor_stop() + return 0 + +if __name__ == "__main__": + main()