thumb_viz.py

Go to the documentation of this file.

"""
Reads data from a serial port and displays it in real-time using matplotlib.
Make sure to set COM_PORT to the correct value before running.
"""
 
import serial
from rendering import PointsInSpace
 
COM_PORT = "COM11"
TRAILING_POINTS = 16
MIN_MESSAGE_BYTES = 16
 
ser = serial.Serial(
    port=COM_PORT,
    baudrate=115200,
    parity=serial.PARITY_NONE,
    stopbits=serial.STOPBITS_ONE,
    bytesize=serial.EIGHTBITS,
    timeout=0,
)
 
print("Connected to: " + ser.portstr)
 
joint_0_list = []
joint_1_list = []
joint_0_mirror_list = []
joint_1_mirror_list = []
 
pp = PointsInSpace(
    "Tip",
    "Joint 0 [Radians]",
    "Joint 1 [Radians]",
    xlim=[-2, 2],
    enable_grid=True,
    enable_legend=True,
)
pp.register_plot("connected", alpha=0.4)
pp.register_plot("mirror", alpha=0.4)
pp.register_plot("torque")
 
while True:
    try:
        # Read bytes goofery
        bytes_to_read = ser.in_waiting
        if bytes_to_read < MIN_MESSAGE_BYTES:
            continue
        line = ser.read(bytes_to_read).decode("utf-8")
        segments = line.split()
 
        # Parse the message by reading the value after each label
        try:
            # Do this in two steps so that values are not changed if not all values exist
            def value_by_label(label):
                return float(segments[segments.index(f"{label}:") + 1])
 
            joint_0 = value_by_label("joint_thetas.at(0)")
            joint_1 = value_by_label("joint_thetas.at(1)")
            joint_0_mirror = value_by_label("mirror_pos_0")
            joint_1_mirror = value_by_label("mirror_pos_2")
            
            joint_0_torque = 0
            joint_1_torque = 0
            try:
                joint_0_torque = value_by_label("joint_0_torque")
                joint_1_torque = value_by_label("joint_1_torque")
            except Exception:
                pass
            
            # print(joint_1_mirror / joint_0_mirror)
 
            joint_0_list.append(joint_0)
            joint_1_list.append(joint_1)
            joint_0_mirror_list.append(joint_0_mirror)
            joint_1_mirror_list.append(joint_1_mirror)
        except Exception as e:
            print(e)
            continue
 
        if len(joint_0_list) > TRAILING_POINTS:
            joint_0_list.pop(0)
            joint_1_list.pop(0)
            joint_0_mirror_list.pop(0)
            joint_1_mirror_list.pop(0)
 
        # Display results
        pp.start_drawing()
        pp.draw_points("connected", joint_0_list, joint_1_list)
        pp.draw_points("mirror", joint_0_mirror_list, joint_1_mirror_list)
        pp.draw_points("torque", joint_0_torque, joint_1_torque)
        pp.end_drawing()
    except Exception as e:
        print(e)
        ser.close()
        print("Closed connection")
        quit()
 
ser.close()
print("Closed connection")