commit 165b17efeca9702002de8ba09d0c909a6b37eb8d
author Maxwell Henderson <mxwhenderson@gmail.com> Thu Mar 09 17:14:04 2023 -0800
committer Maxwell Henderson <mxwhenderson@gmail.com> Thu Mar 09 18:08:20 2023 -0800
tree 882ad605dc1b8caac205768c143e8745e822d514
parent 1944234b4dc8093eb57b921f0510a4fea528b33d

Add indicators for control points in ArmUI

Signed-off-by: Maxwell Henderson <mxwhenderson@gmail.com>
Change-Id: Iccf9046682d38cfbdb87859e6a760e5c0cd9d9e3

y2023/control_loops/python/graph_edit.py

diff --git a/y2023/control_loops/python/graph_edit.py b/y2023/control_loops/python/graph_edit.py
index f18a0b6..3568b15 100644
--- a/y2023/control_loops/python/graph_edit.py
+++ b/y2023/control_loops/python/graph_edit.py
@@ -13,7 +13,7 @@
 gi.require_version('Gtk', '3.0')
 from gi.repository import Gdk, Gtk
 import cairo
-from y2023.control_loops.python.graph_tools import to_theta, to_xy, alpha_blend, shift_angles
+from y2023.control_loops.python.graph_tools import to_theta, to_xy, alpha_blend, shift_angles, get_xy
 from y2023.control_loops.python.graph_tools import l1, l2, joint_center
 from y2023.control_loops.python.graph_tools import DRIVER_CAM_POINTS
 from y2023.control_loops.python import graph_paths
@@ -378,6 +378,7 @@
             random.shuffle(color)
             set_color(cr, Color(color[0], color[1], color[2]))
             self.segments[i].DrawTo(cr, self.theta_version)
+
             with px(cr):
                 cr.stroke()
 
@@ -385,6 +386,19 @@
         color = [0, 0, 0]
         set_color(cr, Color(color[0], color[1], color[2]))
         self.segments[self.index].DrawTo(cr, self.theta_version)
+
+        control1 = get_xy(self.segments[self.index].control1)
+        control2 = get_xy(self.segments[self.index].control2)
+
+        if self.theta_version:
+            control1 = shift_angles(self.segments[self.index].control1)
+            control2 = shift_angles(self.segments[self.index].control2)
+
+        cr.move_to(control1[0] + 0.02, control1[1])
+        cr.arc(control1[0], control1[1], 0.02, 0, 2.0 * np.pi)
+        cr.move_to(control2[0] + 0.02, control2[1])
+        cr.arc(control2[0], control2[1], 0.02, 0, 2.0 * np.pi)
+
         with px(cr):
             cr.stroke()