Program Listing for File odometry.hpp

↰ Return to documentation for file (include/diff_drive_controller/odometry.hpp)

// Copyright 2020 PAL Robotics S.L.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*
 * Author: Luca Marchionni
 * Author: Bence Magyar
 * Author: Enrique Fernández
 * Author: Paul Mathieu
 */

#ifndef DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_
#define DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_

#include "rclcpp/time.hpp"
// \note The versions conditioning is added here to support the source-compatibility with Humble
#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6
#include "rcpputils/rolling_mean_accumulator.hpp"
#else
#include "rcppmath/rolling_mean_accumulator.hpp"
#endif

namespace diff_drive_controller
{
class Odometry
{
public:
  explicit Odometry(size_t velocity_rolling_window_size = 10);

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

  bool update_from_pos(double left_pos, double right_pos, double dt);
  bool update_from_vel(double left_vel, double right_vel, double dt);
  bool try_update_open_loop(double linear_vel, double angular_vel, double dt);
  void setOdometry(double x, double y, double heading);
  [[deprecated("Use setOdometry(0.0, 0.0, 0.0) instead")]] void resetOdometry();

  double getX() const { return x_; }
  double getY() const { return y_; }
  double getHeading() const { return heading_; }
  double getLinear() const { return linear_; }
  double getAngular() const { return angular_; }

  void setWheelParams(double wheel_separation, double left_wheel_radius, double right_wheel_radius);
  void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);

private:
// \note The versions conditioning is added here to support the source-compatibility with Humble
#if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6
  using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator<double>;
#else
  using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
#endif

  [[deprecated("Replaced by void integrate(double linear_vel, double angular_vel, double dt).")]]
  void integrateRungeKutta2(double linear, double angular);
  [[deprecated("Replaced by void integrate(double linear_vel, double angular_vel, double dt).")]]
  void integrateExact(double linear, double angular);

  void integrate(double linear_vel, double angular_vel, double dt);
  void resetAccumulators();

  // Current timestamp:
  rclcpp::Time timestamp_;

  // Current pose:
  double x_;        //   [m]
  double y_;        //   [m]
  double heading_;  // [rad]

  // Current velocity:
  double linear_;   //   [m/s]
  double angular_;  // [rad/s]

  // Wheel kinematic parameters [m]:
  double wheel_separation_;
  double left_wheel_radius_;
  double right_wheel_radius_;

  // Previous wheel position/state [rad]:
  double left_wheel_old_pos_;
  double right_wheel_old_pos_;

  // Rolling mean accumulators for the linear and angular velocities:
  size_t velocity_rolling_window_size_;
  RollingMeanAccumulator linear_accumulator_;
  RollingMeanAccumulator angular_accumulator_;
};

}  // namespace diff_drive_controller

#endif  // DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_