Skip to content
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

Open
MishkaMN opened this issue Jun 7, 2024 · 1 comment
Labels
anomaly Something isn't working enhancement New feature or request

Comments

@MishkaMN
Copy link
Contributor

MishkaMN commented Jun 7, 2024

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):

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.

@MishkaMN MishkaMN added enhancement New feature or request anomaly Something isn't working and removed enhancement New feature or request labels Jun 7, 2024
@MishkaMN
Copy link
Contributor Author

MishkaMN commented Jun 14, 2024

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
In this PR: 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

MishkaMN added a commit to usdot-fhwa-stol/carma-utils that referenced this issue Jun 14, 2024
<!-- 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.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
anomaly Something isn't working enhancement New feature or request
Projects
None yet
Development

No branches or pull requests

1 participant