Python激光雷达点阵 TOFSense-M S激光测距传感器 - MKT-porter

https://support.nooploop.com/cn/tofsense/protocol

https://item.taobao.com/item.htm?_u=q1qf7bf5e103&id=661290637611&pisk=gueLOnY6ZNbHEoeKjk5MZ7psxASGj1qUbyrXE40HNPUTyoK3tb0oNYU4kb9lTvc-ePggK2qnd0G7rrNnRW0ke8UaJpvodzX-PoPN42m3K3n71yputk0363HUIWvo-wk8VrDRoZXcnkrEaY_cop1vk_kjD0TBr2i_fYm8skOHckrEU41eLo1YY3HBYG6IV4stC0n2RYGIAFgs70mWPzgI1Fix0YM7P2GsfmmIV0GIFhstDmmIVBMW1VijmDiQPYst1DusPYGQPlMEQa36A4v8g4-mzF83pL97BDh1skgs1fSoAXp_vV95_rnKuqZIWL9SkDl-GlEhJKriLlaxmyX6prE4QPn_lTQtUPP7P0U13UDUiyyq6l5XClDKSjibCg9jrJFgcVHvV9Ztp2hbKXBXFrZTVScTIi6qpvgQZvrkDwqTpyqUBuxW1vH36jeKht8Ir5qYe0ePzOUY0Rztfr6RRgWun-pyWE0xqBsOXQRrOc7Ao0PFyYwXTc3cvTdyafSwEBecuQRr9fitoGmHaQlGa&spm=a1z09.2.0.0.4f9e2e8do3lrCy&sku_properties=1627207%3A29599107454

import serial
import serial.tools.list_ports




_TOFSense_Frame_Header = b"\x57\x00"
_TOFSense_M_Frame_Header = b"\x57\x01"

def verify_checksum(data):
    """校验和"""
    checksum = sum(data[:-1]) & 0xFF
    return checksum == data[-1]

def check_header(data, header):
    """校验帧头"""
    return data[0] == header[0] and data[1] == header[1]

def nByteUnpack(data, index, byte_num, signed=False):
    """
    返回单个解析数据
    Args:
        data:原数据 bytes
        index:起始位置 int
        byte_num:单个字节数 int
        signed:是否为有符号数 bool
    """
    return (
        int.from_bytes(data[index : index + byte_num], byteorder="little", signed=signed),
        index + byte_num,
    )

def TOFSenseFrame0Unpack(data):
    """TOFSense/TOFSense-F系列帧解析"""
    if not check_header(data, _TOFSense_Frame_Header) or not verify_checksum(data):
        return None
    index = 3
    parsed_data = {}
    parsed_data["id"], index = nByteUnpack(data, index, 1)
    parsed_data["system_time"], index = nByteUnpack(data, index, 4)
    dis, index = nByteUnpack(data, index, 3)
    parsed_data["dis"] = dis / 1000.0
    parsed_data["dis_status"], index = nByteUnpack(data, index, 1)
    parsed_data["signal_strength"], index = nByteUnpack(data, index, 2)
    return parsed_data

def TOFSenseMFrame0Unpack(data):
    """TOFSense-M系列帧解析"""
    if not check_header(data, _TOFSense_M_Frame_Header) or not verify_checksum(data):
        return None
    index = 3
    parsed_data = {}
    parsed_data["id"], index = nByteUnpack(data, index, 1)
    parsed_data["system_time"], index = nByteUnpack(data, index, 4)
    parsed_data["zone_map"], index = nByteUnpack(data, index, 1)
    parsed_data["dis"] = []
    parsed_data["dis_status"] = []
    parsed_data["signal_strength"] = []
    for i in range(parsed_data["zone_map"]):
        dis, index = nByteUnpack(data, index, 3)
        status, index = nByteUnpack(data, index, 1)
        signal, index = nByteUnpack(data, index, 2)
        parsed_data["dis"].append(dis / 1000.0 / 1000.0)
        parsed_data["dis_status"].append(status)
        parsed_data["signal_strength"].append(signal)
    return parsed_data



#TOFSense-M/MS使用示例
import serial
_Frame_Header = b"\x57"  # 帧头0x57
_Frame_Mark = b"\x01"  # 帧关键字0x00
_One_Frmae_length = 400  # 一帧字节数

PORT = "COM26"  # 用户TOF所连接的串口
BAUD = 921600  # 波特率

def open_first_serial(baudrate=BAUD, timeout=1):
    ports = list(serial.tools.list_ports.comports())
    if not ports:
        print("未找到串口设备")
        return None

    first_port = ports[0].device
    try:
        first_port="COM110"
        ser = serial.Serial(port=first_port, baudrate=baudrate, timeout=timeout)
        print(f"已打开串口: {first_port}")
        return ser
    except serial.SerialException as e:
        print(f"无法打开串口 {first_port}: {e}")
        return None

def check_person_within_range(dis_list, threshold=1.0):
    """
    根据测距数据判断前方是否有人（距离小于阈值）。
    
    Args:
        dis_list (list): 激光雷达距离数组（单位：米）。
        threshold (float): 判断距离阈值，默认1.0米。
    
    Returns:
        int: 有人返回1，无人返回0。
    """
    for d in dis_list:
        if 0 < d < threshold:
            return 1
    return 0

def check_person_center_block(dis_list, threshold=1.0):
    """
    判断中间3x3区域是否全部小于阈值。
    
    Args:
        dis_list (list): 距离数组（64个，8x8）。
        threshold (float): 距离阈值（米）。
    
    Returns:
        int: 满足条件返回1，否则返回0。
    """
    if len(dis_list) != 64:
        print("数据长度不是64，判定失败")
        return 0

    # 转换为8x8二维数组
    dis_matrix = np.array(dis_list).reshape((8, 8))

    # 中心区域：第3~5行、第3~5列（索引2~4）
    center_block = dis_matrix[2:5, 2:5]

    # 检查是否所有值都小于阈值且非0（0通常表示无效数据）
    if np.all((center_block > 0) & (center_block < threshold)):
        return 1
    else:
        return 0


# import matplotlib.pyplot as plt
# from mpl_toolkits.mplot3d import Axes3D
# import numpy as np

# def visualize_dis_3d(dis_list):
#     """
#     将 64 个激光雷达距离值绘制成 3D 表面图。
#     Args:
#         dis_list (list): 距离值数组，长度为64（8x8）。
#     """
#     if len(dis_list) != 64:
#         print("数据长度不是 64，无法绘图")
#         return

#     # 将一维列表转为 8x8 二维数组
#     Z = np.array(dis_list).reshape((8, 8))

#     # 构建 X 和 Y 网格
#     X, Y = np.meshgrid(np.arange(8), np.arange(8))

#     # 创建图形窗口
#     fig = plt.figure()
#     ax = fig.add_subplot(111, projection='3d')

#     # 绘制表面图
#     surf = ax.plot_surface(X, Y, Z, cmap='viridis')

#     # 添加颜色条
#     fig.colorbar(surf, shrink=0.5, aspect=10)

#     # 设置标签
#     ax.set_xlabel('X区块')
#     ax.set_ylabel('Y区块')
#     ax.set_zlabel('距离（米）')
#     ax.set_title('TOFSense 距离3D可视化')

#     plt.show()




if __name__ == "__main__":
    try:
        ser = open_first_serial()
    except Exception as e:
        ser = None
        print(f"串口初始化失败: {e}")

    if ser:
        try:
            while True:
                buffer = bytearray()
                data = ser.read(1)
                if data == _Frame_Header:  # 确认帧头
                    buffer.extend(data)
                    data = ser.read(1)
                    if data == _Frame_Mark:  # 确认关键字
                        buffer.extend(data)
                        buffer.extend(ser.read(_One_Frmae_length - 2))
                        tof_data = TOFSenseMFrame0Unpack(buffer)

                         
                         
                        if tof_data:
                            print(tof_data)
                            #flag = check_person_within_range(tof_data["dis"])# 
                            flag = check_person_center_block(tof_data["dis"])

                            print("是否有人：", flag)
                            #visualize_dis_3d(tof_data["dis"])
        except KeyboardInterrupt:
            print("\n程序已手动中断 (Ctrl+C)")
        except Exception as e:
            print(f"\n运行中发生错误: {e}")
        finally:
            ser.close()
            print("串口已关闭")



# 输出：                       
# {'id': 0, 'system_time': 11366, 'zone_map': 64,
#  'dis': [2.154, 2.13243, 2.11173, 2.127596, 2.1398960000000002, 2.1082959999999997, 2.1094299999999997, 2.1198629999999996, 2.172, 2.116596, 2.124196, 2.143163, 2.1267300000000002, 2.107463, 2.123, 2.11543, 2.138163, 2.1318629999999996, 2.141896, 2.139596, 2.11843, 2.13943, 2.124863, 2.11843, 2.129596, 2.149863, 2.16833, 2.1600300000000003, 2.160063, 2.137896, 2.14, 2.11373, 2.15773, 2.126596, 2.1598629999999996, 2.1494630000000003, 2.16493, 2.1447629999999998, 2.13773, 2.10873, 2.1372959999999996, 2.15973, 2.160596, 2.16473, 2.1584630000000002, 2.1400300000000003, 2.11803, 2.11573, 2.1734299999999998, 2.139463, 2.158196, 2.166163, 2.11973, 2.1347300000000002, 2.128163, 2.1134630000000003, 2.167, 2.1527629999999998, 2.133163, 2.1372959999999996, 2.141896, 2.1318960000000002, 2.103863, 2.1174630000000003],
#  'dis_status': [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0],
#  'signal_strength': [20, 21, 24, 26, 29, 23, 21, 22, 20, 26, 32, 25, 24, 28, 19, 21, 25, 22, 29, 26, 21, 21, 22, 21, 26, 22, 30, 27, 34, 29, 16, 24, 24, 26, 22, 28, 36, 31, 24, 24, 23, 24, 26, 24, 28, 27, 27, 24, 21, 28, 32, 25, 24, 24, 25, 28, 20, 31, 25, 23, 29, 29, 22, 28]}

posted on 2025-04-26 05:49 MKT-porter 阅读(37) 评论(0) 收藏举报

刷新页面返回顶部