Closed MishkaMN closed 4 months ago
Issues
3 New issues
0 Accepted issues
Measures
0 Security Hotspots
100.0% Coverage on New Code
0.0% Duplication on New Code
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
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: https://github.com/usdot-fhwa-stol/carma-platform/issues/2401
Related GitHub Issue
fixes: https://github.com/usdot-fhwa-stol/carma-platform/issues/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
Checklist: