chore: move position correction code to lib

[eduardacoppo]

Depends on:

• feat: add position correction status field drivers-ais_base#6
• feat: make position in world to be the vessels drivers-orogen-nmea0183#10

What is this PR for

[sc-67374]
This PR moves the position correction code here

How I did it

Moved the code that was previously in drivers/orogen/nmea0183 and made some improvements in the code.

Results, How I tested

Unit tests

Checklist

• Assign yourself in the PR
• Write unit tests (when relevant)
• Run rubocop and rake tests locally

[doudou]

Style: https://docs.google.com/document/d/16zBGRa58zu7wgRBcGb6IVVQtCfiDqDDhu4vjUVnVSu0/edit?tab=t.0#heading=h.kagqk4knrk4k

Please fix those ... there's quite a few.

[seixasxbr]

what is yaw? give it a more descriptive name

[eduardacoppo]

Yaw is the rotation around the vertical axis in a 3d space... this is already a well-established concept. What would a more descriptive name be? Do you have any suggestions?

[jhonasiv]

He meant the yaw of what with regards to what frame?!

[eduardacoppo]

Did he? Well, I think the name of the function already gives the context we need. But because there's no documentation for this function, I could:

1. add the prefix vessel_ to all the function variables.
2. add a documentation for the standalone functions.

I personally prefer the second option.

[wvmcastro]

I think that the function name does not really tell what it does.

struct CourseOverGround {
    base::Angle const& direction;
    double norm;
};

std::pair<Eigen::Quaterniond, ais_base::PositionCorrectionStatus> selectVesselHeadingSource(
    base::Angle const& compass,
    CourseOverGround const& course_over_ground
)

I think the above is better and no useless comments (as @param some_snake_case_name The some snake case name ) are needed.
Documentation is necessary when the code is not sufficiently expressive, and rewriting function signature in plain english is not documentation for me and I think it is just noisy. It is not a complaint about you specifically, I see this practice in other team members too. Just using this opportunity to raise this discussion

[wvmcastro]

I think that the function name does not really tell what it does.

struct CourseOverGround {
    base::Angle const& direction;
    double norm;
};

std::pair<Eigen::Quaterniond, ais_base::PositionCorrectionStatus> selectVesselHeadingSource(
    base::Angle const& compass,
    CourseOverGround const& course_over_ground
)

I think the above is better and no useless comments (as @param some_snake_case_name The some snake case name ) are needed.
Documentation is necessary when the code is not sufficiently expressive, and rewriting function signature in plain english is not documentation for me and I think it is just noisy. It is not a complaint about you specifically, I see this practice in other team members too. Just using this opportunity to raise this discussion

[wvmcastro]

It is late and I am tired, so I can be very wrong but, shouldn't it be a minus? So it become vessel2sensor_in_world_pos

Suggested change

	vessel2world_UTM.position = sensor2world_UTM.position + sensor2vessel_in_world_pos;
	vessel2world_UTM.position = sensor2world_UTM.position + (-1 * sensor2vessel_in_world_pos);

also I think the naming is a bit confusing, maybe place all math inside a single method would be less confusing

// vessel and sensor orientation are the same 
auto vessel2sensor_pos = -sensor2vessel_pos;
vessel2world.position = sensor2world.position + sensor2world_ori * vessel2sensor_pos;