Some basic set-up for implementing the stop operator
We will move some of the Modifier's code over to the Planner.
Changes added for stop operator
The fields stop_points and stop_times have been added to the Path object and msg. These are vectors corresponding to any points in a path (knot points) that we should stop at and for how long
Trajectory generation now considers stopping at a knot point. It check each segment to see if the ending knot point is one we should stop at. If so, it generates stopped motion states for the value in stop_time.
TODO: Trajectory generation - What if point 0 is a point to stop at? Planner - add finding the stop time calculation in Modifier class
Changes made to generate a trajectory with a stop in it
Some changes were made to the ramp_planner package to incorporate stop operators into the modification process, but altering this package is not done yet.
The Corobot was tested successfully for driving along a trajectory with multiple stops in it.
Stop operator added
Path msg had 2 fields added - stop_points and stop_times. Both are uint32 vectors corresponding to the knot points where stops occur and for how long they occur, respectively.
Path modification package is only called when the operator affects the path. The stop operator does not. A separate method was added to the ramp_planner Modifier class to deal with stop operations. This package also had to remove the values in a Path's stop_points and stop_times vectors if a Delete operation was performed. TODO: What if crossover?
Trajectory generator package had numerous changes to generate trajectories that can handle stops of any time at each knot point (except the last). The total stop time is added to the segment's T_min so that the correct knot points are obtained for a trajectory with stops in it.
If a new msg is created for knot points, it may make the code cleaner because there will be less maintanence for a Path's stop_points and stop_times vectors.