Add timestamps to save ply

[TomMontagnon]

Description

Problem

When I run:

ros2 service call /save_trajectory norlab_icp_mapper_ros/srv/SaveTrajectory "{trajectory_file_name: {data: '$PWD/my_traj.ply'}}"

It saves a trajectory, but without timestamps for each point.
Timestamps are passed to libpointmatcher, but the lib doesn't save them in case of .ply.
However, I need the timestamps, and I believe this is necessary in many cases.

(I was told lately that exporting to .vtk may save the timestamps, but I find working with .ply files to be more convenient anyway.)

Short summary of the solution

Timestamps are passed as int64 to libpointmatcher, but the PLY standard does not support int64.
I therefore had to add two properties int32 to the PLY format: one for seconds and the other for nanoseconds (following the ROS2 convention).

Main changes

• In savePLY, if the data contains times :
  • add two new properties "timestamp_sec" and "timestamp_nsec"
  • split the int64 into two int32 fields
• In loadPLY:
  • In phase three // 3- RESERVE DATAPOINTS MEMORY : If "timestamp_sec" (int32) and "timestamp_nsec" (int32) are present, only allocate one int64 field initialized to 0
  • In phase four // 4- PARSE PLY DATA (vertex) : If I detect "timestamp_sec" (int32) and "timestamp_nsec" (int32), combine them with the appropriate coefficients into the previously-initialized int64 field.
• Added two new types "TIME" in IO.h
• Added a new unit test

Checklist

• The build passes
• All original unit tests pass
• I added a dedicated unit test for the "timestamps" case
• I tested the branch locally for my use case: trajectory saving with timestamps works perfectly

Other info

• Line 80 in norlab_icp_mapper/Trajectory.cpp shows that timestamps are passed correctly to libpointmatcher.
• Line 64 in norlab_icp_mapper/Trajectory.cpp shows that times are int64_t.
• Tracing the code back, line 74 in norlab_icp_mapper_ros/mapper_node.cpp indicates that these times come from cloudMsgIn.header.stamp, which based on the ROS documentation is:

  int32 sec
  uint32 nanosec
  

[sonarqubecloud]

Quality Gate passed

Issues
5 New issues
0 Accepted issues

Measures
0 Security Hotspots
0.0% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarQube Cloud

[boxanm]

Hey @TomMontagnon!
Thanks and good job on the PR! We've been quite busy on our site lately, but I will review the PR once things settle down a bit on our side.