actuation_format_bag: Fix Trajectory writing

The first message in chronological order is currently a KinematicState,
when it should be a Trajectory. Otherwise the PurePursuit algorithm has
a different output on its first Trajectory depending on the state of its
internal buffers.

Issue-Id: SCM-5776
Signed-off-by: Ambroise Vincent <ambroise.vincent@arm.com>
Change-Id: Icf7d144541c31b52a30fbbc66e50144cf15b6014