You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
58 lines
1.6 KiB
58 lines
1.6 KiB
3 months ago
|
import json
|
||
|
|
import socket
|
||
|
|
import time
|
||
|
|
|
||
|
|
# 连接机器人控制器
|
||
|
|
def connectETController(ip, port=8055):
|
||
|
|
sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
|
||
|
|
try:
|
||
|
|
sock.connect((ip, port))
|
||
|
|
return (True, sock)
|
||
|
|
except Exception as e:
|
||
|
|
sock.close()
|
||
|
|
return (False,)
|
||
|
|
|
||
|
|
# 发送命令并接收响应
|
||
|
|
def sendCMD(sock, cmd, params=None, id=1):
|
||
|
|
if not params:
|
||
|
|
params = []
|
||
|
|
else:
|
||
|
|
params = json.dumps(params)
|
||
|
|
sendStr = '{"method":"%s","params":%s,"jsonrpc":"2.0","id":%d}' % (cmd, params, id) + "\n"
|
||
|
|
try:
|
||
|
|
sock.sendall(bytes(sendStr, "utf-8"))
|
||
|
|
ret = sock.recv(1024)
|
||
|
|
jdata = json.loads(str(ret, "utf-8"))
|
||
|
|
if "result" in jdata.keys():
|
||
|
|
return (True, json.loads(jdata["result"]), jdata["id"])
|
||
|
|
elif "error" in jdata.keys():
|
||
|
|
return (False, jdata["error"], jdata["id"])
|
||
|
|
else:
|
||
|
|
return (False, None, None)
|
||
|
|
except Exception as e:
|
||
|
|
return (False, None, None)
|
||
|
|
|
||
|
|
|
||
|
|
def send_Point(sock, cmd, params=None, id=1):
|
||
|
|
if (not params):
|
||
|
|
params = []
|
||
|
|
else:
|
||
|
|
params = json.dumps(params)
|
||
|
|
sendStr = "{{\"method\":\"{0}\",\"params\":{1},\"jsonrpc\":\"2.0\",\"id\":{2}}}".format(cmd, params, id) + "\n"
|
||
|
|
sock.sendall(bytes(sendStr, "utf-8"))
|
||
|
|
|
||
|
|
|
||
|
|
def wait_stop(sock): # 修正：添加sock参数
|
||
|
|
while True:
|
||
|
|
time.sleep(0.1)
|
||
|
|
ret1, result1, id1 = sendCMD(sock, "getRobotState") # getRobotstate
|
||
|
|
if (ret1):
|
||
|
|
if result1 == 0 or result1 == 4:
|
||
|
|
break
|
||
|
|
else:
|
||
|
|
#print("getRobotState failed")
|
||
|
|
continue
|
||
|
|
|
||
|
|
|
||
|
|
|