diff --git a/diff_drive_controller/include/diff_drive_controller/odometry.hpp b/diff_drive_controller/include/diff_drive_controller/odometry.hpp index ae4417a788..21236688d0 100644 --- a/diff_drive_controller/include/diff_drive_controller/odometry.hpp +++ b/diff_drive_controller/include/diff_drive_controller/odometry.hpp @@ -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_; } @@ -60,8 +76,12 @@ class Odometry using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator; #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: diff --git a/diff_drive_controller/src/diff_drive_controller.cpp b/diff_drive_controller/src/diff_drive_controller.cpp index cb4f2edcee..1b57e0a733 100644 --- a/diff_drive_controller/src/diff_drive_controller.cpp +++ b/diff_drive_controller/src/diff_drive_controller.cpp @@ -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 { @@ -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(); + } } } diff --git a/diff_drive_controller/src/odometry.cpp b/diff_drive_controller/src/odometry.cpp index eb025ad23f..41a00ac698 100644 --- a/diff_drive_controller/src/odometry.cpp +++ b/diff_drive_controller/src/odometry.cpp @@ -73,6 +73,27 @@ 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; + + return updateFromVel(left_vel, right_vel, time); +} + bool Odometry::updateFromVelocity(double left_vel, double right_vel, const rclcpp::Time & time) { const double dt = time.seconds() - timestamp_.seconds(); @@ -100,6 +121,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(); + + // 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: @@ -112,6 +158,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; +} + void Odometry::resetOdometry() { x_ = 0.0; @@ -161,6 +224,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_);