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,78 +107,94 @@ def main(stdscr):
     loopdurations = np.zeros(100)
     loopdurations_pos=0
 
+    last_sendosc=0
+    last_receivertde=0
+    last_updatescr=0
+
+    q=[0,0,0,0,0,0]
+    
     try:
         while key != ord('q'):
-            key = stdscr.getch()
+            looptime=time.time()
-            stdscr.clear()
+            
             start = time.time()
             
-            q=[0,0,0,0,0,0]
-            if rtde_r is not None:
-                q=rtde_r.getActualQ()
-            else:
-                _offsets=[12.2,5.3,0,6,12.4,612.91]
-                _factors=[1.2,1.14,1.214,0.9921,1,1.092]
 
-                for _i,_offset in enumerate(_offsets):
+            #Get robot joint angled
-                    q[_i]=(math.sin(time.time()*_factors[_i]+_offset)/2.0+0.5)*(joint_max[_i]-joint_min[_i])+joint_min[_i]
+            if looptime-last_receivertde>=1.0/frequency_rtde:
+                last_receivertde=looptime
+
+                if rtde_r is not None:
+                    q=rtde_r.getActualQ()
+                else:
+                    _offsets=[12.2,5.3,0,6,12.4,612.91]
+                    _factors=[1.2,1.14,1.214,0.9921,1,1.092]
+                    
+                    for _i,_offset in enumerate(_offsets):
+                        q[_i]=(math.sin(time.time()*_factors[_i]+_offset)/2.0+0.5)*(joint_max[_i]-joint_min[_i])+joint_min[_i]
 
 
-            for c in clients:
+            #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]))
 
 
-                for jointi in range(6):
+            #Display
-                    c.send_message("/j"+str(jointi), toDeg(q[jointi]))
+            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(1))
+                stdscr.addstr(0, 0, "loopcounter="+str(loopcounter), curses.color_pair(3))
-            stdscr.addstr(1, 0, "loop usage="+str(round(np.max(loopdurations)/dt,3)), curses.color_pair(1))
+                stdscr.addstr(1, 0, "loop usage="+str(round(np.mean(loopdurations)/loop_dt,3)), curses.color_pair(1))
+                
+                startbarsx=10
+                endbarsx=width-1
+                for j in range(6):
+                    posx=mapFromTo(q[j],joint_min[j],joint_max[j],startbarsx,endbarsx) #scale to screen width
+                    for px in range(width):
+                        diff=abs(posx-px)
+                        poschar=''
+                        
+                        if diff<3:
+                            poschar='-'
+                        if diff<0.5:
+                            poschar='|'
+
+                        if px==startbarsx:
+                            poschar="["
+                        if px==endbarsx:
+                            poschar="]"
+                            
+                        if (len(poschar)>0):
+                            stdscr.addstr(2+j, px, poschar, curses.color_pair(1))
+                    valuetext=str(round(toDeg(q[j]),2))
+                    if q[j]>=0: #has no negative sign
+                        valuetext=" "+valuetext
+                    stdscr.addstr(2+j, 0, valuetext, curses.color_pair(1))
 
                     
+                # Refresh the screen
+                stdscr.refresh()
 
-            startbarsx=10
+            #Looptiming
-            endbarsx=width-1
-            for j in range(6):
-                posx=mapFromTo(q[j],joint_min[j],joint_max[j],startbarsx,endbarsx) #scale to screen width
-                for px in range(width):
-                    diff=abs(posx-px)
-                    poschar=''
-                    
-                    if diff<3:
-                        poschar='-'
-                    if diff<0.5:
-                        poschar='|'
-
-                    if px==startbarsx:
-                        poschar="["
-                    if px==endbarsx:
-                        poschar="]"
-                        
-                    if (len(poschar)>0):
-                        stdscr.addstr(2+j, px, poschar, curses.color_pair(1))
-                valuetext=str(round(toDeg(q[j]),2))
-                if q[j]>=0: #has no negative sign
-                    valuetext=" "+valuetext
-                stdscr.addstr(2+j, 0, valuetext, curses.color_pair(1))
-
-                
-            # Refresh the screen
-            stdscr.refresh()
-
-
             end = time.time()
             loopduration = end - start
             loopdurations[loopdurations_pos]=loopduration
             loopdurations_pos+=1
             loopdurations_pos%=np.size(loopdurations)
 
-            if loopduration < dt:
+            if loopduration < loop_dt:
-                time.sleep(dt - loopduration)
+                time.sleep(loop_dt - loopduration)
             loopcounter += 1
 
     except KeyboardInterrupt:
         
-        print("\nData recording stopped.")
+        print("\nKeyboardInterrupt")
-        curses.endwin()
+    curses.endwin()
         
 
 if __name__ == "__main__": 
@@ -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)