Add a cursor to graph in Spline UI
This connects the graph to the robot that gets drawn along the spline.
As you move your mouse along the spline, you can see what part of the
graph it affects, and when you move your mouse along the graph, the
robot traces out the path of the spline.
This change lays the groundwork for being able to apply velocity limits
at specific points along the spline in the future.
Change-Id: I7d850f27fb25104c0b9d678b6701d5d74a11b05f
Signed-off-by: Ravago Jones <ravagojones@gmail.com>
diff --git a/frc971/control_loops/python/graph.py b/frc971/control_loops/python/graph.py
index 77288ea..f21b2ea 100644
--- a/frc971/control_loops/python/graph.py
+++ b/frc971/control_loops/python/graph.py
@@ -21,15 +21,75 @@
self.canvas = FigureCanvas(fig) # a Gtk.DrawingArea
self.canvas.set_vexpand(True)
self.canvas.set_size_request(800, 250)
+ self.callback_id = self.canvas.mpl_connect('motion_notify_event',
+ self.on_mouse_move)
self.add(self.canvas)
- self.queue = queue.Queue(maxsize=1)
+ # The current graph data
+ self.data = None
+ # The size of a timestep
+ self.dt = 0.00505
+ # The position of the cursor in seconds
+ self.cursor = 0
+
+ # Reference to the parent gtk Widget that wants to get redrawn
+ # when user moves the cursor
+ self.cursor_watcher = None
+ self.cursor_line = None
+
+ self.queue = queue.Queue(maxsize=1)
thread = threading.Thread(target=self.worker)
thread.daemon = True
thread.start()
+ def find_cursor(self):
+ """Gets the cursor position as a distance along the spline"""
+ if self.data is None:
+ return None
+ cursor_index = int(self.cursor / self.dt)
+ # use the time to index into the position data
+ distance_at_cursor = self.data[0][cursor_index - 1]
+ return distance_at_cursor
+
+ def place_cursor(self, distance):
+ """Places the cursor at a certain distance along the spline"""
+ if self.data is None:
+ return
+ # convert distance along spline to time along trajectory
+ index = np.searchsorted(self.data[0], distance, side='left')
+ time = index * self.dt
+ self.cursor = time
+ self.redraw_cursor()
+
+ def on_mouse_move(self, event):
+ """Updates the cursor and all the canvases that watch it on mouse move"""
+ if self.data is None:
+ return
+ total_steps_taken = self.data.shape[1]
+ total_time = self.dt * total_steps_taken
+ if event.xdata is not None:
+ # clip the position if still on the canvas, but off the graph
+ self.cursor = np.clip(event.xdata, 0, total_time)
+
+ self.redraw_cursor()
+
+ # tell the field to update too
+ if self.cursor_watcher is not None:
+ self.cursor_watcher.queue_draw()
+
+ def redraw_cursor(self):
+ """Redraws the cursor line"""
+ # TODO: This redraws the entire graph and isn't very snappy
+ if self.cursor_line: self.cursor_line.remove()
+ self.cursor_line = self.axis.axvline(self.cursor)
+ self.canvas.draw_idle()
+
def schedule_recalculate(self, points):
- if not points.getLibsplines() or self.queue.full(): return
+ """Submits points to be graphed
+
+ Can be superseded by newer points if an old one isn't finished processing.
+ """
+ if not points.getLibsplines(): return
new_copy = copy.deepcopy(points)
# empty the queue
@@ -47,30 +107,28 @@
def recalculate_graph(self, points):
if not points.getLibsplines(): return
- # set the size of a timestep
- dt = 0.00505
# call C++ wrappers to calculate the trajectory
- distanceSpline = DistanceSpline(points.getLibsplines())
- traj = Trajectory(distanceSpline)
+ distance_spline = DistanceSpline(points.getLibsplines())
+ traj = Trajectory(distance_spline)
points.addConstraintsToTrajectory(traj)
traj.Plan()
- XVA = traj.GetPlanXVA(dt)
- if XVA is None: return
+ self.data = traj.GetPlanXVA(self.dt)
+ if self.data is None: return
# extract values to be graphed
- total_steps_taken = XVA.shape[1]
- total_time = dt * total_steps_taken
- time = np.linspace(0, total_time, num=total_steps_taken)
- position, velocity, acceleration = XVA
+ total_steps_taken = self.data.shape[1]
+ total_time = self.dt * total_steps_taken
+ times = np.linspace(0, total_time, num=total_steps_taken)
+ position, velocity, acceleration = self.data
left_voltage, right_voltage = zip(*(traj.Voltage(x) for x in position))
# update graph
self.axis.clear()
- self.axis.plot(time, velocity)
- self.axis.plot(time, acceleration)
- self.axis.plot(time, left_voltage)
- self.axis.plot(time, right_voltage)
+ self.axis.plot(times, velocity)
+ self.axis.plot(times, acceleration)
+ self.axis.plot(times, left_voltage)
+ self.axis.plot(times, right_voltage)
self.axis.legend(
["Velocity", "Acceleration", "Left Voltage", "Right Voltage"])
self.axis.xaxis.set_label_text("Time (sec)")
@@ -80,4 +138,4 @@
self.axis.xaxis.set_ticks(np.linspace(0, total_time, num=8))
# redraw
- self.canvas.draw()
+ self.canvas.draw_idle()
diff --git a/frc971/control_loops/python/path_edit.py b/frc971/control_loops/python/path_edit.py
index 8573257..f0b84a4 100755
--- a/frc971/control_loops/python/path_edit.py
+++ b/frc971/control_loops/python/path_edit.py
@@ -36,6 +36,7 @@
self.points = Points()
self.graph = Graph()
+ self.graph.cursor_watcher = self
self.set_vexpand(True)
self.set_hexpand(True)
# list of multisplines
@@ -232,6 +233,14 @@
# if the mouse is close enough, draw the robot to show its width
if result and result.fun < 2:
self.draw_robot_at_point(cr, distance_spline, result.x)
+ self.graph.place_cursor(result.x[0])
+ elif self.graph.cursor:
+ x = self.graph.find_cursor()
+ self.draw_robot_at_point(cr, distance_spline, x)
+
+ # clear the cursor each draw so that it does not persist
+ # after you move off the spline
+ self.graph.cursor = None
def export_json(self, file_name):
self.path_to_export = os.path.join(