fix(multi_object_tracker): consider vehicle twist direction

[takayuki5168]

Description

Fixed the twist direction bug, which was the object's orientation, but actually the slip angle should be considered.
The kinematics model used in the tracker is CoG frame where the direction of twist and object orientation is not the same.

Tests performed

• planning simulator
• scenario simulator
  • https://evaluation.tier4.jp/evaluation/reports/cb5db4b2-c286-5eb3-996d-676f01957581?project_id=prd_jt

Effects on system behavior

Nothing

Pre-review checklist for the PR author

The PR author must check the checkboxes below when creating the PR.

• I've confirmed the contribution guidelines.
• The PR follows the pull request guidelines.

In-review checklist for the PR reviewers

The PR reviewers must check the checkboxes below before approval.

• The PR follows the pull request guidelines.

Post-review checklist for the PR author

The PR author must check the checkboxes below before merging.

• There are no open discussions or they are tracked via tickets.

After all checkboxes are checked, anyone who has write access can merge the PR.

Summary by CodeRabbit

Release Notes:

• Bug fix: Fixed a bug in the twist direction calculation in the multi-object tracker code.
• Refactor: Updated the calculation of the twist values in the getTrackedObject function of the BicycleTracker, BigVehicleTracker, and NormalVehicleTracker classes to consider the slip angle.

"Bug fixed, twists refined,
Slip angles now aligned.
Objects tracked with precision,
A bug's demise, a coder's mission."

[YoshiRi]

Make sense.
Could you add following two changes?

• add same changes to bicycle_tracker

  twist_with_cov.twist.linear.x = X_t(IDX::VX) * std::cos(X_t(IDX::SLIP));
  twist_with_cov.twist.linear.y = X_t(IDX::VX) * std::sin(X_t(IDX::SLIP));

• add document description in models.md

#### remarks on the output twist

Remarks that the velocity $v_{k}$ is the norm of velocity of vehicle, not the longitudinal velocity.
So the output twist in the object coordinate $(x,y)$ is calculated as follows.

$$
\begin{aligned}
v_{x} &= v_{k} \cos \left(\beta_{k}\right) \\
v_{y} &= v_{k} \sin \left(\beta_{k}\right) 
\end{aligned}
$$

[takayuki5168]

@YoshiRi Thank you. I added them.

[github-actions]

Documentation URL: https://autowarefoundation.github.io/autoware.universe/pr-4637/
Modified URLs:

• https://autowarefoundation.github.io/autoware.universe/pr-4637/perception/multi_object_tracker/models/

[codecov]

Codecov Report

Patch and project coverage have no change.

Comparison is base (31e10cd) 14.89% compared to head (f2e6b80) 14.89%.
Report is 2 commits behind head on main.

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #4637   +/-   ##
=======================================
  Coverage   14.89%   14.89%           
=======================================
  Files        1547     1547           
  Lines      106613   106599   -14     
  Branches    32603    32595    -8     
=======================================
+ Hits        15876    15878    +2     
+ Misses      73432    73417   -15     
+ Partials    17305    17304    -1     

Flag	Coverage Δ		*Carryforward flag
differential	0.00% <0.00%> (?)
total	14.89% <ø> (+<0.01%)	⬆️	Carriedforward from 1d39724

*This pull request uses carry forward flags. Click here to find out more.

Files Changed	Coverage Δ
...ject_tracker/src/tracker/model/bicycle_tracker.cpp	0.00% <0.00%> (ø)
..._tracker/src/tracker/model/big_vehicle_tracker.cpp	0.00% <0.00%> (ø)
...acker/src/tracker/model/normal_vehicle_tracker.cpp	0.00% <0.00%> (ø)

... and 4 files with indirect coverage changes

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[YoshiRi]

LGTM

[github-actions]

CodeRabbit

Walkthrough

The overall change focuses on fixing a bug in the twist direction calculation in the multi-object tracker code. The calculation of the linear and angular components of the twist is updated based on the slip angle (X_t(IDX::SLIP)). The linear x-component is multiplied by the cosine of the slip angle, and the linear y-component is multiplied by the sine of the slip angle. The angular z-component is calculated using the updated linear x-component and a constant value (lr_).

Changes

File	Summary
perception/multi_object_tracker/models.md	Fixes a bug in the twist direction calculation. Introduces a new calculation for the output twist in the object coordinate system. Remarks about velocity and the calculation of the output twist. No changes to exported functions or global data structures.
perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp	Changes the calculation of twist_with_cov.twist.linear.x and twist_with_cov.twist.linear.y in the getTrackedObject function of the BicycleTracker class. Multiplies twist_with_cov.twist.linear.x by the cosine of X_t(IDX::SLIP), and twist_with_cov.twist.linear.y by the sine of X_t(IDX::SLIP). Accounts for slip angle when calculating the twist direction.
perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp	Modifies the getTrackedObject function in the BigVehicleTracker and NormalVehicleTracker classes. Updates the calculation of the linear and angular components of the twist based on the slip angle (X_t(IDX::SLIP)). Multiplies the linear x-component by the cosine of the slip angle, and the linear y-component by the sine of the slip angle. Calculates the angular z-component using the updated linear x-component and a constant value (lr_). No changes to function signature or external interface.

[github-actions]

CodeRabbit

Commits

Files that changed from the base of the PR and between 31e10cd and f2e6b80 commits.

Files selected (4)

• perception/multi_object_tracker/models.md (1)
• perception/multi_object_tracker/src/tracker/model/bicycle_tracker.cpp (1)
• perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp (1)
• perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp (1)

Files skipped from review due to trivial changes (2)

• perception/multi_object_tracker/src/tracker/model/big_vehicle_tracker.cpp
• perception/multi_object_tracker/src/tracker/model/normal_vehicle_tracker.cpp

Review comments generated (2)

• Review: 1
• LGTM: 1

---

Tips

Chat with CodeRabbit Bot (@coderabbitai)

• Reply on review comments left by this bot to ask follow-up questions. A review comment is a comment on a diff or a file.
• Invite the bot into a review comment chain by tagging @coderabbitai in a reply.

Code suggestions

• The bot may make code suggestions, but please review them carefully before committing since the line number ranges may be misaligned.
• You can edit the comment made by the bot and manually tweak the suggestion if it is slightly off.

Pausing incremental reviews

• Add @coderabbitai: ignore anywhere in the PR description to pause further reviews from the bot.

[github-actions]

CodeRabbit

The comments in this section provide an explanation of how the output twist in the object coordinate system is calculated. However, the comments are not formatted correctly and are missing the necessary syntax for mathematical equations. Additionally, the comments refer to variables and symbols that are not defined in this context.

To improve clarity, please update the comments as follows:

68: #### Remarks on the output twist

69: The velocity `v_k` represents the norm of the vehicle's velocity, not the longitudinal velocity.

70: To calculate the output twist in the object coordinate `(x, y)`, use the following formulas:

71: 
72: ```
73: v_x = v_k * cos(beta_k)
74: v_y = v_k * sin(beta_k)
75: ```

76: Where:
77: - `v_x` is the linear x-component of the twist.
78: - `v_y` is the linear y-component of the twist.
79: - `beta_k` is the slip angle of the vehicle.

Please ensure that the variables and symbols used in the comments are consistent with their definitions in the code.

[yukkysaito]

LGTM