add slam module

src/slam_sdk2py_python/setup.py

@@ -0,0 +1,16 @@
+from setuptools import setup, find_packages
+
+setup(name='slam_sdk2py',
+      version='1.0.0',
+      author='Unitree',
+      author_email='unitree@unitree.com',
+      license="BSD-3-Clause",
+      packages=find_packages(),
+      description='Unitree robot sdk version 2 for python',
+      python_requires='>=3.8',
+      install_requires=[
+            "cyclonedds==0.10.2",
+            "numpy",
+            "opencv-python",
+      ],
+      )

src/slam_sdk2py_python/slam_sdk2py/__init__.py

@@ -0,0 +1,5 @@
+from . import idl
+
+__all__ = [
+    'idl'
+]

src/slam_sdk2py_python/slam_sdk2py/idl/__init__.py

@@ -0,0 +1,5 @@
+from . import graph_msgs
+
+__all__ = [
+    "graph_msgs"
+]

src/slam_sdk2py_python/slam_sdk2py/idl/default.py

@@ -0,0 +1,25 @@
+from .graph_msgs.msg.dds_ import *
+
+def graph_msgs_mgs_dds__MultiArrayDimension_():
+    return MultiArrayDimension_('', 0, 0)
+
+def graph_msgs_mgs_dds__MultiArrayLayout_():
+    return MultiArrayLayout_(graph_msgs_mgs_dds__MultiArrayDimension_(), 0)
+
+def graph_msgs_mgs_dds__Float32MutiArray_():
+    return Float32MutiArray_(graph_msgs_mgs_dds__MultiArrayLayout_(), [])
+
+def graph_msgs_mgs_dds__Node_():
+    return Node_([],[],[],[],[],[],[],[],[],[])
+
+def graph_msgs_mgs_dds__QNode_():
+    return QNode_('', graph_msgs_mgs_dds__Node_())
+
+def graph_msgs_mgs_dds__Edge_():
+    return Edge_([],[],[],[],[],[],[],[],[],[],[],[],[])
+
+def graph_msgs_mgs_dds__QEdge_():
+    return QEdge_('', graph_msgs_mgs_dds__Edge_())
+
+def graph_msgs_mgs_dds__QCommand_():
+    return QCommand_(0,'',0,[],[],[],[],0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,[],[])

src/slam_sdk2py_python/slam_sdk2py/idl/graph_msgs/__init__.py

@@ -0,0 +1,7 @@
+# -*- coding: utf-8 -*-
+# @Author: privacy
+# @Date:   2024-07-12 13:42:27
+# @Last Modified by:   privacy
+# @Last Modified time: 2024-07-12 13:48:31
+from . import msg
+__all__ = ["msg", ]

src/slam_sdk2py_python/slam_sdk2py/idl/graph_msgs/msg/__init__.py

@@ -0,0 +1,8 @@
+# -*- coding: utf-8 -*-
+# @Author: privacy
+# @Date:   2024-07-12 13:42:37
+# @Last Modified by:   privacy
+# @Last Modified time: 2024-07-12 13:49:15
+
+from . import dds_
+__all__ = ["dds_", ]

src/slam_sdk2py_python/slam_sdk2py/idl/graph_msgs/msg/dds_/_Edge_.py

@@ -0,0 +1,23 @@
+from dataclasses import dataclass
+
+import cyclonedds.idl as idl
+import cyclonedds.idl.annotations as annotate
+import cyclonedds.idl.types as types
+
+@dataclass
+@annotate.final
+@annotate.autoid("sequential")
+class Edge_(idl.IdlStruct, typename="graph_msgs.msg.dds_.Edge_"):
+    edge_name: types.sequence[types.uint16]
+    start_node_name: types.sequence[types.uint16]
+    end_node_name: types.sequence[types.uint16]
+    edge_length: types.sequence[types.float32]
+    dog_stats: types.sequence[types.uint16]
+    dog_back_stats: types.sequence[types.uint16]
+    dog_speed: types.sequence[types.float32]
+    edge_state: types.sequence[types.uint16]
+    edge_state_1: types.sequence[types.uint16]
+    edge_state_2: types.sequence[types.uint16]
+    edge_state_3: types.sequence[types.uint16]
+    edge_state_4: types.sequence[types.uint16]
+    edge_state_list: types.sequence['slam_sdk2py.idl.graph_msgs.msg.dds_.Float32MultiArray_']

src/slam_sdk2py_python/slam_sdk2py/idl/graph_msgs/msg/dds_/_Float32MultiArray_.py

@@ -0,0 +1,17 @@
+# -*- coding: utf-8 -*-
+# @Author: privacy
+# @Date:   2024-07-12 15:12:31
+# @Last Modified by:   privacy
+# @Last Modified time: 2024-07-12 15:44:50
+from dataclasses import dataclass
+
+import cyclonedds.idl as idl
+import cyclonedds.idl.annotations as annotate
+import cyclonedds.idl.types as types
+
+@dataclass
+@annotate.final
+@annotate.autoid("sequential")
+class Float32MultiArray_(idl.IdlStruct, typename="graph_msgs.msg.dds_.Float32MultiArray_"):
+    layout_: 'slam_sdk2py.idl.graph_msgs.msg.dds_.MultiArrayLayout_'
+    data_: types.sequence[types.float32]

src/slam_sdk2py_python/slam_sdk2py/idl/graph_msgs/msg/dds_/_MultiArrayDimension_.py

@@ -0,0 +1,18 @@
+# -*- coding: utf-8 -*-
+# @Author: privacy
+# @Date:   2024-07-12 15:19:22
+# @Last Modified by:   privacy
+# @Last Modified time: 2024-07-12 15:20:26
+from dataclasses import dataclass
+
+import cyclonedds.idl as idl
+import cyclonedds.idl.annotations as annotate
+import cyclonedds.idl.types as types
+
+@dataclass
+@annotate.final
+@annotate.autoid("sequential")
+class MultiArrayDimension_(idl.IdlStruct, typename="graph_msgs.msg.dds_.MultiArrayDimension_"):
+    label_: 'unitree_sdk2py.idl.std_msgs.msg.dds_.String_'
+    size_ = 0
+    stride_ = 0

src/slam_sdk2py_python/slam_sdk2py/idl/graph_msgs/msg/dds_/_MultiArrayLayout_.py

@@ -0,0 +1,17 @@
+# -*- coding: utf-8 -*-
+# @Author: privacy
+# @Date:   2024-07-12 15:17:23
+# @Last Modified by:   privacy
+# @Last Modified time: 2024-07-12 15:18:50
+from dataclasses import dataclass
+
+import cyclonedds.idl as idl
+import cyclonedds.idl.annotations as annotate
+import cyclonedds.idl.types as types
+
+@dataclass
+@annotate.final
+@annotate.autoid("sequential")
+class MultiArrayLayout_(idl.IdlStruct, typename="graph_msgs.msg.dds_.MultiArrayLayout_"):
+    dim_: 'slam_sdk2py.idl.graph_msgs.msg.dds_.MultiArrayDimension_'
+    data_offset_ = 0

src/slam_sdk2py_python/slam_sdk2py/idl/graph_msgs/msg/dds_/_Node_.py

@@ -0,0 +1,20 @@
+from dataclasses import dataclass
+
+import cyclonedds.idl as idl
+import cyclonedds.idl.annotations as annotate
+import cyclonedds.idl.types as types
+
+@dataclass
+@annotate.final
+@annotate.autoid("sequential")
+class Node_(idl.IdlStruct, typename="graph_msgs.msg.dds_.Node_"):
+    node_name: types.sequence[types.uint16]
+    node_position_x: types.sequence[types.float32]
+    node_position_y: types.sequence[types.float32]
+    node_position_z: types.sequence[types.float32]
+    node_yaw: types.sequence[types.float32]
+    node_attribute: types.sequence[types.uint16]
+    undefined: types.sequence[types.uint16]
+    node_state_2: types.sequence[types.uint16]
+    node_state_3: types.sequence[types.uint16]
+    node_state_list: types.sequence['slam_sdk2py.idl.graph_msgs.msg.dds_.Float32MultiArray_']

src/slam_sdk2py_python/slam_sdk2py/idl/graph_msgs/msg/dds_/_QtCommand_.py

@@ -0,0 +1,29 @@
+from dataclasses import dataclass
+
+import cyclonedds.idl as idl
+import cyclonedds.idl.annotations as annotate
+import cyclonedds.idl.types as types
+
+@dataclass
+@annotate.final
+@annotate.autoid("sequential")
+class QtCommand_(idl.IdlStruct, typename="graph_msg.msg.dds_.QtCommand_"):
+    command: types.uint8
+    seq: 'unitree_sdk2py.idl.std_msgs.msg.dds_.String_'
+    attribute: types.uint8
+    floor_index: types.sequence[types.uint16]
+    pcdmap_index: types.sequence[types.uint16]
+    topomap_index: types.sequence[types.uint16]
+    node_edge_name: types.sequence[types.uint16]
+    quaternion_x: types.float32
+    quaternion_y: types.float32
+    quaternion_z: types.float32
+    quaternion_w: types.float32
+    euler_roll: types.float32
+    euler_pitch: types.float32
+    euler_yaw: types.float32
+    translation_x: types.float32
+    translation_y: types.float32
+    translation_z: types.float32
+    state_1: types.sequence[types.uint16]
+    state_2: types.sequence[types.float32]

src/slam_sdk2py_python/slam_sdk2py/idl/graph_msgs/msg/dds_/_QtEdge_.py

@@ -0,0 +1,12 @@
+from dataclasses import dataclass
+
+import cyclonedds.idl as idl
+import cyclonedds.idl.annotations as annotate
+import cyclonedds.idl.types as types
+
+@dataclass
+@annotate.final
+@annotate.autoid("sequential")
+class QtEdge_(idl.IdlStruct, typename="graph_msgs.msg.dds_.QtEdge_"):
+    seq: 'unitree_sdk2py.idl.std_msgs.msg.dds_.String_'
+    edge: 'slam_sdk2py.idl.graph_msgs.msg.dds_.Edge_'

src/slam_sdk2py_python/slam_sdk2py/idl/graph_msgs/msg/dds_/_QtNode_.py

@@ -0,0 +1,12 @@
+from dataclasses import dataclass
+
+import cyclonedds.idl as idl
+import cyclonedds.idl.annotations as annotate
+import cyclonedds.idl.types as types
+
+@dataclass
+@annotate.final
+@annotate.autoid("sequential")
+class QtNode_(idl.IdlStruct, typename="graph_msgs.msg.dds_.QtNode_"):
+    seq: 'unitree_sdk2py.idl.std_msgs.msg.dds_.String_'
+    node: 'slam_sdk2py.idl.graph_msgs.msg.dds_.Node_'

src/slam_sdk2py_python/slam_sdk2py/idl/graph_msgs/msg/dds_/__init__.py

@@ -0,0 +1,24 @@
+# -*- coding: utf-8 -*-
+# @Author: privacy
+# @Date:   2024-07-12 13:42:45
+# @Last Modified by:   privacy
+# @Last Modified time: 2024-07-12 15:44:08
+
+from ._MultiArrayDimension_ import MultiArrayDimension_
+from ._MultiArrayLayout_ import MultiArrayLayout_
+from ._Float32MultiArray_ import Float32MultiArray_
+from ._Node_ import Node_
+from ._Edge_ import Edge_
+from ._QtNode_ import QtNode_
+from ._QtEdge_ import QtEdge_
+from ._QtCommand_ import QtCommand_
+__all__ = [
+    "MultiArrayDimension_",
+    "MultiArrayLayout_",
+    "Float32MultiArray_",
+    "Node_",
+    "Edge_",
+    "QtNode_",
+    "QtEdge_",
+    "QtCommand_"
+]

src/slam_test.py

@@ -0,0 +1,303 @@
+# -*- coding: utf-8 -*-
+# @Author: privacy
+# @Date:   2024-07-05 15:02:27
+# @Last Modified by:   privacy
+# @Last Modified time: 2024-07-12 15:43:27
+"""
+octet = types.uint8
+unsigned long = types.uint32
+unsigned short = types.uint16
+"""
+import time
+
+from unitree_sdk2py.core.channel import (
+    ChannelFactoryInitialize,
+    ChannelSubscriber,
+    ChannelPublisher
+)
+
+from slam_sdk2py.idl.graph_msgs.msg.dds_ import *
+
+class slamDemo:
+    def __init__(self):
+        self.index = 0
+        # self.currentOdom
+        self.node_name = 0
+
+        ChannelFactoryInitialize(0, "eth0")
+
+        self.pubQtCommand = ChannelPublisher("rt/qt_command", QtCommand_)
+        self.pubQtNode = ChannelPublisher("rt/qt_add_node", QtNode_)
+        self.pubQtEdge = ChannelPublisher("rt/qt_add_edge", QtEdge_)
+
+        self.pubQtCommand.Init()
+        self.pubQtNode.Init()
+        self.pubQtEdge.Init()
+
+        # self.subQtNotice = ChannelSubscriber("rt/qt_notice")
+        # self.subOdommetry = ChannelSubscriber("rt/lio_sam_ros2/mapping/re_location_odometry")
+    
+    def startMapping(self) -> None:
+        """开启建图"""
+        self.index += 1
+        send_msg = QtCommand_()
+        send_msg.command = 3
+        send_msg.attribute = 2
+        send_msg.seq = "index:" + str(self.index) + ";"
+
+        if self.pubQtCommand.Write(send_msg, 0.5):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    def endMapping(self, floor_index=0, pcdmap_index=0) -> None:
+        """结束建图"""
+        self.index += 1
+        send_msg = QtCommand_()
+        send_msg.command = 4
+        send_msg.attribute = 0
+        send_msg.seq = "index:" + str(self.index) + ";"
+        send_msg.floor_index.append(floor_index)
+        send_msg.pcdmap_index.append(pcdmap_index)
+
+        if self.pubQtCommand.Write(send_msg, 0.5):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    # def addNode(self):
+    #     """增加导航拓扑点"""
+    #     send_msg = QtNode_()
+    #     send_msg.seq = "index:" + str(self.index) + ";"
+
+    def addEdge(self, edge_name: int, start_node: int, end_node: int) -> None:
+        """增加导航拓扑边"""
+        self.index += 1
+        send_msg = QtEdge_()
+        send_msg.seq = "index:" + str(index) + ";"
+        send_msg.edge.edge_name_().append(edge_name);
+        send_msg.edge.start_node_name.append(start_node)
+        send_msg.edge.end_node_name.append(end_node)
+        send_msg.edge.dog_speed.append(0.5)
+
+        send_msg.edge.dog_stats.append(0)
+        send_msg.edge.edge_length.append(0)
+        send_msg.edge.dog_back_stats.append(0)
+        send_msg.edge.edge_state.append(0)
+        send_msg.edge.edge_state_1.append(0)
+        send_msg.edge.edge_state_2.append(0)
+        send_msg.edge.edge_state_3.append(0)
+        send_msg.edge.edge_state_4.append(0)
+
+        if self.pubQtEdge.Write(send_msg):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+        """删除拓扑点"""
+
+        """删除拓扑边"""
+
+    def query_nodes(self):
+        """查询点信息"""
+        send_msg = QtCommand_()
+        pass
+
+    def query_edges(self):
+        """查询边信息"""
+        pass
+
+    def startRelocation(self) -> None:
+        """开启重定位"""
+        self.index += 1
+        send_msg = QtCommand_()
+        send_msg.seq = "index:" + str(self.index) + ";"
+        send_msg.command = 6
+        send_msg.attribute = 2
+
+        if self.pubQtCommand.Write(send_msg, 0.5):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    def initPost(self):
+        """重定位初始化"""
+        self.index += 1
+        send_msg = QtCommand_()
+        send_msg.seq = "index:" + str(self.index) + ";"
+        send_msg.command = 7
+        send_msg.quaternion_x = 0
+        send_msg.quaternion_y = 0
+        send_msg.quaternion_z = 0
+        send_msg.quaternion_w = 1
+        send_msg.translation_x = 0
+        send_msg.translation_y = 0
+        send_msg.translation_z = 0
+
+        if self.pubQtCommand.Write(send_msg, 0.5):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    def startNavigation(self):
+        """开启导航"""
+        self.index += 1
+        send_msg = QtCommand_()
+        send_msg.seq = "index:" + str(self.index) + ";"
+        send_msg.command = 8
+
+        if self.pubQtCommand.Write(send_msg, 0.5):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    def defaultMultiNavigation(self):
+        """多点循环导航（默认点）"""
+        send_msg = QtCommand_()
+        send_msg.seq = "index:" + str(self.index) + ";"
+        send_msg.command = 10
+
+        if self.pubQtCommand.Write(send_msg, 0.5):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    def setMultiNavigation(self, nodes: list):
+        """ 多点循环导航（设置点）"""
+        self.index += 1
+        send_msg = QtCommand_()
+        send_msg.seq = "index:" + str(self.index) + ";"
+        send_msg.command = 11
+        for node in nodes:
+            send_msg.node_edge_name.append(node)
+
+        if self.pubQtCommand.Write(send_msg, 0.5):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    def deleteAllNode(self) -> None:
+        self.index += 1
+        send_msg = QtCommand_()
+        send_msg.seq = "index:" + str(self.index) + ";"
+        send_msg.command = 1
+        send_msg.attribute = 1
+        send_msg.node_edge_name.append(999)
+
+        if self.pubQtCommand.Write(send_msg, 0.5):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    def deleteAllEdge(self) -> None:
+        self.index += 1
+        send_msg = QtCommand_()
+        send_msg.seq = "index:" + str(self.index) + ";"
+        # 1为删除指令
+        send_msg.command = 1
+        # 2为边
+        send_msg.attribute = 2
+        send_msg.node_edge_name.append(999)
+
+        if self.pubQtCommand.Write(send_msg, 0.5):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    def pauseNavigation(self) -> None:
+        """暂停导航"""
+        self.index += 1
+        send_msg = QtCommand_()
+        send_msg.seq = "index:" + str(self.index) + ";"
+        send_msg.command = 13
+
+        if self.pubQtCommand.Write(send_msg, 0.5):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    def recoverNavigation(self) -> None:
+        """恢复导航"""
+        self.index += 1
+        send_msg = QtCommand_()
+        send_msg.seq = "index:" + str(self.index) + ";"
+        send_msg.command = 14
+
+        if self.pubQtCommand.Write(send_msg, 0.5):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    def resturnStartNode(self) -> None:
+        """返回起点（默认第一个点为起点）"""
+        self.index += 1
+        send_msg = QtCommand_()
+        send_msg.seq = "index:" + str(self.index) + ";"
+        send_msg.command = 15
+
+        if self.pubQtCommand.Write(send_msg, 0.5):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    def closeAllNodes(self) -> None:
+        """关闭所有节点"""
+        self.index += 1
+        send_msg = QtCommand_()
+        send_msg.seq = "index:" + str(self.index) + ";"
+        send_msg.command = 99
+
+        if self.pubQtCommand.Write(send_msg, 0.5):
+            print("Publish success. msg:", send_msg)
+        else:
+            print("Waitting for subscriber.")
+
+        time.sleep(1)
+
+    # def addNodeAndEdge(self):
+    #     self.index += 1
+    #     send_msg = QtNode_()
+    #     timeNow = time.time()
+
+
+if __name__ == "__main__":
+    # ChannelFactoryInitialize(0, "eth0")
+    # sub = ChannelSubscriber("rt/qt_command", SLAMState_)
+    # sub.Init(SLAMStateHandler, 10)
+
+    # while True:
+    #     time.sleep(10.0)
+
+    slamTest = slamDemo()
+
+    while True:
+        time.sleep(10.0)
+
+
+##########################################

src/tasks.py

@@ -2,7 +2,7 @@
 # @Author: privacy
 # @Date:   2024-06-24 17:04:17
 # @Last Modified by:   privacy
-# @Last Modified time: 2024-07-10 10:52:16
+# @Last Modified time: 2024-07-15 09:43:07
 import time
 import json
 import datetime
@@ -33,6 +33,13 @@ class CustomClient:
     def LowStateHandler(self, msg: LowState_) -> None:
         payload = {
             'bitFlag': msg.bit_flag,
+            'fanFrequency': msg.fan_frequency,
+            'temperature': msg.motor_state.temperature,
+            'reserve': msg.motor_state.reserve
+        }
+        # print(payload)
+        self.queue.put({'topic': 'dog1/fanlt', 'payload': payload})
+        payload = {
             'adcReel': msg.adc_reel,
             'temperatureNtc1': msg.temperature_ntc1,
             'temperatureNtc2': msg.temperature_ntc2,

src/videoclass.py

@@ -2,46 +2,155 @@
 # @Author: privacy
 # @Date:   2024-07-02 13:55:20
 # @Last Modified by:   privacy
-# @Last Modified time: 2024-07-08 15:49:53
+# @Last Modified time: 2024-07-12 16:54:01
+
+# import subprocess
+
+# from utils import get_interface_ip
+
+
+# class VideoServer:
+
+#     def __init__(self, cmd = """ gst-launch-1.0 udpsrc address=230.1.1.1 port=1720 ! queue ! decodebin ! autovideosink """):
+#         super(VideoServer, self).__init__()
+#         self.p = None
+#         self.cmd = cmd.split()
+
+#     def run(self):
+#         print(self.cmd)
+#         self.p = subprocess.Popen(self.cmd)
+
+#     def stop(self):
+#         self.p.terminate()
+#         self.p.wait()
+
+
+# if __name__ == '__main__':
+#     gst = "gst-launch-1.0"
+
+#     rtmp_addr = "rtmp://192.168.1.150:1935/live/stream"
+
+#     # 获取 eth0 网卡的 IP 地址
+#     eth0_ip = get_interface_ip('eth0')
+
+#     if eth0_ip:
+#         print(f"The IP address of eth0 is: {eth0_ip}")
+#     else:
+#         print("Could not find the IP address of eth0.")
+
+#     # cmd = f""" {gst} udpsrc address=230.1.1.1 port=1720 ! queue ! decodebin ! autovideosink """
+#     # cmd = f""" {gst} udpsrc address=230.1.1.1 port=1720 ! queue ! decodebin ! x264enc speed-preset=ultrafast tune=zerolatency ! flvmux ! rtmpsink location="{rtmp_addr}" """
+#     cmd = f""" gst-launch-1.0 udpsrc address=230.1.1.1 port=1720 multicast-iface=eth0 ! """ \
+#            """ application/x-rtp, media=video, encoding-name=H264, framerate=30/1 ! """ \
+#            """ rtph264depay queue-delay=0 ! h264parse ! avdec_h264 ! videoconvert ! """ \
+#            """ x264enc key-int-max=30 speed-preset=ultrafast tune=zerolatency ! """ \
+#            """ flvmux ! rtmpsink location="{rtmp_addr}" """
+
+#     # cmd = f""" ffmpeg -i udp://230.1.1.1:1720?multicast=1 -localaddr={eth0_ip} -c copy -tune zerolatency -sc_threshold 499 -profile high -preset ultrafast -f flv "{rtmp_addr}" """
+
+#     vs = VideoServer(cmd)
+#     vs.run()
+
+
+#########################################################################################
+
+
+import sys
 import subprocess
 
-from utils import get_interface_ip
+# import cv2
+# import numpy as np
+# from unitree_sdk2py.core.channel import ChannelFactoryInitialize
+# from unitree_sdk2py.go2.video.video_client import VideoClient
+
+# rtmp = 'rtmp://180.76.147.97:1935/hls/test'
+rtmp = 'rtmp://192.168.1.150:1935/hls/test'
+
+
+
+
 
+###################################################################################
 
-class VideoServer:
 
-    def __init__(self, cmd = """ D:/software/gstreamer/1.0/msvc_x86_64/bin/gst-launch-1.0 udpsrc address=230.1.1.1 port=1720 ! queue ! decodebin ! autovideosink """):
-        super(VideoServer, self).__init__()
-        self.p = None
-        self.cmd = cmd.split()
+import cv2
+import numpy as np
+from datetime import datetime
 
-    def run(self):
-        print(self.cmd)
-        self.p = subprocess.Popen(self.cmd)
+# 函数：生成带有当前时间的图像
+def generate_time_image(width: int = 1280, height: int = 720):
+    now = datetime.now()
+    time_str = now.strftime("%H:%M:%S")
+    # 创建一个空白图像
+    img = np.zeros((height, width, 3), dtype=np.uint8)
+    # 在图像上写入时间
+    cv2.putText(img, time_str, (50, 150), cv2.FONT_HERSHEY_SIMPLEX, 3, (255, 255, 255), 5)
+    return img
 
-    def stop(self):
-        self.p.terminate()
-        self.p.wait()
+# 直接播放帧
+# while True:
+#     img = generate_time_image()
+#     cv2.imshow('Time', img)
+#     cv2.waitKey(33)  # 等待33毫秒（大约每秒30帧）
+
+# cv2.destroyAllWindows()
+
+
+# self.out = cv2.VideoWriter('appsrc is-live=True stream-type=0 emit-signals=False max-latency=1' + \
+#   ' ! videoconvert ! video/x-raw, format=I420' + \
+#   ' ! x264enc speed-preset=ultrafast bitrate=1200 tune="zerolatency" key-int-max=' + str( keyIntMax ) + \
+#   ' ! video/x-h264,profile=baseline' + \
+#   ' ! rtspclientsink location=%s' %(self.rtsp_url),
+#   cv2.CAP_GSTREAMER, cv2.VideoWriter_fourcc(*'h264'), self.fps, (self.width, self.height), True)
 
 
 if __name__ == '__main__':
-    gst = "D:/desktop/xianrobot/venv/Lib/site-packages/gnome/gst-launch.exe"
-    rtmp_addr = "rtmp://192.168.1.150:1935/live/stream"
+    width = 1280
+    height = 720
+
+#     ChannelFactoryInitialize(0, 'eth0')
+
+#     client = VideoClient()
+#     client.SetTimeout(3.0)
+#     client.Init()
+
+#     code, data = client.GetImageSample()
+
+    command = [
+        'ffmpeg',
+        '-y', '-an',
+        '-f', 'rawvideo',
+        '-vcodec','rawvideo',
+        '-pix_fmt', 'bgr24',
+        '-video_size', f'{width}x{height}',
+        '-i', '-', # 从标准输入读取数据
+        '-c:v','libx264', # 使用x264编码器
+        '-preset', 'ultrafast',
+        '-g', '1', # 设置GOP长度
+        '-keyint_min', '2', # 最小关键帧间隔
+        '-sc_threshold', '0', # 禁止自动添加I帧
+        '-tune', 'zerolatency', # 零延迟
+        '-pix_fmt', 'yuv420p',
+        '-f', 'flv',
+        rtmp]
+     
+    pipe = subprocess.Popen(
+        command,
+        shell=False,
+        stdin=subprocess.PIPE
+    )
 
-    # 获取 eth0 网卡的 IP 地址
-    eth0_ip = get_interface_ip('eth0')
+    for _ in range(100000):
+        img = generate_time_image(width=width, height=height)
+        pipe.stdin.write(img.tostring())
+        cv2.waitKey(33)
 
-    if eth0_ip:
-        print(f"The IP address of eth0 is: {eth0_ip}")
-    else:
-        print("Could not find the IP address of eth0.")
+    # while (code == 0):
+    #     code, data = client.GetImageSample()
 
-    cmd = f""" {gst} udpsrc address=230.1.1.1 port=1720 ! queue ! decodebin ! autovideosink """
-    cmd = f""" {gst} udpsrc address=230.1.1.1 port=1720 ! queue ! decodebin ! x264enc speed-preset=ultrafast tune=zerolatency ! flvmux ! rtmpsink location="{rtmp_addr}" """
-    # cmd = """ gst-launch-1.0 udpsrc address=230.1.1.1 port=1720 multicast-iface=eth0 ! queue ! application/x-rtp, media=video, encoding-name=H264 ! rtph264depay ! h264parse ! flvmux ! rtmpsink location="{rtmp_addr}" """
-    cmd = f""" gst-launch-1.0 udpsrc address=230.1.1.1 port=1720 multicast-iface=<interface_name> ! queue !  application/x-rtp, media=video, encoding-name=H264 ! rtph264depay ! h264parse ! avdec_h264 ! videoconvert ! x264enc speed-preset=ultrafast tune=zerolatency ! flvmux ! rtmpsink location="{rtmp_addr}" """
+    #     image_data = np.frombuffer(bytes(data), dtype=np.uint8)
+    #     image = cv2.imdecode(image_data, cv2.IMREAD_COLOR)
 
-    # cmd = f""" ffmpeg -i udp://230.1.1.1:1720?multicast=1 -localaddr={eth0_ip} -c copy -tune zerolatency -sc_threshold 499 -profile high -preset ultrafast -f flv "{rtmp_addr}" """
+    #     pipe.stdin.write(image.tostring())
 
-    vs = VideoServer(cmd)
-    vs.run()
+    pipe.terminate()