Fix undefined behavior in plan cleanup

[mxgrey]

This PR fixes some undefined behavior that can happen when trying to remove unnecessary motions from a plan. We were accidentally dereferencing an optional that may have a nullopt value.

[codecov]

Codecov Report

Attention: 2 lines in your changes are missing coverage. Please review.

Comparison is base (1f4ddb5) 21.71% compared to head (099fbbd) 21.71%.

Additional details and impacted files

@@           Coverage Diff           @@
##             main     #106   +/-   ##
=======================================
  Coverage   21.71%   21.71%           
=======================================
  Files         361      361           
  Lines       30302    30302           
  Branches    14200    14200           
=======================================
  Hits         6580     6580           
  Misses      14218    14218           
  Partials     9504     9504           

Flag	Coverage Δ
tests	21.71% <0.00%> (ø)

Flags with carried forward coverage won't be shown. Click here to find out more.

Files	Coverage Δ
..._traffic/agv/planning/DifferentialDrivePlanner.cpp	40.98% <0.00%> (ø)