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 7 commits into
base: master
Choose a base branch
from
+153 −45
Open

Update odometry implementation in diff_drive #1854

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 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
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);
Copy link
Contributor

Choose a reason for hiding this comment

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

where did the wheel_radius go?

Copy link
Contributor Author

Choose a reason for hiding this comment

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

My bad! This was meant to be multiplied in the updateFromVel() function. Fixed with d387750.

}
}

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.


// 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