Making repeated state publication optional #595

ayrton04 · 2020-09-18T08:13:24Z

Addresses #574.

As far as how this will behave, if the filter publishes a state with time t, and we later get a measurement with stamp < t, we'll still integrate that measurement and correct our state internally, but we won't publish a state that includes that correction until the next publication cycle after t.

This may get its base changed to a new noetic-devel branch.

ayrton04 · 2020-09-18T08:38:25Z

src/ros_filter.cpp

@@ -991,20 +995,21 @@ namespace RobotLocalization
             "\ntransform_timeout is " << tfTimeout_.toSec() <<
             "\nfrequency is " << frequency_ <<
             "\nsensor_timeout is " << filter_.getSensorTimeout() <<
-             "\ntwo_d_mode is " << (twoDMode_ ? "true" : "false") <<
-             "\nsmooth_lagged_data is " << (smoothLaggedData_ ? "true" : "false") <<
+             "\ntwo_d_mode is " << std::boolalpha << twoDMode_ <<


Sorry, I printed out the new param here using boolalpha, and thought I might as well pick up the others.

SteveMacenski · 2020-09-18T17:49:06Z

src/ros_filter.cpp

+      // If we're trying to publish with the same time stamp, it means that we had a measurement get inserted into the
+      // filter history, and our state estimate was updated after it was already published. As of ROS Noetic, TF2 will
+      // issue warnings whenever this occurs, so we make this behavior optional.
+      corrected_data = (!permitCorrectedPublication_ && lastPublishedStamp_ == filteredPosition.header.stamp);


lastPublishedStamp_ == filteredPosition.header.stamp I don't think that works, because what if its older than just the last update. If you have something with substantial latency (100ms) but your filter is spinning at 100hz, then it could be much older than last, right?

So I'm thinking aloud here. The filter history handles this situation:

In the first cycle, the filter has four measurements to process. They have timestamps 1, 2, 4, and 5. They get processed, and we publish a state with timestamp 5.

In the second cycle, the filter receives only a measurement with timestamp 3. It rewinds history to the posterior state after measurement 2 was fused, and then processes 3, 4, and 5 again. Then it publishes a state with timestamp 5. This creates the issue in question, and is solved by this PR.

Now let's go over your scenario, or at least what I think you are saying.

In the first cycle, the filter has four measurements to process. They have timestamps 1, 2, 4, and 5. They get processed, and we publish a state with timestamp 5.

In the second cycle, the filter has four measurements to process. They have timestamps 6, 7, 8, and 9. They get processed, and we publish a state with timestamp 9.

In the third cycle, the filter receives only a measurement with timestamp 3. It rewinds history to the posterior state after measurement 2 was fused, and then processes 3, 4, 5, 6, 7, 8, and 9 again. Then it publishes a state with stamp 9 again. This also causes the issue, and is solved by this PR.

The filter was never publishing the updated state as it processed the history. It was rewinding, inserting, and processing all the way up to the latest measurement, then publishing. So I think checking the latest publication should work?

Note that in both of those cases, anyone listening to tf will get the exact same behavior before and after this change: the first transform published won't get updated when the second one arrives. It's just that in Noetic, we also get yelled at. :)

There is a third case.

In the first cycle, the filter has four measurements to process. They have timestamps 1, 2, 4, and 5. They get processed, and we publish a state with timestamp 5.

In the second cycle, the filter has two measurements to process. They have timestamps 3 and 6. The filter rewinds history to the posterior state after measurement 2 was fused, and then processes 3, 4, 5, and 6, and publishes the state with timestamp 6. This is obviously not an issue.

Anyway, I will readily admit that I might be totally missing your point, and I apologize if so. Can you provide an example?

OK, I wasn't sure where filteredPosition's stamp was from, I didn't know that it would already be, at this point, rewinded back up to the latest message in all cases. That works.

I might just make it a little more error proof with lastPublishedStamp_ >= filteredPosition.header.stamp. I agree it shouldn't matter, but I think it makes syntactically more clear why this is being checked (or why they might be equal). I feel like a native reader can pull more from that than ==.

Yeah, that situation should never happen, but it also won't hurt to change it, and I agree with the increased clarity.

SteveMacenski · 2020-09-21T18:15:08Z

Shouldn't this be targeted only onto noetic and foxy? I don't think this issue affects melodic

SteveMacenski · 2020-09-21T18:15:35Z

doc/state_estimation_nodes.rst

@@ -174,6 +174,10 @@ If *true*, the state estimation node will publish the transform from the frame s
 ^^^^^^^^^^^^^^^^^^^^^
 If *true*, the state estimation node will publish the linear acceleration state. Defaults to *false*.

+~permit_corrected_publication


Any "new in XYZ distro" marker we can do?

Should we just make a noetic-devel and then change this PR to target that? Then we don't have to worry about that.

SteveMacenski · 2020-09-21T18:19:53Z

src/ros_filter.cpp

+      }
+
+      // Retain the last published stamp so we can detect repeated transforms in future cycles
+      lastPublishedStamp_ = filteredPosition.header.stamp;


This should be in the if statement, or else you could update based on a corrected_data update.

So the earlier suggestion about >= notwithstanding, as of now, if corrected_data == true, then lastPublishedStamp_ == filteredPosition.header.stamp anyway. However, I'm perfectly happy to move this up to line 1992.

ayrton04 · 2020-09-21T19:36:19Z

Shouldn't this be targeted only onto noetic and foxy? I don't think this issue affects melodic

Yes, but we don't have a noetic-devel. Are you cool if we make one now? I'll take care of it.

Will roll your suggestion into the PR tomorrow. Thanks!

SteveMacenski · 2020-09-21T19:43:54Z

Yeah sure

ayrton04 · 2020-10-02T08:56:35Z

@reobaird, were you able to try this out?

Co-authored-by: James <jservos@clearpath.ai>

* navsat_transform diagram to address #550 (#570) * add diagram for navsat_transform Signed-off-by: Mabel Zhang <mabel@openrobotics.org> * add diagram to tutorial Signed-off-by: Mabel Zhang <mabel@openrobotics.org> * Fix frame id of imu in differential mode, closes #482 * Added local Cartesian option to navsat_transform * Fix navsat_transform issue with starting on uneven terrain * Fix typo in navsat_transform_node.rst (#588) "prodives" -> "provides" * Fix sign error in dFY_dP in transfer function jacobian * Making repeated state publication optional (#595) * PR feedback * Fixing build issues * Fixing stamp issues * Linting and uncrustifying Co-authored-by: Mabel Zhang <mabel.m.zhang@gmail.com> Co-authored-by: Jeffrey Kane Johnson <jeff@mapless.ai>

* navsat_transform diagram to address cra-ros-pkg#550 (cra-ros-pkg#570) * add diagram for navsat_transform Signed-off-by: Mabel Zhang <mabel@openrobotics.org> * add diagram to tutorial Signed-off-by: Mabel Zhang <mabel@openrobotics.org> * Fix frame id of imu in differential mode, closes cra-ros-pkg#482 * Added local Cartesian option to navsat_transform * Fix navsat_transform issue with starting on uneven terrain * Fix typo in navsat_transform_node.rst (cra-ros-pkg#588) "prodives" -> "provides" * Fix sign error in dFY_dP in transfer function jacobian * Making repeated state publication optional (cra-ros-pkg#595) * PR feedback * Fixing build issues * Fixing stamp issues * Linting and uncrustifying Co-authored-by: Mabel Zhang <mabel.m.zhang@gmail.com> Co-authored-by: Jeffrey Kane Johnson <jeff@mapless.ai>

…r adopt cra-ros-pkg/robot_localization#595

ayrton04 requested a review from SteveMacenski September 18, 2020 08:13

ayrton04 mentioned this pull request Sep 18, 2020

TF_REPEATED_DATA warning in Noetic #574

Closed

Making repeated state publication optional

e4d0bf8

ayrton04 force-pushed the 574-repeat-publication branch from 706daab to e4d0bf8 Compare September 18, 2020 08:32

ayrton04 commented Sep 18, 2020

View reviewed changes

SteveMacenski reviewed Sep 18, 2020

View reviewed changes

SteveMacenski reviewed Sep 21, 2020

View reviewed changes

ayrton04 mentioned this pull request Sep 21, 2020

New warning message for repeated transforms ros/geometry2#467

Closed

ayrton04 changed the base branch from melodic-devel to noetic-devel September 23, 2020 16:42

PR feedback

a77f202

SteveMacenski approved these changes Sep 23, 2020

View reviewed changes

ayrton04 closed this Oct 2, 2020

ayrton04 reopened this Oct 2, 2020

ayrton04 merged commit 660793f into noetic-devel Oct 7, 2020

ayrton04 deleted the 574-repeat-publication branch October 7, 2020 09:31

ayrton04 added a commit that referenced this pull request Dec 11, 2020

Making repeated state publication optional (#595)

fe2a8b0

servos pushed a commit to clearpathrobotics/robot_localization that referenced this pull request Feb 16, 2021

Copy of Making repeated state publication optional cra-ros-pkg#595

9ce9b1e

servos pushed a commit to clearpathrobotics/robot_localization that referenced this pull request Feb 16, 2021

Copy of Making repeated state publication optional cra-ros-pkg#595

3cb0060

servos mentioned this pull request Feb 16, 2021

Copy of Making repeated state publication optional #595 clearpathrobotics/robot_localization#42

Merged

servos added a commit to clearpathrobotics/robot_localization that referenced this pull request Feb 17, 2021

Copy of Making repeated state publication optional cra-ros-pkg#595 (#42)

506ab2c

Co-authored-by: James <jservos@clearpath.ai>

efernandez mentioned this pull request Oct 26, 2021

[CORE-20551] Only update last published stamp when publishing clearpathrobotics/robot_localization#46

Merged

sktometometo mentioned this pull request Oct 15, 2023

[Fetch] TF Duplicated stamp jsk-ros-pkg/jsk_robot#1876

Open

mqcmd196 added a commit to mqcmd196/robot_localization that referenced this pull request Nov 2, 2023

adopt cra-ros-pkg#595

749f80c

mqcmd196 mentioned this pull request Nov 2, 2023

Melodic backport of https://github.com/cra-ros-pkg/robot_localization/pull/595 #840

Open

mqcmd196 added a commit to mqcmd196/jsk_robot that referenced this pull request Nov 2, 2023

[jsk_fetch_robot] use 2.6.9-suppress version of robot_localization fo…

62aa7b0

…r adopt cra-ros-pkg/robot_localization#595

mqcmd196 mentioned this pull request Nov 2, 2023

[develop/fetch] Suppress TF Duplicated stamp warning in noetic client PC jsk-ros-pkg/jsk_robot#1887

Merged

mqcmd196 added a commit to jsk-ros-pkg/jsk_robot that referenced this pull request Nov 2, 2023

[jsk_fetch_robot] use 2.6.9-suppress version of robot_localization fo…

a8b2ff2

…r adopt cra-ros-pkg/robot_localization#595

tkmtnt7000 pushed a commit to jsk-ros-pkg/jsk_robot that referenced this pull request Nov 9, 2023

[jsk_fetch_robot] use 2.6.9-suppress version of robot_localization fo…

44223c2

…r adopt cra-ros-pkg/robot_localization#595

kindsenior mentioned this pull request Nov 11, 2023

Removed duplications of the odom tf publication yujinrobot/kobuki_desktop#69

Open

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Making repeated state publication optional #595

Making repeated state publication optional #595

ayrton04 commented Sep 18, 2020 •

edited

Loading

ayrton04 Sep 18, 2020

SteveMacenski Sep 18, 2020

ayrton04 Sep 18, 2020

SteveMacenski Sep 21, 2020 •

edited

Loading

ayrton04 Sep 21, 2020

SteveMacenski commented Sep 21, 2020

SteveMacenski Sep 21, 2020

ayrton04 Sep 21, 2020

SteveMacenski Sep 21, 2020

SteveMacenski Sep 21, 2020

ayrton04 Sep 21, 2020

ayrton04 commented Sep 21, 2020

SteveMacenski commented Sep 21, 2020

ayrton04 commented Oct 2, 2020

Making repeated state publication optional #595

Making repeated state publication optional #595

Conversation

ayrton04 commented Sep 18, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SteveMacenski Sep 21, 2020 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

SteveMacenski commented Sep 21, 2020

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

ayrton04 commented Sep 21, 2020

SteveMacenski commented Sep 21, 2020

ayrton04 commented Oct 2, 2020

ayrton04 commented Sep 18, 2020 •

edited

Loading

SteveMacenski Sep 21, 2020 •

edited

Loading