Multi-threading task bug

I have a code that is using
OOP
Multi threading

has bugs has to be done ASAP (Already have the code)
The program is intended as a simple service to release waypoints one at a time to a robot in a way that allows changes/cancellations.

For example, the process could be:

The robot begins with coordinates (2, 1).
We submit a trajectory with points ((4, 3), (5, 3)).
The robot starts to move to (4, 3).
We submit a trajectory with points ((3, 3), (5, 4)).
The robot starts to move to (3, 3) instead of (4, 3).
The robot arrives at (3, 3), and starts to move towards (5, 4).
The robot receives a trajectory with points ().
The robot stops.
(In the real world this might be done with something like ROS, but we minimized external dependencies for this exercise.)

Task:

Expand the test set as needed.
Debug the program to resolve inconsistencies.
Add basic input validation to improve robustness.
Refactor the code to improve readability, maintainability.
if you read please start with "readahmed"
code attached