-
Notifications
You must be signed in to change notification settings - Fork 124
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Detected object state and logics involved in simulation doesn't follow ROS convention in reporting velocity #2401
Comments
During HASS demo preparation on 06/11/2024 and VOICES Pilot 2 demo on 05/01/2024, it was discovered that other vehicles' prediction was flipping back and forth. This meant degraded yielding behavior. Therefore, needed a fix on the CTRV logic to make it work in map frame: PR usdot-fhwa-stol/carma-utils#233, issue: #2398 |
<!-- Thanks for the contribution, this is awesome. --> # PR Details ## Description <!--- Describe your changes in detail --> 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: ![image](https://github.com/usdot-fhwa-stol/carma-utils/assets/20613282/3bbe2fa2-5ed4-4d22-a9e9-a96d3d28dd43) 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: ![image](https://github.com/usdot-fhwa-stol/carma-utils/assets/20613282/827b8229-cb70-4233-aa86-ad25a6dc1bfc) after atan and frame fix: ![image](https://github.com/usdot-fhwa-stol/carma-utils/assets/20613282/04c96e9e-f5d7-4a3f-b24e-901492b08550) 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 <!--- This project only accepts pull requests related to open GitHub issues or Jira Keys --> <!--- If suggesting a new feature or change, please discuss it in an issue first --> <!--- If fixing a bug, there should be an issue describing it with steps to reproduce --> <!--- Please DO NOT name partially fixed issues, instead open an issue specific to this fix --> <!--- Please link to the issue here: --> ## Related Jira Key https://usdot-carma.atlassian.net/browse/CAR-6058 <!-- e.g. CAR-123 --> ## Motivation and Context During HASS and VOICE demo, it was discovered that vehicles's prediction would flip back and forith going opposite direction <!--- Why is this change required? What problem does it solve? --> ## 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 <!--- Please describe in detail how you tested your changes. --> <!--- Include details of your testing environment, and the tests you ran to --> <!--- see how your change affects other areas of the code, etc. --> ## Types of changes <!--- What types of changes does your code introduce? Put an `x` in all the boxes that apply: --> - [X] 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: <!--- Go over all the following points, and put an `x` in all the boxes that apply. --> <!--- If you're unsure about any of these, don't hesitate to ask. We're here to help! --> - [X] I have added any new packages to the sonar-scanner.properties file - [ ] My change requires a change to the documentation. - [X] I have updated the documentation accordingly. - [X] I have read the [**CONTRIBUTING**](https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/Contributing.md) document. - [X] I have added tests to cover my changes. - [X] All new and existing tests passed.
Summary
During carma-system-4.5.0 (lavida release), development started with carma-platform receiving detected object's velocity state in its map frame from CARLA. This meant, the linear velocity was already in map frame (non-zero x and y components for example), but not in its base_link frame which is the ROS convention (usually x non-zero with other zero because x is usually direction of travel for example). It worked out nicely with following logics already expecting velocity states in map frame as well.
However, this contradicted a prediction logic in motion_predict for example that had been developed before and doesn't work unless it is modified to expect map frame velocity as well (see 4th bullet point).
So this issue is to make everything consistent - whether it is to use map frame or base_link frame - and set the convention.
Known affected locations in carma-system-4.5.0 are (could be more):
https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/motion_computation/src/psm_to_external_object_convertor.cpp#L121
https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/motion_computation/src/mobility_path_to_external_object.cpp#L201
https://github.com/usdot-fhwa-stol/carma-platform/blob/develop/motion_computation/src/bsm_to_external_object_convertor.cpp#L92
Version
4.5.0 (Current)
Expected Behavior
See above
Actual Behavior
See above
Steps to Reproduce the Actual Behavior
NA
Related Work
#2407. Since all object stack is in map frame, it is prudent to warn the user if there is a potential frame mismatch.
The text was updated successfully, but these errors were encountered: