LAS/mqtt/mqtt_recv.py

'''
处理mqtt接收的消息
'''

import paho.mqtt.client as mqtt
import queue
import json
import traceback
import re

import common.sys_comm as sys_comm
from common.sys_comm import LOGDBG, LOGINFO, LOGWARN, LOGERR
from common.sys_comm import get_tracker_targets, get_utc_time_ms, get_utc_time_s
from common.sys_comm import POSE_E, DEV_EC
from common.sys_comm import g_sys_conf, g_sys_conf_mtx

import db.db_process as db_process
from db.db_process import db_req_que
from db.db_process import DBRequest

from mqtt.mqtt_topics import Topic_Pattern
import mqtt.mqtt_send as mqtt_send

import device.dev_mng as dev_mng
from device.dev_mng import g_dev_map, g_dev_map_lock, Device



# 数据队列
raw_points_que_forpost = queue.Queue()      # 实时姿态队列
'''
{
    "dev_id": "xxxxxx",
    "raw_points": [...]
}
'''

# 检查topic
def check_topic(topic:str, pattern:str) -> bool:
    topic = topic.strip()
    return bool(re.match(pattern, topic))


"""
{
    "timestamp": 1727323744093,
    "pose": 2,
    "target_point": [
        [
            0.15537149991307939,
            -0.17245136840002878,
            0.5702038151877267,
            1
        ],
        [
            0.15537149991307939,
            -0.17245136840002878,
            0.5702038151877267,
            2
        ]
    ]
}
"""
# 处理来自设备的点云消息： /dev/{device_id}/dsp_data
def deal_dsp_data(msg:mqtt.MQTTMessage):
    try:
        parts = msg.topic.split('/')
        dev_id = parts[2]

        # 未注册的设备，不处理
        with g_dev_map_lock:
            if dev_id not in g_dev_map:
                return
            device:Device = g_dev_map[dev_id]

        payload = json.loads(msg.payload.decode('utf-8'))

        # 处理 target
        if ("tracker_targets" in payload):
            tracker_targets = payload["tracker_targets"]

            timestamp = get_utc_time_s()
            pose = POSE_E.POSE_4.value
            rtd_unit = {
                "timestamp": timestamp,
                "pose": pose,
                "target_point": tracker_targets
            }
            device.put_rtd_unit(rtd_unit)

    except json.JSONDecodeError as e:
        tb_info = traceback.extract_tb(e.__traceback__)
        for frame in tb_info:
            LOGERR(f"[{frame.filename}:{frame.lineno}] @{frame.name}(), error:{e}, {e.doc}")
    except Exception as e:
        tb_info = traceback.extract_tb(e.__traceback__)
        for frame in tb_info:
            LOGERR(f"[{frame.filename}:{frame.lineno}] @{frame.name}(), error: {e}")


# 实时位置姿态
def deal_realtime_pos(msg:mqtt.MQTTMessage):
    try:
        payload = json.loads(msg.payload.decode('utf-8'))
        dev_id   = payload.get("dev_id")
        # 未注册的设备，不处理
        with g_dev_map_lock:
            if dev_id not in g_dev_map:
                return
            device:Device = g_dev_map[dev_id]

        timestamp   = payload.get("timestamp")
        pose        = payload.get("pose")
        target_point    = payload.get("target_point", [])

        rtd_unit = {
            "timestamp": timestamp,
            "pose": pose,
            "target_point": target_point
        }
        device.put_rtd_unit(rtd_unit)

        return

    except json.JSONDecodeError as e:
        tb_info = traceback.extract_tb(e.__traceback__)
        for frame in tb_info:
            LOGERR(f"[{frame.filename}:{frame.lineno}] @{frame.name}(), error:{e}, {e.doc}")
    except Exception as e:
        tb_info = traceback.extract_tb(e.__traceback__)
        for frame in tb_info:
            LOGERR(f"[{frame.filename}:{frame.lineno}] @{frame.name}(), error: {e}")


# 设备消息分发处理：/dev/#
def deal_dev_msg(client:mqtt.Client, userdaata, msg:mqtt.MQTTMessage):
    try:
        topic = msg.topic
        parts = msg.topic.split('/')
        dev_id = parts[2]

        # 设备信息（注册）
        if (check_topic(topic,Topic_Pattern.dev_login)):
            return
            deal_dev_login(msg)

        # 设备心跳保活
        elif (check_topic(topic,Topic_Pattern.dev_keepalive)):
            return
            deal_dev_keepalive(msg)

        # 设备配置信息
        elif (check_topic(topic,Topic_Pattern.dev_rep_dev_param)):
            return
            deal_report_device_param(msg)

        # 设备实时数据
        elif (check_topic(topic,Topic_Pattern.dev_dsp_data)):
            deal_dsp_data(msg)

        # 点云数据
        elif (check_topic(topic,Topic_Pattern.dev_cloudpoint)):
            return
            deal_cloudpoint(msg)

    except json.JSONDecodeError as e:
        tb_info = traceback.extract_tb(e.__traceback__)
        for frame in tb_info:
            LOGERR(f"[{frame.filename}:{frame.lineno}] @{frame.name}(), error:{e}, {e.doc}")
    except Exception as e:
        tb_info = traceback.extract_tb(e.__traceback__)
        for frame in tb_info:
            LOGERR(f"[{frame.filename}:{frame.lineno}] @{frame.name}(), error: {e}")


# 设备代理分发处理：/das/#
def deal_das_msg(client:mqtt.Client, userdaata, msg:mqtt.MQTTMessage):
    try:
        topic = msg.topic
        parts = msg.topic.split('/')
        dev_id = parts[2]
        # if dev_id != "00FFAABBDDN":
        #     return

        # 实时位置姿态
        if (check_topic(topic,Topic_Pattern.das_realtime_pos)):
            deal_realtime_pos(msg)

    except json.JSONDecodeError as e:
        tb_info = traceback.extract_tb(e.__traceback__)
        for frame in tb_info:
            LOGERR(f"[{frame.filename}:{frame.lineno}] @{frame.name}(), error:{e}, {e.doc}")
    except Exception as e:
        tb_info = traceback.extract_tb(e.__traceback__)
        for frame in tb_info:
            LOGERR(f"[{frame.filename}:{frame.lineno}] @{frame.name}(), error: {e}")


# 处理接收的消息入口
def process_message(client:mqtt.Client, userdata, msg:mqtt.MQTTMessage):
    topic = msg.topic
    qos = msg.qos

    if (check_topic(topic,Topic_Pattern.dev_all)):      # 设备
        deal_dev_msg(client, userdata, msg)
    elif (check_topic(topic,Topic_Pattern.mps_all)):     # 小程序服务
        return
        deal_mps_msg(client, userdata, msg)
    elif (check_topic(topic,Topic_Pattern.opc_all)):     # 运维客户端
        return
        deal_opc_msg(client, userdata, msg)
    elif (check_topic(topic,Topic_Pattern.das_all)):     # 设备接入
        deal_das_msg(client, userdata, msg)
    else:
        # LOGDBG(f"recv invalid topic: {msg.topic}")
        return