Skip to content

Update odometry implementation in diff_drive #1854

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
wants to merge 16 commits into
base: master
Choose a base branch
from
+138 −44
Open

Update odometry implementation in diff_drive #1854

Show file tree
Hide file tree
Changes from 7 commits
Commits
Show all changes
16 commits
Select commit Hold shift + click to select a range
fe3a568
Update odometry implementation in diff_drive
Amronos Aug 6, 2025
13edbec
Apply suggestions from code review
Amronos Aug 20, 2025
629cf5e
Fix deprecation warning
Amronos Aug 20, 2025
d387750
Multiply by wheel radius in updateFromVel
Amronos Aug 20, 2025
bd1283b
Fix pre-commit
Amronos Aug 20, 2025
3048023
Merge branch 'master' into update-diff_drive-odom
Amronos Sep 4, 2025
93806d6
Fix build errors
Amronos Sep 4, 2025
1d47fed
Update diff_drive_controller/src/odometry.cpp
Amronos Sep 9, 2025
a52855b
Merge branch 'master' into update-diff_drive-odom
Amronos Sep 9, 2025
77eade3
Apply suggestions from code review
Amronos Sep 24, 2025
831c9c7
Merge branch 'master' into update-diff_drive-odom
Amronos Sep 24, 2025
a95a450
Fix pre-commit
Amronos Sep 24, 2025
8ecb548
Fix pre-commit
Amronos Sep 24, 2025
ec25473
Fix pre-commit
Amronos Oct 3, 2025
3e04f37
Merge branch 'master' into update-diff_drive-odom
Amronos Oct 3, 2025
93b0c0c
Merge branch 'master' into update-diff_drive-odom
Amronos Oct 5, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
20 changes: 20 additions & 0 deletions diff_drive_controller/include/diff_drive_controller/odometry.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,10 +37,26 @@ class Odometry
public:
explicit Odometry(size_t velocity_rolling_window_size = 10);

[[deprecated]]
void init(const rclcpp::Time & time);
[[deprecated(
"Replaced by bool updateFromPos(const double left_pos, const double right_pos, const "
"rclcpp::Time & time).")]]
bool update(double left_pos, double right_pos, const rclcpp::Time & time);
[[deprecated(
"Replaced by bool updateFromVel(const double left_vel, const double right_vel, const "
"rclcpp::Time & time).")]]
bool updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time);
[[deprecated(
"Replaced by bool tryUpdateOpenLoop(const double linear_vel, const double angular_vel, const "
"rclcpp::Time "
"& time).")]]
void updateOpenLoop(double linear, double angular, const rclcpp::Time & time);

bool updateFromPos(const double left_pos, const double right_pos, const rclcpp::Time & time);
bool updateFromVel(const double left_vel, const double right_vel, const rclcpp::Time & time);
bool tryUpdateOpenLoop(
const double linear_vel, const double angular_vel, const rclcpp::Time & time);
void resetOdometry();

double getX() const { return x_; }
Expand All @@ -60,8 +76,12 @@ class Odometry
using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
#endif

[[deprecated("Replaced by void integrate(const double & dx, const double & dheading).")]]
void integrateRungeKutta2(double linear, double angular);
[[deprecated("Replaced by void integrate(const double & dx, const double & dheading).")]]
void integrateExact(double linear, double angular);

void integrate(const double dx, const double dheading);
void resetAccumulators();

// Current timestamp:
Expand Down
93 changes: 48 additions & 45 deletions diff_drive_controller/src/diff_drive_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -160,9 +160,11 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
const double left_wheel_radius = params_.left_wheel_radius_multiplier * params_.wheel_radius;
const double right_wheel_radius = params_.right_wheel_radius_multiplier * params_.wheel_radius;

// Update odometry
bool odometry_updated = false;
if (params_.open_loop)
{
odometry_.updateOpenLoop(linear_command, angular_command, time);
odometry_updated = odometry_.tryUpdateOpenLoop(linear_command, angular_command, time);
}
else
{
Expand Down Expand Up @@ -200,63 +202,64 @@ controller_interface::return_type DiffDriveController::update_and_write_commands

if (params_.position_feedback)
{
odometry_.update(left_feedback_mean, right_feedback_mean, time);
odometry_updated = odometry_.updateFromPos(left_feedback_mean, right_feedback_mean, time);
}
else
{
odometry_.updateFromVelocity(
left_feedback_mean * left_wheel_radius * period.seconds(),
right_feedback_mean * right_wheel_radius * period.seconds(), time);
odometry_updated = odometry_.updateFromVel(left_feedback_mean, right_feedback_mean, time);
}
}

tf2::Quaternion orientation;
orientation.setRPY(0.0, 0.0, odometry_.getHeading());

bool should_publish = false;
try
if (odometry_updated)
{
if (previous_publish_timestamp_ + publish_period_ < time)
tf2::Quaternion orientation;
orientation.setRPY(0.0, 0.0, odometry_.getHeading());

bool should_publish = false;
try
{
previous_publish_timestamp_ += publish_period_;
should_publish = true;
if (previous_publish_timestamp_ + publish_period_ < time)
{
previous_publish_timestamp_ += publish_period_;
should_publish = true;
}
}
}
catch (const std::runtime_error &)
{
// Handle exceptions when the time source changes and initialize publish timestamp
previous_publish_timestamp_ = time;
should_publish = true;
}

if (should_publish)
{
if (realtime_odometry_publisher_->trylock())
catch (const std::runtime_error &)
{
auto & odometry_message = realtime_odometry_publisher_->msg_;
odometry_message.header.stamp = time;
odometry_message.pose.pose.position.x = odometry_.getX();
odometry_message.pose.pose.position.y = odometry_.getY();
odometry_message.pose.pose.orientation.x = orientation.x();
odometry_message.pose.pose.orientation.y = orientation.y();
odometry_message.pose.pose.orientation.z = orientation.z();
odometry_message.pose.pose.orientation.w = orientation.w();
odometry_message.twist.twist.linear.x = odometry_.getLinear();
odometry_message.twist.twist.angular.z = odometry_.getAngular();
realtime_odometry_publisher_->unlockAndPublish();
// Handle exceptions when the time source changes and initialize publish timestamp
previous_publish_timestamp_ = time;
should_publish = true;
}

if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock())
if (should_publish)
{
auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front();
transform.header.stamp = time;
transform.transform.translation.x = odometry_.getX();
transform.transform.translation.y = odometry_.getY();
transform.transform.rotation.x = orientation.x();
transform.transform.rotation.y = orientation.y();
transform.transform.rotation.z = orientation.z();
transform.transform.rotation.w = orientation.w();
realtime_odometry_transform_publisher_->unlockAndPublish();
if (realtime_odometry_publisher_->trylock())
{
auto & odometry_message = realtime_odometry_publisher_->msg_;
odometry_message.header.stamp = time;
odometry_message.pose.pose.position.x = odometry_.getX();
odometry_message.pose.pose.position.y = odometry_.getY();
odometry_message.pose.pose.orientation.x = orientation.x();
odometry_message.pose.pose.orientation.y = orientation.y();
odometry_message.pose.pose.orientation.z = orientation.z();
odometry_message.pose.pose.orientation.w = orientation.w();
odometry_message.twist.twist.linear.x = odometry_.getLinear();
odometry_message.twist.twist.angular.z = odometry_.getAngular();
realtime_odometry_publisher_->unlockAndPublish();
}

if (params_.enable_odom_tf && realtime_odometry_transform_publisher_->trylock())
{
auto & transform = realtime_odometry_transform_publisher_->msg_.transforms.front();
transform.header.stamp = time;
transform.transform.translation.x = odometry_.getX();
transform.transform.translation.y = odometry_.getY();
transform.transform.rotation.x = orientation.x();
transform.transform.rotation.y = orientation.y();
transform.transform.rotation.z = orientation.z();
transform.transform.rotation.w = orientation.w();
realtime_odometry_transform_publisher_->unlockAndPublish();
}
}
}

Expand Down
85 changes: 85 additions & 0 deletions diff_drive_controller/src/odometry.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -73,6 +73,31 @@ bool Odometry::update(double left_pos, double right_pos, const rclcpp::Time & ti
return true;
}

bool Odometry::updateFromPos(
const double left_pos, const double right_pos, const rclcpp::Time & time)
{
const double dt = time.seconds() - timestamp_.seconds();
// We cannot estimate angular velocity with very small time intervals
if (std::fabs(dt) < 1e-6)
{
return false;
}

// Estimate angular velocity of wheels using old and current position [rads/s]:
double left_vel = (left_pos - left_wheel_old_pos_) / dt;
double right_vel = (right_pos - right_wheel_old_pos_) / dt;

// Update old position with current:
left_wheel_old_pos_ = left_pos;
right_wheel_old_pos_ = right_pos;

if (updateFromVel(left_vel, right_vel, time))
{
return true;
}
return false;
}

bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time)
{
const double dt = time.seconds() - timestamp_.seconds();
Expand Down Expand Up @@ -100,6 +125,31 @@ bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcp
return true;
}

bool Odometry::updateFromVel(
const double left_vel, const double right_vel, const rclcpp::Time & time)
{
const double dt = time.seconds() - timestamp_.seconds();
Copy link
Contributor

Choose a reason for hiding this comment

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

this is exactly the pattern mentioned with #271 to be removed?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Sorry, I should have explained this in the PR description.
The reasoning here is the same as that of this #260 (comment).
Additionally, timestamp_ records the last time an update was run; therefore, the correct value of dt can only be obtained from there.
It is used in this function despite (std::fabs(dt) < 1e-6) not being used, as it is also called from updateFromPos() which uses the timestamp_ variable.

Copy link
Contributor

Choose a reason for hiding this comment

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

Sorry, I still don't understand. Why not using the period argument from the update method and pass it to the update method?

Copy link
Contributor Author

@Amronos Amronos Sep 9, 2025 •
edited
Loading

Choose a reason for hiding this comment

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

  • Basically due to the if (std::fabs(dt) < 1e-6) {return false;} check in updateFromPos() we need to maintain the timestamp_ variable.
  • timestamp_ is only updated when the check returns false, and we continue with the code. If timestamp_ was updated every time, it would yield incorrect results, as we only update odometry when the code passes through that check.
  • If period was directly passed, then it would yield an incorrect dt (dt will now be replaced with period), as the current way to calculate dt is through the timestamp_, which is not updated every time the method is called.
  • The updateFromVel() function can be changed to use period if the arguments of updateFromPos() remain the same, and we pass it dt from there when using position feedback, and when using velocity feedback, we pass period from the main update method. This can only be done, though, if we are sure that the user can't/won't switch between velocity and position feedback while the controller is running (add read_only parameters to diff_drive_controller #1781).

Copy link
Contributor

Choose a reason for hiding this comment

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

Thanks, I understand the motivation now.

  1. Is the period argument ever that small so we need the check? (might come from ROS 1 days)
  2. If so (maybe the first update call after activation or something like this. This can't happen on a regular basis), Is the error really that high if we use the period and still skip if it is close to zero?

Copy link
Contributor

Choose a reason for hiding this comment

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

ros-controls/ros2_control#2336

Copy link
Contributor Author

Choose a reason for hiding this comment

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

  1. I also thought about this, as the period argument would only be this small if the update rate is 10^6; I don't think people set the update rate that high, but I still kept the check. Because of Period is zero at first loop of ros controller ros2_control#2336, ig we will either way need it.
  2. Didn't think about it like that :). There would only be a minor 10^-6 or less seconds unaccounted for, which shouldn't have any effect on the odometry. That is assuming that the update rate would never be set that high to make this error accumulate.

So should I pass period to both the functions, in updateFromPos() keep the zero check, and stop the use of timestamp_ in the new functions?

Copy link
Contributor

Choose a reason for hiding this comment

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

yes please do so 👍

Copy link
Contributor Author

Choose a reason for hiding this comment

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

Done in 77eade3.


// Compute linear and angular velocities of the robot:
const double linear_vel = (left_vel * left_wheel_radius_ + right_vel * right_wheel_radius_) * 0.5;
const double angular_vel =
(right_vel * right_wheel_radius_ - left_vel * left_wheel_radius_) / wheel_separation_;

// Integrate odometry:
integrate(linear_vel * dt, angular_vel * dt);

timestamp_ = time;

// Estimate speeds using a rolling mean to filter them out:
linear_accumulator_.accumulate(linear_vel);
angular_accumulator_.accumulate(angular_vel);

linear_ = linear_accumulator_.getRollingMean();
angular_ = angular_accumulator_.getRollingMean();

return true;
}

void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time & time)
{
/// Save last linear and angular velocity:
Expand All @@ -112,6 +162,23 @@ void Odometry::updateOpenLoop(double linear, double angular, const rclcpp::Time
integrateExact(linear * dt, angular * dt);
}

bool Odometry::tryUpdateOpenLoop(
const double linear_vel, const double angular_vel, const rclcpp::Time & time)
{
const double dt = time.seconds() - timestamp_.seconds();

// Integrate odometry:
integrate(linear_vel * dt, angular_vel * dt);

timestamp_ = time;

// Save last linear and angular velocity:
linear_ = linear_vel;
angular_ = angular_vel;

return true;
Copy link
Contributor

Choose a reason for hiding this comment

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

why a return value, which is always true?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

This is there to maintain consistency with other update methods, make it easier to add code to this function in the future that may return false, and make the diff_drive_controller.cpp code simpler.
updateFromVel() also has this line for the same reason.
I can change everything back to the normal updateOpenLoop() function if needed, though.

}

void Odometry::resetOdometry()
{
x_ = 0.0;
Expand Down Expand Up @@ -161,6 +228,24 @@ void Odometry::integrateExact(double linear, double angular)
}
}

void Odometry::integrate(const double dx, const double dheading)
{
if (fabs(dheading) < 1e-6)
{
// For very small dheading, approximate to linear motion
x_ += (dx * std::cos(heading_));
y_ += (dx * std::sin(heading_));
heading_ += dheading;
}
else
{
const double heading_old = heading_;
heading_ += dheading;
x_ += ((dx / dheading) * (std::sin(heading_) - std::sin(heading_old)));
y_ += -(dx / dheading) * (std::cos(heading_) - std::cos(heading_old));
}
}

void Odometry::resetAccumulators()
{
linear_accumulator_ = RollingMeanAccumulator(velocity_rolling_window_size_);
Expand Down