SS-814 Revert "add intial_pose_is_ready flag" - Fix: robot localized with rviz, but keeps doing initialization behaviour #66
Conversation
This file contains hidden or 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
This reverts commit 6d97e0e. If an initial pose is recieved before activate, it is cached and used later when amcl is activated. This introduces a bug where initial_pose_is_ready_ never gets set to True. initial_pose_is_ready_ should have been set to true in places where the already existing initial_pose_is_known_ is set. (mainly handleInitialPose()) initial_pose_is_ready_ would then become identical to initial_pose_is_known_, with the only difference being that the new variable is set before handleInitialPose() which delays setting it to true due to a lookuptransform inside - this was supposedly done for a minor time optimization as lookup transform can take 100ms.
|
TanmayDeshmukh
changed the title
TanmayDeshmukh
changed the title
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.
Problem
If robot is localized very quickly with rviz at launch (before AMCL was activated), it appears to be localized on rviz, but BT continues to perform initial location behaviour (spinning in place).
This can also happen if it saw a fiducial only once, and then robot spun and the fiducial immediately went out of sight.
Solution/Reason
This reverts commit 6d97e0e, which introduces a new flag
initial_pose_is_ready_, to use in place of the existinginitial_pose_is_ready_.If an initial pose is received before activate, it is cached (through
last_published_pose_) and used later when amcl is activated. This introduces a bug where the newly introducedinitial_pose_is_ready_never gets set to True so the service call BT uses to check if robot is localized keeps returning falseinitial_pose_is_ready_should have been set to true in places where the already existinginitial_pose_is_known_is set. (insidehandleInitialPose()andglobalLocalizationCallback())initial_pose_is_ready_would then become identical toinitial_pose_is_known_, with the only difference being that the new variable is set beforehandleInitialPose().handleInitialPose()delays setting it to true due to a lookuptransform inside - so the introduction of the new variable this was seemingly done for a minor time optimization as lookup transform takes time (100ms?). However this doesn't actually work because service calls and initial pose callback are on the same (main)executor anyway, which means while an initial pose is being processed, initialPoseStatus service calls get queued and wont respond untilhandleInitialPose() completes.Note: AMCL has only two threads:
Related PR -