connect
This commit is contained in:
parent
8babbd50f5
commit
81761e7fbb
104
client_socket.py
104
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
|
||||
|
|
@ -372,6 +333,3 @@ class SocketRobotArm:
|
|||
)
|
||||
)
|
||||
return result
|
||||
|
||||
|
||||
SocketRobotArm()
|
||||
|
|
|
|||
57
gui_test.py
57
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()
|
||||
|
||||
|
|
|
|||
15
juce_init.py
15
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
|
||||
27
main.py
27
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__":
|
||||
|
|
|
|||
|
|
@ -0,0 +1,12 @@
|
|||
[
|
||||
{
|
||||
"name": "big",
|
||||
"host": "192.168.70.55",
|
||||
"slave_id": 11
|
||||
},
|
||||
{
|
||||
"name": "small",
|
||||
"host": "192.168.70.65",
|
||||
"slave_id": 22
|
||||
}
|
||||
]
|
||||
|
|
@ -2,23 +2,19 @@ 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)
|
||||
|
||||
with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
|
||||
s.bind((HOST, PORT))
|
||||
s.listen()
|
||||
conn, addr = s.accept()
|
||||
with conn:
|
||||
def handle_client(conn, addr):
|
||||
print(f"Connected by {addr}")
|
||||
while True:
|
||||
try:
|
||||
while True:
|
||||
data = conn.recv(1024)
|
||||
if data:
|
||||
if not data:
|
||||
break
|
||||
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"]:
|
||||
|
|
@ -34,5 +30,21 @@ with socket.socket(socket.AF_INET, socket.SOCK_STREAM) as s:
|
|||
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"by {addr} error {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()
|
||||
print(f"Server listening on {HOST}:{PORT}")
|
||||
while True:
|
||||
conn, addr = s.accept()
|
||||
handle_client(conn, addr)
|
||||
|
|
|
|||
Loading…
Reference in New Issue