YDLIDAR X4 Proを使ってみる　その２（ミニPC、Linuxでの動作確認）

git clone https://github.com/YDLIDAR/YDLidar-SDK.git
cd YDLidar-SDK
mkdir build
cd build
cmake ..
make
sudo make install

sudo apt install cmake
sudo apt install make
sudo apt install build-essential
sudo apt install swig python3-dev python3-pip

cd
cd YDLidar-SDK/
pip install .
python3 setup.py build
sudo python3 setup.py install

[error] Error, cannot bind to the specified [serial port:/dev/ttyUSB0] and [baudrate:512000]
[error] Error initializing YDLIDAR check Comms.

ls /dev/ttyUSB*

pip install numpy
pip install matplotlib
python3 plot_tof_test.py

# --- 修正前 ---
# laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 512000)
# laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TOF);
# laser.setlidaropt(ydlidar.LidarPropSampleRate, 20);
# laser.setlidaropt(ydlidar.LidarPropSingleChannel, False);

# --- 修正後 ---
laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 128000)
laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TRIANGLE)
laser.setlidaropt(ydlidar.LidarPropSampleRate, 5)
laser.setlidaropt(ydlidar.LidarPropSingleChannel, True)

import os
import ydlidar
import time
import sys
from matplotlib.patches import Arc
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import numpy as np

RMAX = 32.0


fig = plt.figure()
fig.canvas.manager.set_window_title('YDLidar LIDAR Monitor')
lidar_polar = plt.subplot(polar=True)
lidar_polar.autoscale_view(True,True,True)
lidar_polar.set_rmax(RMAX)
lidar_polar.grid(True)
ports = ydlidar.lidarPortList();
port = "/dev/ydlidar";
for key, value in ports.items():
    port = value;
    
laser = ydlidar.CYdLidar();
laser.setlidaropt(ydlidar.LidarPropSerialPort, port);
laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 128000)
laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TRIANGLE);
laser.setlidaropt(ydlidar.LidarPropDeviceType, ydlidar.YDLIDAR_TYPE_SERIAL);
laser.setlidaropt(ydlidar.LidarPropScanFrequency, 10.0);
laser.setlidaropt(ydlidar.LidarPropSampleRate, 5);
laser.setlidaropt(ydlidar.LidarPropSingleChannel, True);
laser.setlidaropt(ydlidar.LidarPropMaxAngle, 180.0);
laser.setlidaropt(ydlidar.LidarPropMinAngle, -180.0);
laser.setlidaropt(ydlidar.LidarPropMaxRange, 32.0);
laser.setlidaropt(ydlidar.LidarPropMinRange, 0.01);
scan = ydlidar.LaserScan()

def animate(num):
    
    r = laser.doProcessSimple(scan);
    if r:
        angle = []
        ran = []
        intensity = []
        for point in scan.points:
            angle.append(point.angle);
            ran.append(point.range);
            intensity.append(point.intensity);
        lidar_polar.clear()
        lidar_polar.scatter(angle, ran, c=intensity, cmap='hsv', alpha=0.95)

ret = laser.initialize();
if ret:
    ret = laser.turnOn();
    if ret:
        ani = animation.FuncAnimation(fig, animate, interval=50)
        plt.show()
    laser.turnOff();
laser.disconnecting();
plt.close();

import os
import ydlidar
import time
import sys
import socket
import json
import matplotlib.pyplot as plt
import matplotlib.animation as animation
import numpy as np

# --- 通信設定 (受信するPCのIPに合わせて変更してください) ---
UDP_IP = "localhost"  # 例: 受信側のPCのIPアドレス
UDP_PORT = 5005          # 受信側のポート番号

# --- LiDAR設定 ---
RMAX = 12.0              # 表示する最大距離 (m) X4 Pro/S2 Proに合わせて調整
SCAN_FREQ = 10.0         # スキャン周波数 (Hz) : 高速化
SAMPLE_RATE = 5          # サンプルレート (K)

# ソケットの準備
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)

# グラフの初期設定
fig = plt.figure(figsize=(8, 8))
fig.canvas.manager.set_window_title('YDLidar X4/S2 Pro Monitor & UDP Sender')
lidar_polar = plt.subplot(polar=True)
lidar_polar.autoscale_view(True, True, True)
lidar_polar.set_rmax(RMAX)
lidar_polar.grid(True)

# LiDARポートの検索と設定
ports = ydlidar.lidarPortList()
port = "/dev/ttyUSB0"

laser = ydlidar.CYdLidar()
laser.setlidaropt(ydlidar.LidarPropSerialPort, port)
laser.setlidaropt(ydlidar.LidarPropSerialBaudrate, 128000)
laser.setlidaropt(ydlidar.LidarPropLidarType, ydlidar.TYPE_TRIANGLE)
laser.setlidaropt(ydlidar.LidarPropDeviceType, ydlidar.YDLIDAR_TYPE_SERIAL)

# --- スペックに合わせた高速化設定 ---
laser.setlidaropt(ydlidar.LidarPropScanFrequency, SCAN_FREQ)
laser.setlidaropt(ydlidar.LidarPropSampleRate, SAMPLE_RATE)
laser.setlidaropt(ydlidar.LidarPropSingleChannel, True) # 重要
laser.setlidaropt(ydlidar.LidarPropMaxAngle, 180.0)
laser.setlidaropt(ydlidar.LidarPropMinAngle, -180.0)
laser.setlidaropt(ydlidar.LidarPropMaxRange, 16.0)
laser.setlidaropt(ydlidar.LidarPropMinRange, 0.05)

scan = ydlidar.LaserScan()

def animate(num):
    r = laser.doProcessSimple(scan)
    if r:
        angle = []
        ran = []
        intensity = []
        
        # 送信用データリスト (JSON用)
        # データ量を減らすため、距離が0の点は除外しても良い
        export_points = []

        for point in scan.points:
            # プロット用データ
            angle.append(point.angle)
            ran.append(point.range)
            intensity.append(point.intensity)
            
            # UDP送信用データの作成
            # フォーマット: [角度(rad), 距離(m), 強度]
            # 通信量削減のため小数点以下3桁に丸める
            if point.range > 0:
                export_points.append([
                    round(point.angle, 3),
                    round(point.range, 3),
                    int(point.intensity)
                ])

        # --- 1. プロットの更新 ---
        lidar_polar.clear()
        lidar_polar.set_rmax(RMAX)
        lidar_polar.grid(True)
        # s=2.0 で点を小さくし、高密度でも見やすくする
        lidar_polar.scatter(angle, ran, c=intensity, cmap='hsv', s=2.0, alpha=0.8)

        # --- 2. UDP送信処理 ---
        try:
            # タイムスタンプと点群データをJSON化
            payload = {
                "t": time.time(),     # タイムスタンプ
                "c": len(export_points), # 点の数
                "d": export_points    # データ本体 [[ang, dist, int], ...]
            }
            json_data = json.dumps(payload).encode('utf-8')
            
            # データサイズが大きすぎる場合は警告 (UDPパケット制限への配慮)
            if len(json_data) > 60000:
                print(f"Warning: Payload size {len(json_data)} bytes is too large for UDP.")
            else:
                sock.sendto(json_data, (UDP_IP, UDP_PORT))
                
        except Exception as e:
            print(f"UDP Send Error: {e}")

ret = laser.initialize()
if ret:
    ret = laser.turnOn()
    if ret:
        # intervalを短くして描画更新を速くする
        ani = animation.FuncAnimation(fig, animate, interval=10)
        plt.show()
    laser.turnOff()
laser.disconnecting()
plt.close()

{
  "t": 1707103500.123,      // タイムスタンプ (UNIX time)
  "c": 405,                 // 点群の数 (Count)
  "d": [                    // データ配列 (Data)
    [ -3.141, 2.501, 10 ],  // [ 角度(ラジアン), 距離(メートル), 強度 ]
    [ -3.120, 2.505, 12 ],
    ...
  ]
}