Fix/slope inaccuracies

[SzymonParapura]

Description

Abstract

This pull request introduces a fix for lanelet slope inaccuracies in the FollowTrajectoryAction and WalkStraightAction.

Details

In the case of FollowTrajectoryAction and WalkStraightAction, an issue arises in the algorithm responsible for determining the entity's position. During each simulation step, displacement is calculated based on the velocity vector and step_time. When the entity moves along a sloped lanelet (e.g., driving uphill), the calculated displacement can sometimes exceed the remaining distance to the end of the lanelet.

To address transitions between lanelets, the method CanonicalizedLaneletPose::adjustOrientationAndOzPosition adjusts the entity's orientation to match the lanelet's pitch angle and sets the entity's height to the lanelet's elevation. As a result, the cumulative displacement computed by the makeUpdatedStatus function may not always reflect the actual distance traveled.

The proposed solution involves checking the remaining distance to the end of the current lanelet. If the calculated displacement exceeds this distance, the entity's position is updated to the next lanelet in a controlled manner. This ensures accurate positioning and smooth transitions between lanelets.

Code Changes

Pose

• The moveTowardsLaneletPose function calculates the new position of an entity along a lanelet based on a desired velocity and step time. The function accounts for cases where the entity reaches the end of the current lanelet and needs to transition to the next lanelet. Link

Follow Trajectory Action

• The makeUpdatedStatus method has been updated to fix issues. Link

Walk Straight Action

• The method ActionNode::calculateUpdatedEntityStatusInWorldFrame has been modified to fix the issue with WalkStraightAction. It updates the position and orientation of the entity, ensuring precise movement along the lanelet and accurate handling of transitions between lanelets. Link.

New Vector and Quaternion Operations

• Added function to calculate the norm of a quaternion. Link

• Added function to normalize a quaternion. Link

• An operator overload for the + operator was added to facilitate vector addition. This allows combining custom vector types with Eigen::Vector3d seamlessly. Link

References

Internal link 1
Internal link 2

Destructive Changes

--

Known Limitations

--

[github-actions]

Checklist for reviewers ☑️

All references to "You" in the following text refer to the code reviewer.

• Is this pull request written in a way that is easy to read from a third-party perspective?
• Is there sufficient information (background, purpose, specification, algorithm description, list of disruptive changes, and migration guide) in the description of this pull request?
• If this pull request contains a destructive change, does this pull request contain the migration guide?
• Labels of this pull request are valid?
• All unit tests/integration tests are included in this pull request? If you think adding test cases is unnecessary, please describe why and cross out this line.
• The documentation for this pull request is enough? If you think adding documents for this pull request is unnecessary, please describe why and cross out this line.

[sonarqubecloud]

Quality Gate passed

Issues
3 New issues
0 Accepted issues

Measures
0 Security Hotspots
55.2% Coverage on New Code
0.0% Duplication on New Code

See analysis details on SonarQube Cloud