Detected object state and logics involved in simulation doesn't follow ROS convention in reporting velocity
MishkaMN opened this issue · comments
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):
- Publishing sensor detected objects from CARLA to carma-platform: Currently in map frame
- Cooperative Perception stack, external_list_to_detection_converter. Since input to the core library is expected to be in map frame, it is just passing the data through currently, so they are in map frame. If base_link is preferred, there needs to be conversion here on the input to MOT, core library. and possibly also here on the output.
- CV model in motion_predict is currently in map frame
- However, CTRV model in motion_predict is expecting input not in map frame from this logic. The logic aims to calculate yaw in map frame here using vehicle's orientation in map frame + and velocity direction in its base_link frame, which is the ROS convention.
- Entire logic in yield_plugin is currently in map frame.
- Since yield if expecting velocity to be in map frame, following external_object creation logics from bsm, psm, and mobility_path will not work with yield:
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.
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