odometry.hpp

// Copyright 2022 Pixel Robotics.
//
// 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: Tony Najjar
 */
 
#ifndef TRICYCLE_CONTROLLER__ODOMETRY_HPP_
#define TRICYCLE_CONTROLLER__ODOMETRY_HPP_
 
#include <cmath>
 
#include "rclcpp/time.hpp"
#include "rcppmath/rolling_mean_accumulator.hpp"
 
namespace tricycle_controller
{
class Odometry
{
public:
  explicit Odometry(size_t velocity_rolling_window_size = 10);
 
  bool update(double left_vel, double right_vel, const rclcpp::Duration & dt);
  void updateOpenLoop(double linear, double angular, const rclcpp::Duration & dt);
  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 wheel_radius);
  void setVelocityRollingWindowSize(size_t velocity_rolling_window_size);
 
private:
  using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
 
  void integrateRungeKutta2(double linear, double angular);
  void integrateExact(double linear, double angular);
  void resetAccumulators();
 
  // 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 wheelbase_;
  double wheel_radius_;
 
  // Rolling mean accumulators for the linear and angular velocities:
  size_t velocity_rolling_window_size_;
  RollingMeanAccumulator linear_accumulator_;
  RollingMeanAccumulator angular_accumulator_;
};
 
}  // namespace tricycle_controller
 
#endif  // TRICYCLE_CONTROLLER__ODOMETRY_HPP_