Motion computation ctrv angle fix

[MishkaMN]

PR Details

Description

This PR fixes the issue where predicted states of CTRV motion model is showing opposite direction (often flips back and forth). This is seriously impacting yielding behavior of carma due to bad prediction.
So I put an example scenario where heavy vehicle is red, carma is green (you can see the camera view on right) and predicted states are showing as transforms.
Truck is actually travelling lower left corner of the intersection as you can see from the truck's front being shown in the camera, but it is predicted backwards.

Before:
Because velocity is already in the map frame, if we add that angle to orientation like the old code, it will flip the direction to the opposite:

This is because old code assumed velocity was in base_link frame (which often just non-zero x value without noise which is the direction of travel) and pose orientation was the mostly the main yaw in CTRV. However, in simulation, all objects reported and used in carma's world model are already in map frame velocity, so it was "double counting".

atan fix:
previously the function was only getting the angle from y value and not accounting x, which means angle in 3rd quadrant will be in 4th erroneously:

after atan and frame fix:

I think technically the original code is correct, however, without this fix the current logic will not work.
I have documented why we needed this "workaround" at the moment in the code and in here:
usdot-fhwa-stol/carma-platform#2401

Related GitHub Issue

fixes: usdot-fhwa-stol/carma-platform#2398

Related Jira Key

https://usdot-carma.atlassian.net/browse/CAR-6058

Motivation and Context

During HASS and VOICE demo, it was discovered that vehicles's prediction would flip back and forith going opposite direction

How Has This Been Tested?

sim pc 2, made scenario-runner heavy vehicle to travel lower left of the intersection and observed the prediction.
And also tested successfully on VOICES with simulated carma detecting real life carma's trajectory correctly using CTRV

Types of changes

• Defect fix (non-breaking change that fixes an issue)
• New feature (non-breaking change that adds functionality)
• Breaking change (fix or feature that cause existing functionality to change)

Checklist:

• I have added any new packages to the sonar-scanner.properties file
• My change requires a change to the documentation.
• I have updated the documentation accordingly.
• I have read the CONTRIBUTING document.
• I have added tests to cover my changes.
• All new and existing tests passed.

[sonarcloud]

Quality Gate passed

Issues
3 New issues
0 Accepted issues

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

See analysis details on SonarCloud

[MishkaMN]

NOTE: We are currently just passing the oriectiontation. It means it might look like vehicle moving along correct path, but may look as if facing constant direction