159 lines
3.4 KiB
Python
159 lines
3.4 KiB
Python
import argparse
|
|
|
|
|
|
import time
|
|
import math
|
|
import numpy as np
|
|
import curses
|
|
|
|
|
|
|
|
from helpfunctions import *
|
|
from ui import UI
|
|
from oscsend import OSCSend
|
|
from oscreceive import OSCReceive
|
|
from ur import UR
|
|
|
|
|
|
|
|
#Looptimings
|
|
frequency_loop=500
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
joint_min=[toRad(-360),toRad(-360),toRad(-360),toRad(-360),toRad(-360),toRad(0)]
|
|
joint_max=[toRad(360),toRad(360),toRad(360),toRad(360),toRad(360),toRad(360)]
|
|
|
|
|
|
ui = UI(joint_min,joint_max)
|
|
|
|
oscsend = OSCSend()
|
|
oscreceive = None
|
|
ur = UR(joint_min,joint_max)
|
|
|
|
|
|
def main(stdscr):
|
|
|
|
ui.begin(stdscr)
|
|
|
|
|
|
loop_dt = 1 / frequency_loop
|
|
|
|
|
|
|
|
|
|
loopcounter = 0
|
|
loopdurations = np.zeros(100)
|
|
loopdurations_pos=0
|
|
|
|
|
|
|
|
try:
|
|
while ui.getKey() != ord('q'):
|
|
looptime = time.time()
|
|
|
|
start = time.time()
|
|
|
|
ur.receive(looptime)
|
|
|
|
#send data to all osc receivers
|
|
oscsend.send(looptime,ur.getReceivedQ())
|
|
|
|
if ur is not None:
|
|
ur.send(looptime)
|
|
|
|
#Display
|
|
ui.update(ur.getReceivedQ(),looptime)
|
|
|
|
#Looptiming
|
|
end = time.time()
|
|
loopduration = end - start
|
|
loopdurations[loopdurations_pos]=loopduration
|
|
loopdurations_pos+=1
|
|
loopdurations_pos%=np.size(loopdurations)
|
|
|
|
ui.setDebugtext(0,"loopcounter="+str(loopcounter))
|
|
ui.setDebugtext(1,"loop usage="+str(round(np.mean(loopdurations)/loop_dt,3)))
|
|
if oscreceive is not None:
|
|
ui.setDebugtext(2,"osc receive frequency="+str(round(oscreceive.getCurrentReceiveFrequency(),0)))
|
|
|
|
|
|
|
|
|
|
if loopduration < loop_dt:
|
|
time.sleep(loop_dt - loopduration)
|
|
loopcounter += 1
|
|
|
|
except KeyboardInterrupt:
|
|
|
|
print("\nKeyboardInterrupt")
|
|
curses.endwin()
|
|
|
|
|
|
if __name__ == "__main__":
|
|
parser = argparse.ArgumentParser()
|
|
parser.add_argument("--listenip",
|
|
default="127.0.0.1", help="The ip to listen on")
|
|
parser.add_argument("--listenport",
|
|
type=int, default=5005, help="The port to listen on")
|
|
|
|
parser.add_argument("--clientip",
|
|
default="127.0.0.1", help="The ip to connect to")
|
|
parser.add_argument("--port",
|
|
type=int, default=5005, help="The port to connect to")
|
|
|
|
|
|
parser.add_argument("--robotip",
|
|
default="127.0.0.1", help="The ip to connect to")
|
|
|
|
parser.add_argument("--test", action='store_true', default=False, help="Disable network and robot communication")
|
|
|
|
|
|
'''
|
|
parser.add_argument(
|
|
"-fs",
|
|
"--frequencysend",
|
|
dest="frequencysend",
|
|
help="the frequency at which the robot control data is send",
|
|
type=float,
|
|
default=500.0,
|
|
metavar="<frequency>")
|
|
|
|
parser.add_argument(
|
|
"-fr",
|
|
"--frequencyreceive",
|
|
dest="frequencyreceive",
|
|
help="the frequency at which the robot actualq data is received",
|
|
type=float,
|
|
default=500.0,
|
|
metavar="<frequency>")
|
|
'''
|
|
|
|
args = parser.parse_args()
|
|
|
|
|
|
|
|
|
|
|
|
if args.test is not True:
|
|
ur.connect(args.robotip)
|
|
|
|
oscreceive = OSCReceive(args.listenip, args.listenport,ur)
|
|
|
|
oscsend.setupReceivers([args.clientip],args.port)
|
|
|
|
|
|
|
|
|
|
curses.wrapper(main)
|
|
|
|
|
|
if oscreceive is not None:
|
|
oscreceive.disconnect()
|
|
if ur is not None:
|
|
ur.disconnect()
|
|
|