add looptiming

urosc.py

@@ -18,7 +18,11 @@ clients = []
 rtde_r=None
 server=None
 
-
+#Looptimings
+frequency_loop=500
+frequency_oscsend=50
+frequency_rtde=None 
+frequency_scr=30
 
 def toRad(d):
     return d/360*2*math.pi
@@ -75,12 +79,6 @@ joint_max=[toRad(360),toRad(360),toRad(360),toRad(360),toRad(360),toRad(360)]
 
 
 def main(stdscr):
-    """Main entry point allowing external calls
-
-    Args:
-      args ([str]): command line parameter list
-    """
-
     # Clear and refresh the screen for a blank canvas
     stdscr.clear()
     stdscr.refresh()
@@ -99,9 +97,8 @@ def main(stdscr):
     
     height, width = stdscr.getmaxyx()
 
-    scrfrequency=30
     
-    dt = 1 / scrfrequency
+    loop_dt = 1 / frequency_loop
     
     
     
@@ -110,13 +107,23 @@ def main(stdscr):
     loopdurations = np.zeros(100)
     loopdurations_pos=0
 
-    try:
-        while key != ord('q'):
-            key = stdscr.getch()
-            stdscr.clear()
-            start = time.time()
+    last_sendosc=0
+    last_receivertde=0
+    last_updatescr=0
 
     q=[0,0,0,0,0,0]
+    
+    try:
+        while key != ord('q'):
+            looptime=time.time()
+            
+            start = time.time()
+            
+
+            #Get robot joint angled
+            if looptime-last_receivertde>=1.0/frequency_rtde:
+                last_receivertde=looptime
+
                 if rtde_r is not None:
                     q=rtde_r.getActualQ()
                 else:
@@ -127,16 +134,22 @@ def main(stdscr):
                         q[_i]=(math.sin(time.time()*_factors[_i]+_offset)/2.0+0.5)*(joint_max[_i]-joint_min[_i])+joint_min[_i]
 
 
+            #send data to all osc clients
+            if looptime-last_sendosc>=1.0/frequency_oscsend:
+                last_sendosc=looptime
                 for c in clients:                
-            
-                
                     for jointi in range(6):
                         c.send_message("/j"+str(jointi), toDeg(q[jointi]))
 
-            stdscr.addstr(0, 0, "loopcounter="+str(loopcounter), curses.color_pair(1))
-            stdscr.addstr(1, 0, "loop usage="+str(round(np.max(loopdurations)/dt,3)), curses.color_pair(1))
 
+            #Display
+            if looptime-last_updatescr>=1.0/frequency_scr:
+                last_updatescr=looptime
+                key = stdscr.getch()
+                stdscr.clear()
 
+                stdscr.addstr(0, 0, "loopcounter="+str(loopcounter), curses.color_pair(3))
+                stdscr.addstr(1, 0, "loop usage="+str(round(np.mean(loopdurations)/loop_dt,3)), curses.color_pair(1))
                 
                 startbarsx=10
                 endbarsx=width-1
@@ -167,20 +180,20 @@ def main(stdscr):
                 # Refresh the screen
                 stdscr.refresh()
 
-
+            #Looptiming
             end = time.time()
             loopduration = end - start
             loopdurations[loopdurations_pos]=loopduration
             loopdurations_pos+=1
             loopdurations_pos%=np.size(loopdurations)
 
-            if loopduration < dt:
-                time.sleep(dt - loopduration)
+            if loopduration < loop_dt:
+                time.sleep(loop_dt - loopduration)
             loopcounter += 1
 
     except KeyboardInterrupt:
         
-        print("\nData recording stopped.")
+        print("\nKeyboardInterrupt")
     curses.endwin()
         
 
@@ -213,6 +226,8 @@ if __name__ == "__main__":
         metavar="<frequency>")
     args = parser.parse_args()
 
+    frequency_rtde=args.frequency
+
     clientips=[args.clientip]
  
     
@@ -221,7 +236,7 @@ if __name__ == "__main__":
 
         clients=setupClients(clientips,args.port)
 
-        rtde_r=setupRTDE(args.robotip,args.frequency)
+        rtde_r=setupRTDE(args.robotip,frequency_rtde)
 
     
     curses.wrapper(main)