Fix trajectory display.

[kpwelsh]

Description

The RViz plugin for displaying planned trajectories handles elapsed time incorrectly. Probably sometime in the last few years the dt passed to Update was switched from "s" to "ns," and this plugin wasn't updated.

Also, the plugin doesn't show the last pose in the trajectory. When we are on the last pose in the trajectory, we call displaying_trajectory_message_->getWayPointDurationFromPrevious(current_state_ + 1); to determine how long to display the pose for. This returns 0 and we accordingly display it for 0s.

Instead, I chose to put in a constant 1s display time (scaled by the user's trajectory playback rate) at the end of each trajectory so it will pause at the end briefly before looping back to the start, or disappearing.

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[henningkayser]

Thank you! Can you retarget this change to 'main'? That is what we use as development head. We can backport any bugfixes to humble or other stable branches.

[github-actions]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

[github-actions]

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

[mergify]

Please target the main branch for development, we will backport the changes to humble for you if approved and if they don't break API.