Fix/slope inaccuracies #1493
Open
Fix/slope inaccuracies #1493
+254
−41
Conversation
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Checklist for reviewers ☑️All references to "You" in the following text refer to the code reviewer.
|
SzymonParapura
added
the
bump minor
…dEntityStatusInWorldFrame
…ntityStatusInWorldFrame
This reverts commit 26f8a65.
…etPose and return it
…yStatusInWorldFrame
…moveTowardsLaneletPose
…ose for WalkStraightAction, tidy up
- Adjusted entity position when transitioning between lanelets to reduce slope errors
… position updates
SzymonParapura
added
wait for regression test
and removed
wait for regression test
labels
- Updated makeUpdatedStatus to fix an issue with toCanonicalizedLaneletPose
SzymonParapura
added
wait for regression test
and removed
wait for regression test
labels
|
There is no regression |
HansRobo
reviewed
Feb 4, 2025
| updated_pose.orientation = status->getMapPose().orientation * delta_quaternion; | ||
|
|
||
| /// @note Apply position change | ||
| /// @todo first determine global desired_velocity, calculate position change using it |
There was a problem hiding this comment.
This todo has already finished?
If so, please delete.
There was a problem hiding this comment.
I left the todo on purpose because the calculations are done in a specific coordinate system (desired_velocity is given in the local frame). This conversion to the global frame might still need to be done. I also wanted to point this out for others working on the code to avoid potential mistakes from misinterpreting the reference frame.
HansRobo
reviewed
Feb 4, 2025
HansRobo
reviewed
Feb 4, 2025
|
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
Add this suggestion to a batch that can be applied as a single commit.
This suggestion is invalid because no changes were made to the code.
Suggestions cannot be applied while the pull request is closed.
Suggestions cannot be applied while viewing a subset of changes.
Only one suggestion per line can be applied in a batch.
Add this suggestion to a batch that can be applied as a single commit.
Applying suggestions on deleted lines is not supported.
You must change the existing code in this line in order to create a valid suggestion.
Outdated suggestions cannot be applied.
This suggestion has been applied or marked resolved.
Suggestions cannot be applied from pending reviews.
Suggestions cannot be applied on multi-line comments.
Suggestions cannot be applied while the pull request is queued to merge.
Suggestion cannot be applied right now. Please check back later.
Description
Abstract
This pull request introduces a fix for lanelet slope inaccuracies in the
FollowTrajectoryActionandWalkStraightAction.Details
In the case of
FollowTrajectoryActionandWalkStraightAction, 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::adjustOrientationAndOzPositionadjusts 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 themakeUpdatedStatusfunction 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
updatePositionForLaneletTransitionfunction accounts for cases where the entity reaches the end of the current lanelet and needs to transition to the next lanelet. LinkFollow Trajectory Action
makeUpdatedStatusmethod has been updated to fix issues. LinkWalk Straight Action
ActionNode::calculateUpdatedEntityStatusInWorldFramehas been modified to fix the issue withWalkStraightAction. 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
--