mirror of
https://github.com/dptech-corp/Uni-Lab-OS.git
synced 2025-12-14 13:14:39 +00:00
101 lines
3.1 KiB
Python
101 lines
3.1 KiB
Python
import socket
|
|
import json
|
|
import time
|
|
from pydantic import BaseModel
|
|
|
|
|
|
class AgvNavigator:
|
|
def __init__(self, host):
|
|
self.control_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|
self.receive_socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
|
self.control_socket.connect((host, 19206))
|
|
self.receive_socket.connect((host, 19204))
|
|
self.rec_cmd_code = {
|
|
"pose" : "03EC",
|
|
"status" : "03FC",
|
|
"nav" : "0BEB"
|
|
}
|
|
self.status_list = ['NONE', 'WAITING', 'RUNNING', 'SUSPENDED', 'COMPLETED', 'FAILED', 'CANCELED']
|
|
self._pose = []
|
|
self._status = 'NONE'
|
|
self.success = False
|
|
|
|
@property
|
|
def pose(self) -> list:
|
|
data = self.send('pose')
|
|
|
|
try:
|
|
self._pose = [data['x'], data['y'], data['angle']]
|
|
except:
|
|
print(data)
|
|
|
|
return self._pose
|
|
|
|
@property
|
|
def status(self) -> str:
|
|
data = self.send('status')
|
|
self._status = self.status_list[data['task_status']]
|
|
return self._status
|
|
|
|
def send(self, cmd, ex_data = '', obj = 'receive_socket'):
|
|
data = bytearray.fromhex(f"5A 01 00 01 00 00 00 00 {self.rec_cmd_code[cmd]} 00 00 00 00 00 00")
|
|
if ex_data:
|
|
data_ = ex_data
|
|
data[7] = len(data_)
|
|
data= data + bytearray(data_,"utf-8")
|
|
|
|
cmd_obj = getattr(self, obj)
|
|
cmd_obj.sendall(data)
|
|
response_data = b""
|
|
while True:
|
|
part = cmd_obj.recv(4096) # 每次接收 4096 字节
|
|
response_data += part
|
|
if len(part) < 4096: # 当接收到的数据少于缓冲区大小时，表示接收完毕
|
|
break
|
|
|
|
response_str = response_data.hex()
|
|
json_start = response_str.find('7b') # 找到JSON的开始位置
|
|
if json_start == -1:
|
|
raise "Error: No JSON data found in response."
|
|
|
|
json_data = bytes.fromhex(response_str[json_start:])
|
|
|
|
# 尝试解析 JSON 数据
|
|
try:
|
|
parsed_json = json.loads(json_data.decode('utf-8'))
|
|
return parsed_json
|
|
|
|
except json.JSONDecodeError as e:
|
|
raise f"JSON Decode Error: {e}"
|
|
|
|
|
|
def send_nav_task(self, command:str):
|
|
self.success = False
|
|
# source,target = cmd.replace(' ','').split("->")
|
|
|
|
target = json.loads(command)['target']
|
|
json_data = {}
|
|
# json_data["source_id"] = source
|
|
json_data["id"] = target
|
|
# json_data["use_down_pgv"] = True
|
|
result = self.send('nav', ex_data=json.dumps(json_data), obj="control_socket")
|
|
try:
|
|
if result['ret_code'] == 0:
|
|
# print(result)
|
|
while True:
|
|
if self.status == 'COMPLETED':
|
|
break
|
|
time.sleep(1)
|
|
self.success = True
|
|
except:
|
|
self.success = False
|
|
|
|
|
|
def __del__(self):
|
|
self.control_socket.close()
|
|
self.receive_socket.close()
|
|
|
|
if __name__ == "__main__":
|
|
agv = AgvNavigator("192.168.1.42")
|
|
# print(agv.pose)
|
|
agv.send_nav_task('LM14') |