From 81761e7fbbcce1bb6f5f69b860da421085d7e2e8 Mon Sep 17 00:00:00 2001 From: Kseninia Mikhaylova Date: Mon, 14 Oct 2024 17:20:31 +0300 Subject: [PATCH] connect --- client_socket.py | 106 +++++++++++++----------------------------- gui_test.py | 57 +++++++++++++++++++---- juce_init.py | 15 +----- main.py | 27 +++++++---- robots.json | 12 +++++ test_socket_server.py | 72 ++++++++++++++++------------ 6 files changed, 153 insertions(+), 136 deletions(-) create mode 100644 robots.json diff --git a/client_socket.py b/client_socket.py index 50196bc..456a945 100644 --- a/client_socket.py +++ b/client_socket.py @@ -1,10 +1,9 @@ import socket import json import os -import sys +from typing import Literal import math from pprint import pprint -import tkinter import threading import pybullet as p import pybullet_data @@ -23,73 +22,63 @@ class SocketRobotArm: physical_speed = 10 # laser_id = 15 laser_id = 14 - # filename = 'half-sphere-no-angle' + filename = "test" urdf_filename = "sample" - # urdf_filename = "fanucM16ib" pass_size = 4 + Status = Literal["connected", "not_connected", "error"] def __init__(self, *args, **kwargs): - self.socket = socket.socket() - self.host = MODBUS_SERVER_HOST + self.socket = None + self.host = None self.port = 9760 + self.status: SocketRobotArm.Status = "not_connected" - if len(sys.argv) and "--test" in sys.argv: - self.host = "127.0.0.1" - self.port = 65432 + # self.host = MODBUS_SERVER_HOST + # if kwargs['test'] or len(sys.argv) and "--test" in sys.argv: + # self.host = "127.0.0.1" + # self.port = 65432 - self.tkinter_root = tkinter.Tk() - self.tkinter_info_label = tkinter.Label(self.tkinter_root) - self.tkinter_exit = tkinter.Button( - self.tkinter_root, text="Exit", command=self.close - ) - self.tkinter_start_cycle = tkinter.Button( - self.tkinter_root, text="Start cycle", command=self.cycle_start - ) - self.tkinter_imitate = tkinter.Button( - self.tkinter_root, text="Imitate", command=self.imitate - ) - self.tkinter_upd_model = tkinter.Button( - self.tkinter_root, text="Update model state", command=self.upd_model - ) self.physics_client = None self.body_id = None - threading.Thread(target=self.run_pybullet, daemon=True).start() - self.socket.connect((self.host, self.port)) - self.cycle_base() - - self.tkinter_root.mainloop() + # self.socket.connect((self.host, self.port)) + # self.cycle_base() def __exit__(self, exc_type, exc_value, traceback): print("exiting") self.socket.close() def run_pybullet(self): - self.physics_client = p.connect(p.GUI) + self.physics_client = p.connect(p.DIRECT) p.setGravity(0, 0, -9.81, physicsClientId=self.physics_client) p.setAdditionalSearchPath(pybullet_data.getDataPath()) self.body_id = p.loadURDF( f"urdf/{self.urdf_filename}.urdf", [0, 0, 0], useFixedBase=True, - globalScaling=1.5, ) - while True: - p.stepSimulation() - time.sleep(1 / 24) - def close(self): - print("close") - # self.tkinter_info_label.destroy() - self.tkinter_root.quit() self.socket.close() self.socket = None + self.status = "not_connected" + # self.socket = None # sys.exit() + def connect(self, host, slave_id): + self.host = host + self.slave_id = slave_id + if self.socket is None: + self.socket = socket.socket() + self.socket.connect((self.host, self.port)) + self.status = "connected" + + def get_status(self): + return self.status + def upd_model(self): self.get_axis() self.set_text(text=f"Координаты осей {self.start_axis_coordinates}") @@ -99,17 +88,9 @@ class SocketRobotArm: self.get_world() self.set_text(text=f"Мировые координаты {self.start_world_coordinates}") time.sleep(0.5) - # self.set_joint(self.convert_to_joint(self.start_world_coordinates[0:3])) + self.set_joint(self.convert_to_joint(self.start_world_coordinates[0:3])) def cycle_base(self): - self.tkinter_root.geometry("700x400") - - self.tkinter_imitate.pack() - self.tkinter_upd_model.pack() - - self.tkinter_start_cycle.pack(pady=20) - self.tkinter_exit.pack(pady=20) - self.upd_model() self.get_command_count() @@ -132,27 +113,14 @@ class SocketRobotArm: + [self.set_physical_speed(False), self.set_output_laser(False)] ) - print(self.convert_file_to_join()) - print(self.steps_from_file()) - - - self.tkinter_info_label.config(text=f"Отправка данных...") - self.tkinter_info_label.pack() - self.tkinter_root.update() - step = 4 empty = 1 for i in range(0, len(self.add_rcc_list), step): - if not self.tkinter_info_label: - return if not self.socket: return print(f"Отправка данных {i}...{i+step-1}") - # self.update_text(m, text=f"Отправка данных {i}...{i+step-1}") - self.tkinter_info_label.config(text=f"Отправка данных {i}...{i+step-1}") - self.tkinter_root.update() - # print(empty) + self.send_data(make_addrcc_data(self.add_rcc_list[i : i + step], empty)) empty = 0 time.sleep(0.05) @@ -179,6 +147,9 @@ class SocketRobotArm: print(joint_angles) return joint_angles + def set_text(self, text): + print(text) + def set_joint(self, coordinates): num_joints = p.getNumJoints(self.body_id) for joint_index in range(0, num_joints): @@ -190,18 +161,8 @@ class SocketRobotArm: controlMode=p.POSITION_CONTROL, targetPosition=coordinates[joint_index], ) - self.tkinter_info_label.config( - text=f"Ось {joint_index} повернуть на {coordinates[joint_index]} градусов" - ) - self.tkinter_info_label.pack() - self.tkinter_root.update() time.sleep(0.01) - def set_text(self, text): - label = tkinter.Label(self.tkinter_root, text=text) - label.pack() - self.tkinter_root.update() - def send_data(self, data): if not self.socket: return @@ -267,7 +228,7 @@ class SocketRobotArm: pairs = zip(self.start_world_coordinates, point) if type == "free": pairs = zip(self.start_axis_coordinates, point) - + m0, m1, m2, m3, m4, m5 = [round(sum(i), 3) for i in pairs] if type == "line": step.update({"action": "10"}) @@ -372,6 +333,3 @@ class SocketRobotArm: ) ) return result - - -SocketRobotArm() diff --git a/gui_test.py b/gui_test.py index 9c0d76a..4bdaaa5 100644 --- a/gui_test.py +++ b/gui_test.py @@ -1,8 +1,9 @@ from juce_init import START_JUCE_COMPONENT import popsicle as juce +import time -class ChangeRobot(juce.Component): +class ChangeRobot(juce.Component, juce.Timer): backgroundColour = juce.Colours.lightblue textColour = juce.Colours.black @@ -11,13 +12,20 @@ class ChangeRobot(juce.Component): robotsLabel = juce.Label("", "Выберите робота") robotsRadio = [] robotsControls = [robotsLabel] + robotsRadio - robotButtonsId = 1001 - def __init__(self, robots, updateRobot): + connectButton = juce.TextButton("Соединить") + + old_status = None + counter = 0 + + def __init__(self, robots, updateRobot, status): super().__init__() + juce.Timer.__init__(self) + self.robots = robots or [] self.updateRobot = updateRobot + self.status = status for r in self.robots: self.robotsRadio.append( @@ -29,10 +37,13 @@ class ChangeRobot(juce.Component): self.addAndMakeVisible(s) for s in self.robotsRadio: + s.onClick = self.setConnectText s.setRadioGroupId(self.robotButtonsId, juce.dontSendNotification) - s.onClick = self.updateToggleState s.setClickingTogglesState(True) + self.addAndMakeVisible(self.connectButton) + self.connectButton.onClick = self.connectRobot + def paint(self, g: juce.Graphics): g.fillAll(self.backgroundColour) @@ -44,15 +55,42 @@ class ChangeRobot(juce.Component): s.setBounds(10 if i == 0 else 20, (i * 20), self.getWidth() - 20, 20) - def updateToggleState(self): + h = i + 2 + self.connectButton.setBounds(10, (h * 20), self.getWidth() - 20, 20) + + def setConnectText(self): + if self.status() == "not_connected": + self.connectButton.setButtonText("Connect") + if self.status() == "connected": + self.connectButton.setButtonText("Disconnect") + + def timerCallback(self): + new_status = self.status() + self.counter += 1 + if new_status == self.old_status and self.counter <= 10: + self.connectButton.setButtonText(self.connectButton.getButtonText() + ".") + else: + self.old_status = None + self.stopTimer() + self.setConnectText() + + def connectRobot(self): + self.setConnectText() + res = None for i, s in enumerate(self.robotsRadio): if s.getToggleState() == True: - if self.updateRobot: - self.updateRobot(self.robots[i]) - else: - print(self.robots[i]) + res = i break + if res == None: + self.connectButton.setButtonText("Выберите робота") + return + + self.old_status = self.status() + self.startTimer(500) + time.sleep(0.3) + self.updateRobot(self.robots[i]) + class RightPanel(juce.Component): def __init__(self, panels): @@ -89,7 +127,6 @@ class MainContentComponent(juce.Component): robotArgs = kwargs.get("robotPanel") robotPanel = ChangeRobot(**robotArgs) - self.rightPanel = RightPanel(panels=[robotPanel]) self.mainPanel = MainPanel() diff --git a/juce_init.py b/juce_init.py index 4963fc1..445f3f8 100644 --- a/juce_init.py +++ b/juce_init.py @@ -93,17 +93,4 @@ def START_JUCE_COMPONENT(ComponentClass, name, **kwargs): DefaultApplication, catchExceptionsAndContinue=kwargs.get("catchExceptionsAndContinue", False), ) - - -def timeit(func): - @wraps(func) - def timeit_wrapper(*args, **kwargs): - start_time = time.perf_counter() - result = func(*args, **kwargs) - total_time = time.perf_counter() - start_time - print( - f"Function {func.__name__} Took {total_time:.4f} seconds" - ) # {args} {kwargs} - return result - - return timeit_wrapper + return DefaultApplication \ No newline at end of file diff --git a/main.py b/main.py index 3273cfb..9a7a015 100644 --- a/main.py +++ b/main.py @@ -1,31 +1,42 @@ -import threading +import json from juce_init import START_JUCE_COMPONENT from gui_test import MainContentComponent +from client_socket import SocketRobotArm +import time class MyApp: - robots = [ - {"name": "big", "host": "192.168.70.55", "slave_id": 11}, - {"name": "small", "host": "192.168.70.65", "slave_id": 22}, - ] - selected_robot = 0 + with open("./robots.json", "r") as file: + robots = json.load(file) def __init__(self): + self.startRobot() self.startGui() def startGui(self): - START_JUCE_COMPONENT( + self.gui_app = START_JUCE_COMPONENT( MainContentComponent, name="ROBOT GUI", robotPanel={ "robots": self.robots, "updateRobot": self.updateRobot, + "status": self.robot_app.get_status, }, ) + def startRobot(self): + self.robot_app = SocketRobotArm() + def updateRobot(self, robot): if robot in self.robots: - self.selected_robot = robot + selected_robot = robot + selected_robot = {"name": "test", "host": "127.0.0.1", "slave_id": 11} + + if self.robot_app.status == "connected": + self.robot_app.close() + time.sleep(0.3) + else: + self.robot_app.connect(selected_robot["host"], selected_robot["slave_id"]) if __name__ == "__main__": diff --git a/robots.json b/robots.json new file mode 100644 index 0000000..0840739 --- /dev/null +++ b/robots.json @@ -0,0 +1,12 @@ +[ + { + "name": "big", + "host": "192.168.70.55", + "slave_id": 11 + }, + { + "name": "small", + "host": "192.168.70.65", + "slave_id": 22 + } +] \ No newline at end of file diff --git a/test_socket_server.py b/test_socket_server.py index 0d01be5..1a1af98 100644 --- a/test_socket_server.py +++ b/test_socket_server.py @@ -2,37 +2,49 @@ import socket import json HOST = "127.0.0.1" # Standard loopback interface address (localhost) -PORT = 65432 # Port to listen on (non-privileged ports are > 1023) +PORT = 9760 # Port to listen on (non-privileged ports are > 1023) + +def handle_client(conn, addr): + print(f"Connected by {addr}") + try: + while True: + data = conn.recv(1024) + if not data: + break + req = json.loads(data) + res = {"queryData": ["ok"]} + + if "queryAddr" in req.keys() and "axis-0" in req["queryAddr"]: + res["queryData"] = [-60, 40, -10, -60, -75, 0] + + if "queryAddr" in req and "world-0" in req["queryAddr"]: + res["queryData"] = [ + 643.622, + -1289.604, + 254.682, + 124.70, + 24.209, + -58.492, + ] + + if req["reqType"] == "command": + res["cmdReply"] = ["ok"] + conn.sendall(json.dumps(res).encode()) + except json.JSONDecodeError as e: + print(f"JSON decode error: {e}") + except socket.error as e: + print(f"Socket error: {e}") + except Exception as e: + print(f"Unexpected error: {e}") + finally: + conn.close() + print(f"Connection with {addr} closed") + with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s: s.bind((HOST, PORT)) s.listen() - conn, addr = s.accept() - with conn: - print(f"Connected by {addr}") - while True: - try: - data = conn.recv(1024) - if data: - req = json.loads(data) - res = {"queryData": ["ok"]} - - if "queryAddr" in req.keys() and "axis-0" in req["queryAddr"]: - # res["queryData"] = [-59.696, 39.438, -7.478, -58.198, -76.606, 0.0] - res["queryData"] = [-60, 40, -10, -60, -75, 0] - - if "queryAddr" in req and "world-0" in req["queryAddr"]: - res["queryData"] = [ - 643.622, - -1289.604, - 254.682, - 124.70, - 24.209, - -58.492, - ] - - if req["reqType"] == "command": - res["cmdReply"] = ["ok"] - conn.sendall(json.dumps(res).encode()) - except Exception as e: - print(f"by {addr} error {e}") + print(f"Server listening on {HOST}:{PORT}") + while True: + conn, addr = s.accept() + handle_client(conn, addr)