Check consistency in sensor frame_id in topics and calibration.yaml

[miguelriemoliveira]

In #498 I used a configuration where the calibration.yaml said the sensor data was in one tf frame, and then it was in another.

The Calibration.yaml

  ouster_os1:
    link: "ouster_os1_0_lidar_center"
    parent_link: "base_link"
    child_link: "ouster_os1_0_base_link"
    topic_name: "/ouster_cloud_os1_0/points"
    #throttle: 10
    modality: lidar3d

and the calibration configuration report:

But when we run a rostopic echo of the /ouster_cloud_os1_0/points topic we get

header: 
  seq: 1268
  stamp: 
    secs: 1658155460
    nsecs: 245768772
  frame_id: "ouster_os1_0_base_link"
height: 64
width: 1024
fields: "<array type: sensor_msgs/PointField, length: 9>"
is_bigendian: False
point_step: 48
row_step: 49152
data: "<array type: uint8, length: 3145728>"
is_dense: True

Here it says the frame_id was the ouster_os1_0_base_link, and in the configuration it said the frame is ouster_os1_0_lidar_center.

This is causing all sorts of problems later on.

The calibration package configuration should notice this and abort with an error message.

[miguelriemoliveira]

Implemented.

If inconsistency found, outputs something like:

Checking consistency of configuration tf frames with the messages on corresponding topic ...
Loading bagfile /home/mike/bagfiles/agri_gaia/recording_2022-07-18-16-44-17_0.bag
Error: sensor ouster_os1 calibration config defined link ouster_os1_0_lidar_center but bagfile messages on topic /ouster_cloud_os1_0/points have header.frame_id ouster_os1_0_base_link. These should be the same! Check #514.

In fact we should remove the link field from the calibration.yaml sensor config, since we can collect it from the header.frame_id of the messages.

Should do this in a later version of ATOM.