Added gui to echo server, and adding visualision or robot and movements.
This commit is contained in:
parent
eaab4691cd
commit
08792f8182
@ -1,36 +1,87 @@
|
||||
import time
|
||||
import threading
|
||||
from RSIPI.rsi_api import RSIAPI
|
||||
from RSIPI.trajectory_planner import generate_trajectory
|
||||
from RSIPI.rsi_graphing import plot_csv_file
|
||||
|
||||
|
||||
def generate_pulsed_trajectory(total_mm=1.0, step_mm=0.01):
|
||||
"""
|
||||
Creates a pulsed trajectory that moves a total distance by sending small RKorr deltas
|
||||
followed by resets to zero.
|
||||
"""
|
||||
steps = int(total_mm / step_mm)
|
||||
trajectory = []
|
||||
for _ in range(steps):
|
||||
trajectory.append({"X": step_mm}) # Pulse
|
||||
trajectory.append({"X": 0.0}) # Reset
|
||||
return trajectory
|
||||
|
||||
|
||||
def feed_pulsed_trajectory(api, trajectory):
|
||||
"""
|
||||
Sends each RKorr step synchronised with IPOC changes.
|
||||
"""
|
||||
stop_event = threading.Event()
|
||||
|
||||
def feeder():
|
||||
last_ipoc = api.get_ipoc()
|
||||
index = 0
|
||||
|
||||
while index < len(trajectory) and not stop_event.is_set():
|
||||
current_ipoc = api.get_ipoc()
|
||||
|
||||
if current_ipoc != last_ipoc:
|
||||
last_ipoc = current_ipoc
|
||||
val = trajectory[index].get("X", 0.0)
|
||||
api.update_cartesian(X=val)
|
||||
index += 1
|
||||
|
||||
time.sleep(0.0005)
|
||||
|
||||
stop_event.set()
|
||||
|
||||
thread = threading.Thread(target=feeder)
|
||||
thread.start()
|
||||
return stop_event, thread
|
||||
|
||||
|
||||
def reset_rkorr(api):
|
||||
for axis in ["X", "Y", "Z", "A", "B", "C"]:
|
||||
api.update_variable(f"RKorr.{axis}", 0.0)
|
||||
|
||||
|
||||
def main():
|
||||
api = RSIAPI("RSI_EthernetConfig.xml")
|
||||
|
||||
print("⚙️ Starting RSI...")
|
||||
api.start_rsi()
|
||||
time.sleep(1.0)
|
||||
|
||||
print("📦 Starting CSV log...")
|
||||
api.start_logging() # Automatically creates a timestamped file
|
||||
# Create trajectories
|
||||
forward = generate_pulsed_trajectory(total_mm=500.0, step_mm=0.01)
|
||||
backward = generate_pulsed_trajectory(total_mm=-500.0, step_mm=-0.01)
|
||||
|
||||
time.sleep(1.0) # Let handshake and logging settle
|
||||
loop_count = 5
|
||||
print(f"🔁 Starting looped motion (±1 mm), {loop_count} cycles")
|
||||
|
||||
# ✅ Send a few small RKorr pulses
|
||||
print("🔁 Sending RKorr pulses...")
|
||||
for _ in range(5):
|
||||
api.update_variable("RKorr.X", 0.5)
|
||||
time.sleep(0.008) # 2 cycles
|
||||
api.update_variable("RKorr.X", 0.0)
|
||||
time.sleep(0.008)
|
||||
for i in range(loop_count):
|
||||
print(f"➡️ Loop {i+1}: Forward")
|
||||
fwd_done, _ = feed_pulsed_trajectory(api, forward)
|
||||
while not fwd_done.is_set():
|
||||
time.sleep(0.005)
|
||||
|
||||
print("🛑 Stopping CSV log...")
|
||||
api.stop_logging()
|
||||
reset_rkorr(api)
|
||||
|
||||
print("🛑 Stopping RSI...")
|
||||
api.stop_rsi()
|
||||
print(f"⬅️ Loop {i+1}: Backward")
|
||||
bwd_done, _ = feed_pulsed_trajectory(api, backward)
|
||||
while not bwd_done.is_set():
|
||||
time.sleep(0.005)
|
||||
|
||||
print("✅ Log complete. You can now visualise it with rsi_graphing or plot_csv_file().")
|
||||
reset_rkorr(api)
|
||||
|
||||
print("✅ Motion complete. RSI still running.")
|
||||
|
||||
print("✅ Motion complete. RSI still running. Waiting for operator or Ctrl+C...")
|
||||
while True:
|
||||
time.sleep(1)
|
||||
|
||||
if __name__ == "__main__":
|
||||
main()
|
||||
|
||||
@ -1,11 +1,11 @@
|
||||
|
||||
import socket
|
||||
import time
|
||||
import xml.etree.ElementTree as ET
|
||||
import logging
|
||||
import threading
|
||||
from rsi_config import RSIConfig
|
||||
from src.RSIPI.rsi_config import RSIConfig
|
||||
|
||||
# ✅ Configure Logging
|
||||
LOGGING_ENABLED = True
|
||||
|
||||
if LOGGING_ENABLED:
|
||||
@ -17,9 +17,7 @@ if LOGGING_ENABLED:
|
||||
)
|
||||
|
||||
class EchoServer:
|
||||
"""A UDP echo server that sends test messages to the RSI client and logs received messages."""
|
||||
|
||||
def __init__(self, config_file, delay_ms=4):
|
||||
def __init__(self, config_file, delay_ms=4, mode="relative"):
|
||||
self.config = RSIConfig(config_file)
|
||||
network_settings = self.config.get_network_settings()
|
||||
|
||||
@ -27,107 +25,114 @@ class EchoServer:
|
||||
self.client_address = ("127.0.0.1", network_settings["port"])
|
||||
self.udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
|
||||
self.udp_socket.bind(self.server_address)
|
||||
|
||||
self.last_received = None
|
||||
self.ipoc_value = 123456
|
||||
self.delay_ms = delay_ms / 1000 # Convert milliseconds to seconds
|
||||
self.delay_ms = delay_ms / 1000
|
||||
self.mode = mode.lower() # relative or absolute
|
||||
|
||||
self.send_variables = self.config.get_send_variables()
|
||||
self.receive_variables = self.config.get_receive_variables()
|
||||
# Internal simulation state
|
||||
self.state = {
|
||||
"RIst": {k: 0.0 for k in ["X", "Y", "Z", "A", "B", "C"]},
|
||||
"AIPos": {f"A{i}": 0.0 for i in range(1, 7)},
|
||||
"ELPos": {f"E{i}": 0.0 for i in range(1, 7)},
|
||||
"DiO": 0,
|
||||
"DiL": 0
|
||||
}
|
||||
|
||||
self.running = True # ✅ Control flag for graceful shutdown
|
||||
self.running = True
|
||||
self.thread = threading.Thread(target=self.send_message, daemon=True)
|
||||
|
||||
logging.info(f"✅ Echo Server started on {self.server_address}")
|
||||
logging.info(f"📡 Sending messages to RSI Client on port {network_settings['port']}")
|
||||
logging.info(f"⏳ Message delay set to {delay_ms}ms")
|
||||
print("✅ Echo Server Started")
|
||||
print(f"📡 Sending messages to RSI Client on port {network_settings['port']}")
|
||||
print(f"✅ Echo Server started in {self.mode.upper()} mode.")
|
||||
|
||||
def receive_and_process(self):
|
||||
try:
|
||||
self.udp_socket.settimeout(self.delay_ms)
|
||||
data, addr = self.udp_socket.recvfrom(1024)
|
||||
xml_string = data.decode()
|
||||
root = ET.fromstring(xml_string)
|
||||
self.last_received = xml_string # ✅ Save for GUI
|
||||
|
||||
for elem in root:
|
||||
tag = elem.tag
|
||||
if tag in ["RKorr", "AKorr"]:
|
||||
for axis, value in elem.attrib.items():
|
||||
value = float(value)
|
||||
if tag == "RKorr" and axis in self.state["RIst"]:
|
||||
if self.mode == "relative":
|
||||
self.state["RIst"][axis] += value
|
||||
else:
|
||||
self.state["RIst"][axis] = value
|
||||
elif tag == "AKorr" and axis in self.state["AIPos"]:
|
||||
if self.mode == "relative":
|
||||
self.state["AIPos"][axis] += value
|
||||
else:
|
||||
self.state["AIPos"][axis] = value
|
||||
elif tag in ["DiO", "DiL"]:
|
||||
if tag in self.state:
|
||||
self.state[tag] = int(elem.text.strip())
|
||||
elif tag == "IPOC":
|
||||
self.ipoc_value = int(elem.text.strip())
|
||||
|
||||
logging.debug(f"✅ Processed input: {ET.tostring(root).decode()}")
|
||||
except socket.timeout:
|
||||
pass # Just ignore and continue
|
||||
except ConnectionResetError:
|
||||
print("⚠️ Connection was reset by client. Waiting before retry...")
|
||||
time.sleep(0.5)
|
||||
except Exception as e:
|
||||
print(f"[ERROR] Failed to process input: {e}")
|
||||
|
||||
def generate_message(self):
|
||||
"""Generate a properly formatted RSI message dynamically based on the config file."""
|
||||
root = ET.Element("Rob", Type="KUKA") # ✅ Correct root element
|
||||
root = ET.Element("Rob", Type="KUKA")
|
||||
|
||||
for tag, value in self.send_variables.items():
|
||||
if isinstance(value, dict):
|
||||
element = ET.SubElement(root, tag.split(".")[0]) # Create main element
|
||||
for sub_key, sub_value in value.items():
|
||||
if isinstance(sub_value, bool):
|
||||
sub_value = int(sub_value) # Convert booleans to 0/1
|
||||
element.set(sub_key, f"{sub_value:.1f}" if isinstance(sub_value, float) else str(sub_value))
|
||||
else:
|
||||
element = ET.SubElement(root, tag)
|
||||
if isinstance(value, bool):
|
||||
value = int(value) # Convert booleans to 0/1
|
||||
element.text = f"{value:.1f}" if isinstance(value, float) else str(value)
|
||||
for key in ["RIst", "AIPos", "ELPos"]:
|
||||
element = ET.SubElement(root, key)
|
||||
for sub_key, value in self.state[key].items():
|
||||
element.set(sub_key, f"{value:.2f}")
|
||||
|
||||
for key in ["DiO", "DiL"]:
|
||||
ET.SubElement(root, key).text = str(self.state[key])
|
||||
|
||||
ET.SubElement(root, "IPOC").text = str(self.ipoc_value)
|
||||
|
||||
message = ET.tostring(root, encoding="utf-8").decode()
|
||||
logging.debug(f"📤 Sent XML Message:\n{message}")
|
||||
print(f"📤 Sent XML Message:\n{message}")
|
||||
return message
|
||||
return ET.tostring(root, encoding="utf-8").decode()
|
||||
|
||||
def send_message(self):
|
||||
"""Send messages every 4ms or 12ms without waiting for a response."""
|
||||
while self.running:
|
||||
try:
|
||||
message = self.generate_message()
|
||||
print(f"Sending message:\n{message}")
|
||||
logging.debug(f"📡 Sending message to RSI client with IPOC={self.ipoc_value}...")
|
||||
print(f"📡 Sending message to RSI client with IPOC={self.ipoc_value}...")
|
||||
self.udp_socket.sendto(message.encode(), self.client_address) # ✅ Send without waiting
|
||||
|
||||
# ✅ Attempt to receive a response
|
||||
self.udp_socket.settimeout(self.delay_ms)
|
||||
try:
|
||||
response, addr = self.udp_socket.recvfrom(1024)
|
||||
received_message = response.decode()
|
||||
|
||||
logging.debug(f"📩 Received XML Message:\n{received_message}")
|
||||
print(f"📩 Received XML Message:\n{received_message}")
|
||||
|
||||
except socket.timeout:
|
||||
print("⚠️ No response received before next send cycle.")
|
||||
|
||||
time.sleep(self.delay_ms) # ✅ Apply user-defined delay (4ms or 12ms)
|
||||
self.increment_ipoc() # ✅ Increment IPOC for next message
|
||||
|
||||
except ConnectionResetError:
|
||||
logging.warning("⚠️ RSI Client is not responding. Retrying...")
|
||||
print("⚠️ RSI Client is not responding. Retrying in 2 seconds...")
|
||||
time.sleep(2) # ✅ Instead of crashing, wait and retry
|
||||
|
||||
self.receive_and_process()
|
||||
response = self.generate_message()
|
||||
self.udp_socket.sendto(response.encode(), self.client_address)
|
||||
self.ipoc_value += 4
|
||||
time.sleep(self.delay_ms)
|
||||
except Exception as e:
|
||||
logging.error(f"❌ Unexpected error: {e}")
|
||||
print(f"❌ Unexpected error: {e}")
|
||||
|
||||
def increment_ipoc(self):
|
||||
"""Increment the IPOC value by 4 milliseconds for the next message."""
|
||||
self.ipoc_value += 4
|
||||
logging.debug(f"🔄 IPOC incremented to {self.ipoc_value}")
|
||||
print(f"🔄 IPOC incremented to {self.ipoc_value}")
|
||||
print(f"[ERROR] EchoServer error: {e}")
|
||||
time.sleep(1)
|
||||
|
||||
def start(self):
|
||||
"""Start the Echo Server message loop in a separate thread."""
|
||||
self.running = True
|
||||
self.thread.start()
|
||||
|
||||
def stop(self):
|
||||
"""Gracefully stop the Echo Server."""
|
||||
print("🛑 Stopping Echo Server...")
|
||||
logging.info("🛑 Stopping Echo Server...")
|
||||
self.running = False # ✅ Stop the loop
|
||||
self.thread.join() # ✅ Wait for the thread to finish
|
||||
self.udp_socket.close() # ✅ Close the socket
|
||||
logging.info("✅ Echo Server Stopped")
|
||||
print("✅ Echo Server Stopped")
|
||||
self.running = False
|
||||
self.thread.join()
|
||||
self.udp_socket.close()
|
||||
print("✅ Echo Server Stopped.")
|
||||
|
||||
if __name__ == "__main__":
|
||||
config_file = "RSI_EthernetConfig.xml"
|
||||
echo_server = EchoServer(config_file, delay_ms=4)
|
||||
import argparse
|
||||
parser = argparse.ArgumentParser(description="Run Echo Server for RSI Simulation")
|
||||
parser.add_argument("--config", type=str, default="RSI_EthernetConfig.xml", help="Path to RSI config file")
|
||||
parser.add_argument("--mode", type=str, choices=["relative", "absolute"], default="relative", help="Correction mode")
|
||||
parser.add_argument("--delay", type=int, default=4, help="Delay between messages in ms")
|
||||
|
||||
args = parser.parse_args()
|
||||
server = EchoServer(config_file=args.config, delay_ms=args.delay, mode=args.mode)
|
||||
|
||||
try:
|
||||
echo_server.start()
|
||||
server.start()
|
||||
while True:
|
||||
time.sleep(1) # Keep main thread alive
|
||||
time.sleep(1)
|
||||
except KeyboardInterrupt:
|
||||
echo_server.stop()
|
||||
server.stop()
|
||||
|
||||
Loading…
Reference in New Issue
Block a user