Thiết kế website giá rẻ

Question

I extracted the WebRTC part from go2_ros2_sdk for controlling Go2 (without ROS) but encountered the following issues:

webrtc_driver.py


import base64
import hashlib
import json
import logging
import time

import aiohttp
from aiortc import RTCPeerConnection, RTCSessionDescription
from aiortc.contrib.media import MediaBlackhole

logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)


class Go2Connection():
    def __init__(self, robot_ip, robot_num, token="", on_validated=None, on_message=None, on_open=None):
        self.pc = RTCPeerConnection()
        self.robot_ip = robot_ip
        self.robot_num = str(robot_num)
        self.token = token
        self.robot_validation = "PENDING"
        self.on_validated = on_validated
        self.on_message = on_message
        self.on_open = on_open
        
        self.audio_track = MediaBlackhole()
        self.video_track = MediaBlackhole()
        
        self.data_channel = self.pc.createDataChannel("data", id=0)
        self.data_channel.on("open", self.on_data_channel_open)
        self.data_channel.on("message", self.on_data_channel_message)
        
        self.pc.on("track", self.on_track)
        self.pc.on("connectionstatechange", self.on_connection_state_change)

    def on_connection_state_change(self):
        logger.info(f"Connection state is {self.pc.connectionState}")

    def on_track(self, track):
        logger.info(f"Receiving {track.kind}")

    async def generate_offer(self):
        await self.audio_track.start()
        await self.video_track.start()
        offer = await self.pc.createOffer()
        await self.pc.setLocalDescription(offer)
        return offer.sdp
    
    async def set_answer(self, sdp):
        answer = RTCSessionDescription(sdp, type="answer")
        await self.pc.setRemoteDescription(answer)
    
    def on_data_channel_open(self):
        logger.info("Data channel is open")
        if self.on_open:
            self.on_open()
    
    def on_data_channel_message(self, msg):
        logger.info(f"Received message: {msg}")

        if self.data_channel.readyState != "open":
            self.data_channel._setReadyState("open")

        try:
            if isinstance(msg, str):
                msgobj = json.loads(msg)
                if msgobj.get("type") == "validation":
                    self.validate_robot_conn(msgobj)
            if self.on_message:
                self.on_message(msg, msgobj, self.robot_num)
        except json.JSONDecodeError:
            pass
    
    async def connect(self):
        offer = await self.generate_offer()
        url = f"http://{self.robot_ip}:8081/offer"
        headers = {"Content-Type": "application/json"}
        data = {
            "sdp": offer,
            "id": "STA_localNetwork",
            "type": "offer",
            "token": "",
        }

        connected = False
        while not connected:
            async with aiohttp.ClientSession() as session:
                async with session.post(url, json=data, headers=headers) as resp:
                    if resp.status == 200:
                        answer_data = await resp.json()
                        answer_sdp = answer_data.get("sdp")
                        await self.set_answer(answer_sdp)
                        connected = True
                    else:
                        logger.info(f"Failed to get answer from server: Reason: {resp}")
                        logger.info("Try to reconnect...")
                        time.sleep(1)

    def validate_robot_conn(self, message):
        if message.get("data") == "Validation Ok.":
            self.robot_validation = "SUCCESS"
            if self.on_validated:
                self.on_validated(self.robot_num)
        else:
            self.publish(
                "",
                self.encrypt_key(message.get("data")),
                "validation",
            )

    def publish(self, topic, data, msg_type):
        if self.data_channel.readyState != "open":
            logger.info(f"Data channel is not open. State is {self.data_channel.readyState}")
            return
        
        payload = {
            "type": msg_type,
            "topic": topic,
            "data": data,
        }

        payload_dumped = json.dumps(payload)
        logger.info(f"-> Sending message {payload_dumped}")
        self.data_channel.send(payload_dumped)

    @staticmethod
    def hex_to_base64(hex_str):
        bytes_array = bytes.fromhex(hex_str)
        return base64.b64encode(bytes_array).decode("utf-8")

    @staticmethod
    def encrypt_key(key):
        prefixed_key = f"UnitreeGo2_{key}"
        encrypted = Go2Connection.encrypt_by_md5(prefixed_key)
        return Go2Connection.hex_to_base64(encrypted)

    @staticmethod
    def encrypt_by_md5(input_str):
        hash_obj = hashlib.md5()
        hash_obj.update(input_str.encode("utf-8"))
        return hash_obj.hexdigest()

When asyncio.run(connect()) finishes, the connection automatically closes: INFO:scripts.webrtc_driver:Connection state is closed.

import asyncio
import logging
import time

from scripts.go2_func import gen_command
from scripts.webrtc_driver import Go2Connection

logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)

robot_ip = "myip"
token = ""
robot_num = "0"

conn = Go2Connection(
    robot_ip=robot_ip,
    robot_num=robot_num,
    token=token,
    on_validated=lambda _: logger.info("Robot validated"),
    on_message=lambda msg, _, __: logger.info(f"Message received: {msg}")
)

async def connect():
    await conn.connect()
    logger.info("Connected to robot")

    while conn.robot_validation == "PENDING":
        await asyncio.sleep(0.1)

    if conn.robot_validation != "SUCCESS":
        logger.error("Failed to validate robot connection")
        return

    logger.info("Robot connection validated successfully")

def fun1():
    cmd = gen_command(1029)
    conn.data_channel.send(cmd)
    print('fun1 called')

def fun2():
    cmd = gen_command(1029)
    conn.data_channel.send(cmd)
    print('fun2 called')


if __name__ == "__main__":
    asyncio.run(connect())
    time.sleep(3)
    fun1()
    time.sleep(10)
    fun2()

I tried adding nest_async(), which resolved the automatic disconnection issue, but found that although fun1 and fun2 are executed correctly, Go2 has no response.

import asyncio
import logging
import time

import nest_asyncio
from scripts.go2_func import gen_command
from scripts.webrtc_driver import Go2Connection

nest_asyncio.apply()

logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)

robot_ip = "myip"
token = ""
robot_num = "0"

conn = Go2Connection(
    robot_ip=robot_ip,
    robot_num=robot_num,
    token=token,
    on_validated=lambda _: logger.info("Robot validated"),
    on_message=lambda msg, _, __: logger.info(f"Message received: {msg}")
)

async def connect():
    await conn.connect()
    logger.info("Connected to robot")

    while conn.robot_validation == "PENDING":
        await asyncio.sleep(0.1)

    if conn.robot_validation != "SUCCESS":
        logger.error("Failed to validate robot connection")
        return

    logger.info("Robot connection validated successfully")

def fun1():
    cmd = gen_command(1029)
    conn.data_channel.send(cmd)
    print('fun1 called')

def fun2():
    cmd = gen_command(1029)
    conn.data_channel.send(cmd)
    print('fun2 called')


if __name__ == "__main__":
    asyncio.run(connect())
    time.sleep(3)
    fun1()
    time.sleep(10)
    fun2()

fun1 called.
fun2 called.

Then, I made fun1 and fun2 async. They both executed, but only fun2 successfully controlled Go2, while fun1 still had no response.

import asyncio
import logging
import time
from functools import wraps

import nest_asyncio
from scripts.go2_func import gen_command
from scripts.webrtc_driver import Go2Connection

nest_asyncio.apply()

logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)

robot_ip = "192.168.68.66"
token = ""
robot_num = "0"

conn = Go2Connection(
    robot_ip=robot_ip,
    robot_num=robot_num,
    token=token,
    on_validated=lambda _: logger.info("Robot validated"),
    on_message=lambda msg, _, __: logger.info(f"Message received: {msg}")
)

async def connect():
    await conn.connect()
    logger.info("Connected to robot")

    while conn.robot_validation == "PENDING":
        await asyncio.sleep(0.1)

    if conn.robot_validation != "SUCCESS":
        logger.error("Failed to validate robot connection")
        return

    logger.info("Robot connection validated successfully")

def ensure_connected(func):
    @wraps(func)
    async def wrapper(*args, **kwargs):
        if conn.pc.connectionState != "connected":
            logger.info("Robot not connected")
            return
        if conn.data_channel.readyState != "open":
            logger.info("Data channel not opened")
            return
        return await func(*args, **kwargs)
    return wrapper

@ensure_connected
async def fun1():
    cmd = gen_command(1029)
    logger.info(f'Sending command from fun1: {cmd}')
    conn.data_channel.send(cmd)
    logger.info('fun1 called')

@ensure_connected
async def fun2():
    cmd = gen_command(1029)
    logger.info(f'Sending command from fun2: {cmd}')
    conn.data_channel.send(cmd)
    logger.info('fun2 called')


if __name__ == "__main__":
    asyncio.run(connect())
    time.sleep(3)
    asyncio.run(fun1())
    time.sleep(10)
    asyncio.run(fun2())

fun1 called.
fun2 called.

Only when I write all operations under a single async def can I control multiple commands correctly.

import asyncio
import logging

from scripts.go2_constants import ROBOT_CMD
from scripts.go2_func import gen_command, gen_mov_command
from scripts.webrtc_driver import Go2Connection

logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)


async def main():
    robot_ip = "myip"
    token = ""
    robot_num = "0"

    conn = Go2Connection(
        robot_ip=robot_ip,
        robot_num=robot_num,
        token=token,
        on_validated=lambda _: logger.info("Robot validated"),
        on_message=lambda msg, _, __: logger.info(f"Message received: {msg}")
    )
    await conn.connect()
    logger.info("Connected to robot")

    while conn.robot_validation == "PENDING":
        await asyncio.sleep(0.1)

    if conn.robot_validation != "SUCCESS":
        logger.error("Failed to validate robot connection")
        return

    logger.info("Robot connection validated successfully")

    cmd = gen_command(1029)
    conn.data_channel.send(cmd)

    await asyncio.sleep(10)

    cmd = gen_command(1029)
    conn.data_channel.send(cmd)

    await asyncio.sleep(5)

    await conn.pc.close()
    logger.info("Disconnected from robot to stop movement")

if __name__ == "__main__":
    asyncio.run(main())

Also the following method can work successfully.

import asyncio
import logging
import nest_asyncio
from functools import wraps
from scripts.go2_func import gen_command
from scripts.webrtc_driver import Go2Connection

nest_asyncio.apply()

logging.basicConfig(level=logging.INFO)
logger = logging.getLogger(__name__)

robot_ip = "myip"
token = ""
robot_num = "0"

conn = Go2Connection(
    robot_ip=robot_ip,
    robot_num=robot_num,
    token=token,
    on_validated=lambda _: logger.info("Robot validated"),
    on_message=lambda msg, _, __: logger.info(f"Message received: {msg}")
)

async def connect():
    await conn.connect()
    logger.info("Connected to robot")

    while conn.robot_validation == "PENDING":
        await asyncio.sleep(0.1)

    if conn.robot_validation != "SUCCESS":
        logger.error("Failed to validate robot connection")
        return

    logger.info("Robot connection validated successfully")

def ensure_connected(func):
    @wraps(func)
    async def wrapper(*args, **kwargs):
        if conn.pc.connectionState != "connected":
            logger.info("Robot not connected")
            return
        if conn.data_channel.readyState != "open":
            logger.info("Data channel not opened")
            return
        return await func(*args, **kwargs)
    return wrapper

@ensure_connected
async def fun1():
    cmd = gen_command(1029)
    conn.data_channel.send(cmd)
    print('fun1 called')

@ensure_connected
async def fun2():
    cmd = gen_command(1029)
    conn.data_channel.send(cmd)
    print('fun2 called')


if __name__ == "__main__":
    loop = asyncio.get_event_loop()
    loop.run_until_complete(connect())
    loop.create_task(fun1())
    loop.create_task(fun2())
    loop.run_forever()

What might be causing this issue? How can I ensure that different functions are called correctly without placing all commands in the same coroutine?

Thiết kế website giá rẻ

Danh mục

Cannot control go2 when use webrtc method

webrtc_driver.py