From 21afcccf78a144176aa077509cece303cd2f6781 Mon Sep 17 00:00:00 2001 From: weiquan Date: Fri, 12 Nov 2021 15:20:38 +0800 Subject: [PATCH] Increase tcp/ip control --- demo/Client.py | 46 +--- demo/{REMADE.md => SOCT_REMADE.md} | 4 +- demo/Server.py | 368 +++++++++++++++++++++++------ docs/README.md | 34 ++- pymycobot/__init__.py | 4 +- pymycobot/mycobotsocket.py | 22 +- tests/test_socket.py | 4 - 7 files changed, 348 insertions(+), 134 deletions(-) rename demo/{REMADE.md => SOCT_REMADE.md} (78%) diff --git a/demo/Client.py b/demo/Client.py index a5382d3..29bce82 100644 --- a/demo/Client.py +++ b/demo/Client.py @@ -1,43 +1,5 @@ +#!/usr/bin/env python3 # coding:utf-8 -# // An highlighted block -import traceback -import socket -import time -import sys -# RPi's IP -SERVER_IP = "192.168.10.115" # 输入正确的目标ip地址，请查看树莓派ip -SERVER_PORT = 9000 - -print("Starting socket: TCP...") - -print("Please input!") -sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) -sock.connect((SERVER_IP, SERVER_PORT)) # 打开链接 -444 -sock.sendall(str.encode("12312")) -received = sock.recv(1024) -print("read: " + received.decode()) -while True: - try: - # - # if len(data)>0: - command = input() - command += '\n' - print("send: " + command) - try: - sock.sendall(str.encode(command)) - received = sock.recv(1024) - print("read: " + received.decode()) - except ConnectionRefusedError: - print('error') - sock.close() - pass - except BlockingIOError: - pass - except: - pass - - except Exception: - # print("exception") - print(traceback.format_exc()) - sys.exit(1) +from pymycobot.mycobotsocket import MyCobotSocket +m = MyCobotSocket("192.168.10.115", "/dev/ttyAMA0", "1000000") +print(m.send_command("get_coords")) diff --git a/demo/REMADE.md b/demo/SOCT_REMADE.md similarity index 78% rename from demo/REMADE.md rename to demo/SOCT_REMADE.md index c1897b1..e8e22cf 100644 --- a/demo/REMADE.md +++ b/demo/SOCT_REMADE.md @@ -1,7 +1,7 @@ # 使用说明 ## 使用说明 -* "Client.py"是客户端的样例，使用在电脑中 -* "Servo.py"是服务器端的执行文件，使用前要修改对应的ip地址信息，防止设备无法连接 +* "Client.py"是客户端的样例，使用在你的电脑中运行 +* "Servo.py"是服务器端的执行文件，在机械臂中运行 ## 参考内容 * [Socket 使用解析](https://blog.csdn.net/pashanhu6402/article/details/96428887?ops_request_misc=%257B%2522request%255Fid%2522%253A%2522162392592016780357215629%2522%252C%2522scm%2522%253A%252220140713.130102334..%2522%257D&request_id=162392592016780357215629&biz_id=0&utm_medium=distribute.pc_search_result.none-task-blog-2~all~top_positive~default-1-96428887.first_rank_v2_pc_rank_v29&utm_term=socket&spm=1018.2226.3001.4187) diff --git a/demo/Server.py b/demo/Server.py index 3900991..d8a179f 100644 --- a/demo/Server.py +++ b/demo/Server.py @@ -1,87 +1,307 @@ -import time -import socketserver -from pymycobot.mycobot import MyCobot -import numpy as np +#!/usr/bin/env python3 +# coding:utf-8 +import re +import socket +from pymycobot import MyCobot -class TCPMessageHandler(socketserver.StreamRequestHandler): +def re_data_1(command): + # (all_data) + r1 = re.compile(r'[(](.*?)[)]') + all_data = re.findall(r1, command)[0] + return all_data - def handle(self): - """ - Process socket data +def re_data_2(command): + r2 = re.compile(r'[[](.*?)[]]') + data_list = re.findall(r2, command)[0] + return data_list + +def get_data(command): + # get speed + # get () content + all_data = re_data_1(command) # "[0,0,0,0,0,0],60" + # get laet , + spe = re.search(r'\s*([^,]*)$',all_data) + speed = int(spe.group(1)) - """ - # process received data. + # get angles or coords + # get [] content + angles = [float(i) for i in re_data_2(all_data).split(",")] + return angles,speed + +def main(HOST,PORT): + s = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + s.bind((HOST,PORT)) + print("Binding succeeded!") + s.listen(1) + try: while True: - try: - # data=bytes.decode(self.rfile.readline().strip()) - data = bytes.decode(self.request.recv(1024)) - print(data) - except Exception as ex: - print(ex) - return - - data = data.split(',') - commend = [] - - for dt in data: + conn, addr = s.accept() + port_baud = [] + command = "" + while True: try: - if dt.isdigit(): - commend.append(int(dt)) - else: - commend.append(float(dt)) - except ValueError: - commend.append(None) + data = conn.recv(1024) + command = data.decode('utf-8') + if data.decode('utf-8') == "" or data.decode('utf-8') == "end": + break + res = None + command = command.replace(" ","") + if len(port_baud)<2: + port_baud.append(command) + if len(port_baud)==2: + mc = MyCobot(port_baud[0],port_baud[1]) + port_baud.append(1) + if "get_angles" in command: + res = mc.get_angles() + + elif command == "power_on": + mc.power_on() - if len(commend) > 0: - print("Received:%s" % commend) - self.request.sendall(str.encode(str(commend))) + elif command == "power_off": + mc.power_off() + elif command == "is_power_on": + res = mc.is_power_on() + elif command == "release_all_servos": + mc.release_all_servos() + elif command == "is_controller_connected": + res = mc.is_controller_connected() -class SocketServer(object): - def __init__(self, host, port): - super().__init__() - self.host = host - self.port = port + elif "sync_send_angles" in command: + all_data = re_data_1(command) + angles = [float(i) for i in re_data_2(command).split(",")] + datas = all_data.split(',') + # get laet , + if len(datas) == 7: + speed = int(datas[-1]) + + timeout = 7 + else: + speed = int(datas[-2]) + timeout = int(datas[-1]) + mc.sync_send_angles(angles,speed,timeout) + + while True: + if mc.is_in_position(angles,0): + break - def start_server(self): - while True: - try: - server = socketserver.TCPServer((self.host, self.port), - TCPMessageHandler) - break - except: - time.sleep(1) - continue - try: - print("server open seccess") - server.serve_forever() - - finally: - print("server close") - server.server_close() - - -class Robot(object): - def __init__(self, serial, baud): - super().__init__() - self.serial = serial - self.baud = baud - - def start_Robot(self): - try: - mc = MyCobot(self.serial, self.baud) # 树莓派版本打开机械臂，串口固定，无需USB连接 - print(mc) # 打印机械臂端口信息 - except: - print("can not find cobot") - exit(0) + elif "sync_send_coords" in command: + all_data = re_data_1(command) + coords = [float(i) for i in re_data_2(command).split(",")] + datas = all_data.split(',') + if len(datas) == 8: + speed = int(datas[-2]) + mode = int(datas[-1]) + timeout = 7 + else: + speed = int(data[-3]) + mode = int(datas[-2]) + timeout = int(datas[-1]) + mc.sync_send_coords(coords,speed,mode,timeout) + while True: + if mc.is_in_position(coords,1): + break + + elif "send_angles" in command: + angles, speed = get_data(command) + mc.send_angles(angles,speed) + elif "send_angle" in command: + all_data = re_data_1(command).split(',') # ['1','60','40'] + mc.send_angle(int(all_data[0]),float(all_data[1]),int(all_data[2])) + elif command == "get_coords": + res = mc.get_coords() + + elif "send_coords" in command: + all_data = re_data_1(command) + coords = [float(i) for i in re_data_2(command).split(",")] + speed = int(all_data.split(',')[-2]) + model = re.search(r'\S([^,]*)$',all_data) + model = int(model.group(1)) + mc.send_coords(coords,speed,model) + + elif "is_in_position" in command: + data_list, id = get_data(command) + res = mc.is_in_position(data_list,id) + + elif "is_moving" in command: + res = mc.is_moving() + + + elif "jog_angle" in command: + all_data = re_data_1(command).split(',') # ['1','1','70'] + mc.jog_angle(int(all_data[0]),int(all_data[1]),int(all_data[2])) + + elif "jog_coord" in command: + all_data = re_data_1(command).split(',') # ['x','1','70'] + mc.jog_angle(int(all_data[0]),int(all_data[1]),int(all_data[2])) + + elif "stop" in command: + mc.stop() + + elif "jog_stop" in command: + mc.jog_stop() + + elif "set_encoders" in command: + encoders,speed = get_data(command) + mc.set_encoders(encoders,speed) + + elif "set_encoder" in command: + + all_data = re_data_1(command).split(',') # ['1','2046'] + mc.set_encoder(int(all_data[0]),int(all_data[1])) + + elif "get_encoders" in command: + res = mc.get_encoders() + + + elif "get_encoder" in command: + + all_data = re_data_1(command) # '1' + res = mc.get_encoder(int(all_data)) + + + elif "is_paused" in command: + res = mc.is_paused() + + + elif "paues" in command: + mc.pause() + + elif "resume" in command: + mc.resume() + + elif "get_speed" in command: + res = mc.get_speed() + + + elif "set_speed" in command: + + all_data = re_data_1(command) # '100' + mc.set_speed(int(all_data)) + + elif "get_joint_min_angle" in command: + + all_data = re_data_1(command) # '1' + res = mc.get_joint_min_angle(int(all_data)) + + + elif "get_joint_max_angle" in command: + + all_data = re_data_1(command) # '1' + res = mc.get_joint_max_angle(int(all_data)) + + + elif "is_servo_enable" in command: + + all_data = re_data_1(command) # '1' + res = mc.is_servo_enable(int(all_data)) + + + elif "is_all_servo_enable" in command: + res = mc.is_all_servo_enable() + + + elif "set_servo_data" in command: + + all_data = re_data_1(command).slipt(',') # '1,20,2046' + mc.set_servo_data(int(all_data[0]),int(all_data[1]),int(all_data[2])) + + elif "get_servo_data" in command: + + all_data = re_data_1(command).slipt(',') # '1,20' + res = mc.get_servo_data(int(all_data[0]),int(all_data[1])) + + + elif "set_servo_calibration" in command: + + all_data = re_data_1(command) # '1' + mc.set_servo_calibration(int(all_data)) + + elif "release_servo" in command: + + all_data = re_data_1(command) # '1' + + mc.release_servo(int(all_data)) + + elif "focus_servo" in command: + + all_data = re_data_1(command) # '1' + mc.focus_servo(int(all_data)) + + elif "set_color" in command: + + all_data = re_data_1(command).slipt(',') # ['0','0','0'] + mc.set_color(int(all_data[0]),int(all_data[1]),int(all_data[2])) + + elif "set_pin_mode" in command: + + all_data = re_data_1(command).slipt(',') # ['20','0'] + mc.set_pin_mode(int(all_data[0]),int(all_data[1])) + + elif "set_digital_output" in command: + + all_data = re_data_1(command).slipt(',') # ['20','0'] + mc.set_digital_output(int(all_data[0]),int(all_data[1])) + + elif "get_digital_input" in command: + + all_data = re_data_1(command) # '3' + mc.get_digital_input(int(all_data)) + + elif "set_pwm_output" in command: + + all_data = re_data_1(command).slipt(',') # ['20','0','100'] + mc.set_pwm_output(int(all_data[0]),int(all_data[1]),int(all_data[2])) + + elif "get_gripper_value" in command: + res = mc.get_gripper_value() + + + elif "set_gripper_state" in command: + + all_data = re_data_1(command).slipt(',') # ['0','100'] + mc.set_gripper_state(int(all_data[0]),int(all_data[1])) + + elif "set_gripper_value" in command: + + all_data = re_data_1(command).slipt(',') # ['0','100'] + mc.set_gripper_value(int(all_data[0]),int(all_data[1])) + + elif "set_gripper_ini" in command: + mc.set_gripper_ini() + + elif "is_gripper_moving" in command: + res = mc.is_gripper_moving() + + + elif "get_basic_input" in command: + + all_data = re_data_1(command) # 2 + res = mc.get_basic_input(int(all_data)) + + + elif "set_basic_output" in command: + + all_data = re_data_1(command).slipt(',') # ['20','1'] + mc.set_basic_output(int(all_data[0]),int(all_data[1])) + + elif "gpio_init" in command: + mc.gpio_init() + + elif "gpio_output" in command: + all_data = re_data_1(command).slipt(',') # ['20','1'] + mc.gpio_output(int(all_data[0]),int(all_data[1])) + + conn.sendall(str.encode(str(res))) + except Exception as e: + conn.sendall(str.encode("ERROR:"+str(e))) + break + except: + conn.close() if __name__ == "__main__": - host = '192.168.10.191' # 输入本机IP地址 - port = 9000 - serial = '' - baud = 1000000 - server = SocketServer(host, port) # 声明服务器端口 - mycobot = Robot(serial, baud) # 声明机械臂接口 - server.start_server() # 永久打开服务器 + HOST = socket.gethostbyname(socket.gethostname()) + PORT = 9000 + main(HOST,PORT) diff --git a/docs/README.md b/docs/README.md index 0bad36d..e98e541 100644 --- a/docs/README.md +++ b/docs/README.md @@ -76,6 +76,9 @@ We support Python2, Python3.5 or later. - [utils (Module)](#utils-module) - [get_port_list](#get_port_list) - [detect_port_of_basic](#detect_port_of_basic) + - [MyCobotSocket](#mycobotsocket) + - [Client](#client) + - [Server](#server) @@ -734,10 +737,39 @@ from pymycobot import utils port = utils.detect_port_of_basic() if port is None: - raise Exception('Detection failed.') + raise Exception('Detection failed.') mycobot = MyCobot(port, 115200) ``` +## MyCobotSocket + +> Note: +> Only supports python3 +> The robotic arm that uses this class of premise has a server and has been turned on. + +Use TCP/IP to control the robotic arm + +### Client + +```python +# demo +from pymycobot import MyCobotSocket +# Port 9000 is used by default +mc = MyCobotSocket("192.168.10.115","/dev/ttyAMA0","1000000") + +mc.send_command("power_on") + +res = mc.send_command("get_angles") +print(res) +mc.send_command("send_angles([0,0,0,0,0,0],30)") +... + +``` + +### Server + +Server file is in the demo folder + --- More demo can go to [here](../demo). diff --git a/pymycobot/__init__.py b/pymycobot/__init__.py index 78a6e4b..322ec66 100644 --- a/pymycobot/__init__.py +++ b/pymycobot/__init__.py @@ -6,6 +6,7 @@ from pymycobot.generate import MyCobotCommandGenerator from pymycobot.mycobot import MyCobot from pymycobot.mypalletizer import MyPalletizer +from pymycobot.mycobotsocket import MyCobotSocket from pymycobot.genre import Angle, Coord from pymycobot import utils @@ -16,9 +17,10 @@ "Coord", "utils", "MyPalletizer", + "MyCobotSocket" ] -__version__ = "2.6.1" +__version__ = "2.6.2" __author__ = "Elephantrobotics" __email__ = "weiquan.xu@elephantrobotics.com" __git_url__ = "https://github.com/elephantrobotics/pymycobot" diff --git a/pymycobot/mycobotsocket.py b/pymycobot/mycobotsocket.py index bd65df8..f1224ee 100644 --- a/pymycobot/mycobotsocket.py +++ b/pymycobot/mycobotsocket.py @@ -3,22 +3,21 @@ from __future__ import division import socket import time -import sys -# import threading class MyCobotSocket: """MyCobot Python API Socket communication class.""" - def __init__(self, ip, port, baudrate="115200", timeout=0.1, debug=False): + def __init__(self, ip, port, baudrate="1000000"): """ Args: + ip : ip address port : port string - baudrate : baud rate string, default '115200' - timeout : default 0.1 - debug : whether show debug info + baudrate : baud rate string, default '1000000' """ - self.SERVER_IP = ip # 输入正确的目标ip地址，请查看树莓派ip + # 输入正确的目标ip地址，请查看树莓派ip + self.SERVER_IP = ip + # 端口号 self.SERVER_PORT = 9000 self.sock = self.connect() self.send_command(port) @@ -31,9 +30,12 @@ def connect(self): def send_command(self, command): # 发送数据 - self.sock.sendall((command+"~").encode()) + self.sock.sendall(command.encode()) # 接收数据 data = self.sock.recv(1024) - if data.decode() == "None": + res = data.decode() + if res == "None": return "" - return data.decode() + elif 'ERROR' in res: + raise Exception(command+"\n"+res) + return res diff --git a/tests/test_socket.py b/tests/test_socket.py index fdf118c..3c7c208 100644 --- a/tests/test_socket.py +++ b/tests/test_socket.py @@ -22,7 +22,3 @@ # m.send_command( # "sync_send_coords([53,53,412,-90.7,-1.5,12.67],30,1)") # print(m.send_command("is_in_position([53,53,412,-90.7,-1.5,12.67],1)")) - -# m.send_command("jog_angle(1,1,20)") -# time.sleep(2) -# m.send_command("jog_stop")