Skip to content

9.2.0
Compare
Choose a tag to compare
@github-actions github-actions released this 05 Feb 05:05
· 238 commits to master since this release
9.2.0
132c03d

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.

Screenshot from 2024-12-20 17-16-20

Code Changes

Pose

  • The updatePositionForLaneletTransition 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

--

Related Issues

Assets 2
Loading