add console q view

tests/curses_actualq.py

@@ -0,0 +1,153 @@
+from rtde_receive import RTDEReceiveInterface as RTDEReceive
+import time
+import argparse
+import sys
+import curses
+import math
+
+
+def toRad(d):
+    return d/360*2*math.pi
+
+def toDeg(r):
+    return r*360/2/math.pi
+
+
+def mapFromTo(x,a,b,c,d):
+   y=(x-a)/(b-a)*(d-c)+c
+   return y
+
+
+def parse_args(args):
+    """Parse command line parameters
+
+    Args:
+      args ([str]): command line parameters as list of strings
+
+    Returns:
+      :obj:`argparse.Namespace`: command line parameters namespace
+    """
+    parser = argparse.ArgumentParser(
+        description="Record data example")
+    parser.add_argument(
+        "-ip",
+        "--robot_ip",
+        dest="ip",
+        help="IP address of the UR robot",
+        type=str,
+        default='localhost',
+        metavar="<IP address of the UR robot>")
+    parser.add_argument(
+        "-o",
+        "--output",
+        dest="output",
+        help="data output (.csv) file to write to (default is \"robot_data.csv\"",
+        type=str,
+        default="robot_data.csv",
+        metavar="<data output file>")
+    parser.add_argument(
+        "-f",
+        "--frequency",
+        dest="frequency",
+        help="the frequency at which the data is recorded (default is 500Hz)",
+        type=float,
+        default=500.0,
+        metavar="<frequency>")
+
+    return parser.parse_args(args)
+
+joint_min=[toRad(-90),toRad(-90-40),toRad(90-90),toRad(-180-80),toRad(-90-90),toRad(0-75)]
+joint_max=[toRad(90),toRad(-90+40),toRad(90+40),toRad(-180+110),toRad(-90+90),toRad(0+75)]
+
+
+
+def main(stdscr):
+    args=sys.argv[1:]
+    """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()
+
+    curses.cbreak()
+    stdscr.keypad(1)
+    stdscr.nodelay(1)
+
+    
+    # Start colors in curses
+    curses.start_color()
+    curses.init_pair(1, curses.COLOR_CYAN, curses.COLOR_BLACK)
+    curses.init_pair(2, curses.COLOR_RED, curses.COLOR_BLACK)
+    curses.init_pair(3, curses.COLOR_BLACK, curses.COLOR_WHITE)
+
+    
+    height, width = stdscr.getmaxyx()
+    
+
+    args = parse_args(args)
+    dt = 1 / args.frequency
+    rtde_r = RTDEReceive(args.ip, args.frequency)
+    
+    
+    key = ''
+    i = 0
+    try:
+        while key != ord('q'):
+            key = stdscr.getch()
+            stdscr.clear()
+            start = time.time()
+            
+            q=rtde_r.getActualQ()
+            #print(q)
+            #sys.stdout.write("\r")
+            #sys.stdout.write("{:3d} samples.".format(i))
+            #sys.stdout.flush()
+
+            stdscr.addstr(0, 0, "test "+str(i), curses.color_pair(1))
+
+
+            for j in range(6):
+                posx=mapFromTo(q[j],joint_min[j],joint_max[j],0,width) #scale to screen width
+                for px in range(width):
+                    diff=abs(posx-px)
+                    poschar=''
+                    if diff<3:
+                        poschar='.'
+                    if diff<2:
+                        poschar='i'
+                    if diff<0.5:
+                        poschar='|'
+                        
+                    if (len(poschar)>0):
+                        stdscr.addstr(2+j, px, poschar, curses.color_pair(1))
+                valuetext=str(round(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()
+            duration = end - start
+
+            if duration < dt:
+                time.sleep(dt - duration)
+            i += 1
+
+    except KeyboardInterrupt:
+        
+        print("\nData recording stopped.")
+        curses.endwin()
+        exit()
+
+
+if __name__ == "__main__":
+    curses.wrapper(main)
+    

tests/cursestest.py

@@ -0,0 +1,92 @@
+import sys,os
+import curses
+
+def draw_menu(stdscr):
+    k = 0
+    cursor_x = 0
+    cursor_y = 0
+
+    # Clear and refresh the screen for a blank canvas
+    stdscr.clear()
+    stdscr.refresh()
+
+    # Start colors in curses
+    curses.start_color()
+    curses.init_pair(1, curses.COLOR_CYAN, curses.COLOR_BLACK)
+    curses.init_pair(2, curses.COLOR_RED, curses.COLOR_BLACK)
+    curses.init_pair(3, curses.COLOR_BLACK, curses.COLOR_WHITE)
+
+    # Loop where k is the last character pressed
+    while (k != ord('q')):
+
+        # Initialization
+        stdscr.clear()
+        height, width = stdscr.getmaxyx()
+
+        if k == curses.KEY_DOWN:
+            cursor_y = cursor_y + 1
+        elif k == curses.KEY_UP:
+            cursor_y = cursor_y - 1
+        elif k == curses.KEY_RIGHT:
+            cursor_x = cursor_x + 1
+        elif k == curses.KEY_LEFT:
+            cursor_x = cursor_x - 1
+
+        cursor_x = max(0, cursor_x)
+        cursor_x = min(width-1, cursor_x)
+
+        cursor_y = max(0, cursor_y)
+        cursor_y = min(height-1, cursor_y)
+
+        # Declaration of strings
+        title = "Curses example"[:width-1]
+        subtitle = "Written by Clay McLeod"[:width-1]
+        keystr = "Last key pressed: {}".format(k)[:width-1]
+        statusbarstr = "Press 'q' to exit | STATUS BAR | Pos: {}, {}".format(cursor_x, cursor_y)
+        if k == 0:
+            keystr = "No key press detected..."[:width-1]
+
+        # Centering calculations
+        start_x_title = int((width // 2) - (len(title) // 2) - len(title) % 2)
+        start_x_subtitle = int((width // 2) - (len(subtitle) // 2) - len(subtitle) % 2)
+        start_x_keystr = int((width // 2) - (len(keystr) // 2) - len(keystr) % 2)
+        start_y = int((height // 2) - 2)
+
+        # Rendering some text
+        whstr = "Width: {}, Height: {}".format(width, height)
+        stdscr.addstr(0, 0, whstr, curses.color_pair(1))
+
+        # Render status bar
+        stdscr.attron(curses.color_pair(3))
+        stdscr.addstr(height-1, 0, statusbarstr)
+        stdscr.addstr(height-1, len(statusbarstr), " " * (width - len(statusbarstr) - 1))
+        stdscr.attroff(curses.color_pair(3))
+
+        # Turning on attributes for title
+        stdscr.attron(curses.color_pair(2))
+        stdscr.attron(curses.A_BOLD)
+
+        # Rendering title
+        stdscr.addstr(start_y, start_x_title, title)
+
+        # Turning off attributes for title
+        stdscr.attroff(curses.color_pair(2))
+        stdscr.attroff(curses.A_BOLD)
+
+        # Print rest of text
+        stdscr.addstr(start_y + 1, start_x_subtitle, subtitle)
+        stdscr.addstr(start_y + 3, (width // 2) - 2, '-' * 4)
+        stdscr.addstr(start_y + 5, start_x_keystr, keystr)
+        stdscr.move(cursor_y, cursor_x)
+
+        # Refresh the screen
+        stdscr.refresh()
+
+        # Wait for next input
+        k = stdscr.getch()
+
+def main():
+    curses.wrapper(draw_menu)
+
+if __name__ == "__main__":
+    main()