Navigation solution for multiple segments with different speeds (never stop)

edit

We would like to follow a trajectory containing multiple segments as close as possible.

The reason we have multiple segments:

• Bookkeeping: continue as you left off, notify user of progress
• Actions need to be performed at the end of every segment.

Also we have different velocities for every segment:

• Some live operations require slow speed
• External requirements not driving-related
• Slow turning, high straights

Requirements:

1. Action interface which returns upon completion of the segment
2. Maximum(desired) velocity for every segment
3. Ability to reduce speed on-demand (external triggers: safety, max-torque reached)
4. Negative velocities for driving backwards
5. Non-zero end-velocity for a segment (we 'never' need to stop for our operations)
6. Segment transition velocity needs to be the minimal velocity of the two maximum/desired segment velocities (follows from (2). Or 0.0 in case next segment has a different sign (forward segment to backwards segment)
7. Acceleration/deceleration limits need to be taken into account. So deceleration needs to happen on the 'fastest' segment
8. Segments can have backwards speed
9. ROS1 or ROS2

Not-required:

• a. Dynamic obstacle avoidance
• b. Recalculations of current segment
• c. Recovery behavior

Assumptions:

• I. Feasible paths
• II. Next segments matches previous segment in direction (not in desired speed)

Considerations:

There are already packages availabe for trajectory tracking: timed_path_follower, trajectory_tracker and also navigation stacks which allow for path execution instead of endgoal: move_base_flex, robot_navigation, navigation2

I get the feeling we don't need sampling based planners because the solution direction is simple: as close to the trajectory as possible.

Also being able to 'stack' actiongoals would be convenient. So send multiple goals to the navigation stack which will be processed in-order. This will be convenient with the non-zero end-velocity requirement. We do need the goals to return exactly upon finising the segment (operations need to be performed there).

Reducing the velocity can also be done after the navigation stack, so it's a loose requirement.

Question:

What would be the best way to go around this?

• Write something completely from scratch
• Wrap an existing navigation stack
• Improve upon an existing navigation stack (which one would be best suited?)