Skip to content

Fix #2035: Initialize previous_publish_timestamp_ in on_configure#2084

Closed
Bharath4444 wants to merge 2 commits intoros-controls:masterfrom
Bharath4444:fix-issue-2035-timestamp
Closed

Fix #2035: Initialize previous_publish_timestamp_ in on_configure#2084
Bharath4444 wants to merge 2 commits intoros-controls:masterfrom
Bharath4444:fix-issue-2035-timestamp

Conversation

@Bharath4444
Copy link
Copy Markdown

Description

This PR addresses issue #2035 by moving the initialization of previous_publish_timestamp_ into the on_configure() lifecycle method.

Previously, this variable was initialized inside a catch block within the update loop, effectively using exception handling for normal control flow. Moving it to on_configure aligns it with how previous_update_timestamp_ is handled and ensures the timestamp is valid before the update loop begins.

Related Issues

Closes #2035

Type of Change

  • Bug fix (non-breaking change which fixes an issue)
  • New feature (non-breaking change which adds functionality)
  • Breaking change (fix or feature that would cause existing functionality to not work as expected)

Checklist

  • I have performed a self-review of my code
  • I have commented my code, particularly in hard-to-understand areas
  • My changes generate no new warnings

@@ -220,18 +220,10 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
orientation.setRPY(0.0, 0.0, odometry_.getHeading());

bool should_publish = false;
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this variable can/should be removed now.

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I guess it is still needed to be able to control the frequency of publishing the message

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You are right, we can combine the logic and we can remove it

Copy link
Copy Markdown
Member

@saikishor saikishor left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Combine the logic and remove should_publish variable

Copy link
Copy Markdown
Member

@christophfroehlich christophfroehlich left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This just removes the logic introduced with #585

Are we sure that this is not needed anymore?

previous_publish_timestamp_ = time;
should_publish = true;
}
previous_publish_timestamp_ += publish_period_;
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
previous_publish_timestamp_ += publish_period_;
previous_publish_timestamp_ = time;

@christophfroehlich I think this should fix #585 and also be better overall.

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

no because the comparison above fails if the time source changed.


bool should_publish = false;
try
if (previous_publish_timestamp_ + publish_period_ <= time)
Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
if (previous_publish_timestamp_ + publish_period_ <= time)
if (previous_publish_timestamp_ + publish_period_ > time)

I was misunderstanding the logic here. I think with this change and the earlier one everything should work. This should cause this if statement to pass as true the first time in simulation and from there on work correctly.

@christophfroehlich

Copy link
Copy Markdown
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

still: This comparison will throw an exception if the timesource changes

@github-actions
Copy link
Copy Markdown
Contributor

This PR is stale because it has been open for 45 days with no activity. Please tag a maintainer for help on completing this PR, or close it if you think it has become obsolete.

@github-actions github-actions Bot added the stale label Mar 12, 2026
@mergify
Copy link
Copy Markdown
Contributor

mergify Bot commented Mar 12, 2026

This pull request is in conflict. Could you fix it @Bharath4444?

@christophfroehlich
Copy link
Copy Markdown
Member

@Bharath4444 could you fix the remaining threads please?

@Bharath4444 Bharath4444 force-pushed the fix-issue-2035-timestamp branch from cd0b235 to 04e7542 Compare March 13, 2026 12:32
@github-actions github-actions Bot removed the stale label Mar 13, 2026
Copy link
Copy Markdown
Member

@christophfroehlich christophfroehlich left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please don't do force pushes if a PR is already under review. Have you changed anything, have you considered the remarks in the threads?

@codecov
Copy link
Copy Markdown

codecov Bot commented Mar 13, 2026

Codecov Report

❌ Patch coverage is 66.66667% with 1 line in your changes missing coverage. Please review.
✅ Project coverage is 84.85%. Comparing base (b3e8a30) to head (04e7542).
⚠️ Report is 10 commits behind head on master.

Files with missing lines Patch % Lines
...iff_drive_controller/src/diff_drive_controller.cpp 66.66% 0 Missing and 1 partial ⚠️
Additional details and impacted files
@@            Coverage Diff             @@
##           master    #2084      +/-   ##
==========================================
- Coverage   84.85%   84.85%   -0.01%     
==========================================
  Files         153      153              
  Lines       15016    15011       -5     
  Branches     1298     1297       -1     
==========================================
- Hits        12742    12737       -5     
  Misses       1800     1800              
  Partials      474      474              
Flag Coverage Δ
unittests 84.85% <66.66%> (-0.01%) ⬇️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines Coverage Δ
...iff_drive_controller/src/diff_drive_controller.cpp 80.92% <66.66%> (-0.29%) ⬇️
🚀 New features to boost your workflow:
  • ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

@christophfroehlich christophfroehlich requested review from Juliaj and removed request for Amronos March 26, 2026 07:45
@RicardoMan
Copy link
Copy Markdown

I read through the diff and the review comments. It seems the main issue is that removing the try-catch leaves no handling for when the clock source changes at runtime. The comparison on line 241 would still throw in that case.

One idea: check if the clock type changed before doing the arithmetic, using get_clock_type() from the rclcpp::Clock API (https://docs.ros.org/en/humble/p/rclcpp/generated/classrclcpp_1_1Clock.html). If the types don't match, reinitialize previous_publish_timestamp_ with the current time. This avoids using exceptions for normal control flow while still handling the timesource change.

@christophfroehlich
Copy link
Copy Markdown
Member

Thanks for your work and comments, but I think we will just remove this functionality with #2245

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

[diff-drive-controller] Move previous_publish_timestamp_ initialization out of exception block

5 participants