Fix loss of first waypoint in upsample trajectory

[thettasch]

The current implementation of the upscale trajectory task fails to pass the first waypoint through to the output. This small patch fixes the problem. I have also adjusted the tests to account for the extra point in the outputs. I have tested this successfully with tesseract 0.20.2.

Demonstration of bug

I ran the following dummy trajectory through the UpsampleTrajectoryTask with longest_valid_segment_length=0.5:

Composite Instruction, Description: Tesseract Composite Instruction
{
  Move Instruction, Move Type: 1, State WP: Pos=0 0 0 0 0 0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=1 0 0 0 0 0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=2 0 0 0 0 0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=3 0 0 0 0 0
, Description: Tesseract Move Instruction
}

The current implementation returns the following. Note the loss of the 0 0 0 0 0 0 waypoint.

Composite Instruction, Description: Tesseract Composite Instruction
{
  Move Instruction, Move Type: 1, State WP: Pos=0.333333        0        0        0        0        0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=0.666667        0        0        0        0        0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=1 0 0 0 0 0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=1.33333       0       0       0       0       0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=1.66667       0       0       0       0       0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=2 0 0 0 0 0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=2.33333       0       0       0       0       0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=2.66667       0       0       0       0       0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=3 0 0 0 0 0
, Description: Tesseract Move Instruction
}

After applying my simple fix, the following is returned:

Composite Instruction, Description: Tesseract Composite Instruction
{
  Move Instruction, Move Type: 1, State WP: Pos=0 0 0 0 0 0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=0.333333        0        0        0        0        0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=0.666667        0        0        0        0        0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=1 0 0 0 0 0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=1.33333       0       0       0       0       0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=1.66667       0       0       0       0       0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=2 0 0 0 0 0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=2.33333       0       0       0       0       0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=2.66667       0       0       0       0       0
, Description: Tesseract Move Instruction
  Move Instruction, Move Type: 1, State WP: Pos=3 0 0 0 0 0
, Description: Tesseract Move Instruction
}

Why the bug occurs

When looping through the waypoints of a composite instruction, pairs of waypoints are examined and interpolated between if necessary. To avoid duplication of the endpoints, the loop at line 162 ignores the first point (since it was already added as the last point of the previous interpolation). In doing so, the very first waypoint is lost when continue is called at line 140. The fix is simply to add the very first waypoint to the resulting composite instruction.

[Levi-Armstrong]

Can you rebase on master?

[thettasch]

Can you rebase on master?

Done

[Levi-Armstrong]

LGTM, pending pipeline

[Levi-Armstrong]

Thank you for the fix!

[thettasch]

It's a pleasure!