22 #ifndef DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_
23 #define DIFF_DRIVE_CONTROLLER__ODOMETRY_HPP_
27 #include "rclcpp/time.hpp"
29 #if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6
30 #include "rcpputils/rolling_mean_accumulator.hpp"
32 #include "rcppmath/rolling_mean_accumulator.hpp"
35 namespace diff_drive_controller
40 explicit Odometry(
size_t velocity_rolling_window_size = 10);
42 void init(
const rclcpp::Time & time);
43 bool update(
double left_pos,
double right_pos,
const rclcpp::Time & time);
44 bool updateFromVelocity(
double left_vel,
double right_vel,
const rclcpp::Time & time);
45 void updateOpenLoop(
double linear,
double angular,
const rclcpp::Time & time);
48 double getX()
const {
return x_; }
49 double getY()
const {
return y_; }
50 double getHeading()
const {
return heading_; }
51 double getLinear()
const {
return linear_; }
52 double getAngular()
const {
return angular_; }
54 void setWheelParams(
double wheel_separation,
double left_wheel_radius,
double right_wheel_radius);
55 void setVelocityRollingWindowSize(
size_t velocity_rolling_window_size);
59 #if RCPPUTILS_VERSION_MAJOR >= 2 && RCPPUTILS_VERSION_MINOR >= 6
60 using RollingMeanAccumulator = rcpputils::RollingMeanAccumulator<double>;
62 using RollingMeanAccumulator = rcppmath::RollingMeanAccumulator<double>;
65 void integrateRungeKutta2(
double linear,
double angular);
66 void integrateExact(
double linear,
double angular);
67 void resetAccumulators();
70 rclcpp::Time timestamp_;
82 double wheel_separation_;
83 double left_wheel_radius_;
84 double right_wheel_radius_;
87 double left_wheel_old_pos_;
88 double right_wheel_old_pos_;
91 size_t velocity_rolling_window_size_;
92 RollingMeanAccumulator linear_accumulator_;
93 RollingMeanAccumulator angular_accumulator_;
Definition: odometry.hpp:38
void updateOpenLoop(double linear, double angular, const rclcpp::Time &time)
Definition: odometry.cpp:98