steering_odometry.hpp

// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
//
// 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.
//
// Authors: dr. sc. Tomislav Petkovic, Dr. Ing. Denis Štogl
//
 
#ifndef STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_
#define STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_
 
#include <cmath>
#include <tuple>
#include <vector>
 
#include <rclcpp/time.hpp>
#include "rcppmath/rolling_mean_accumulator.hpp"
 
namespace steering_odometry
{
const unsigned int BICYCLE_CONFIG = 0;
const unsigned int TRICYCLE_CONFIG = 1;
const unsigned int ACKERMANN_CONFIG = 2;
 
inline bool is_close_to_zero(double val) { return std::fabs(val) < 1e-6; }
 
class SteeringOdometry
{
public:
  explicit SteeringOdometry(size_t velocity_rolling_window_size = 10);
 
  void init(const rclcpp::Time & time);
 
  bool update_from_position(
    const double traction_wheel_pos, const double steer_pos, const double dt);
 
  bool update_from_position(
    const double right_traction_wheel_pos, const double left_traction_wheel_pos,
    const double steer_pos, const double dt);
 
  bool update_from_position(
    const double right_traction_wheel_pos, const double left_traction_wheel_pos,
    const double right_steer_pos, const double left_steer_pos, const double dt);
 
  bool update_from_velocity(
    const double traction_wheel_vel, const double steer_pos, const double dt);
 
  bool update_from_velocity(
    const double right_traction_wheel_vel, const double left_traction_wheel_vel,
    const double steer_pos, const double dt);
 
  bool update_from_velocity(
    const double right_traction_wheel_vel, const double left_traction_wheel_vel,
    const double right_steer_pos, const double left_steer_pos, const double dt);
 
  void update_open_loop(const double v_bx, const double omega_bz, const double dt);
 
  void set_odometry_type(const unsigned int type);
 
  double get_heading() const { return heading_; }
 
  double get_x() const { return x_; }
 
  double get_y() const { return y_; }
 
  double get_linear() const { return linear_; }
 
  double get_angular() const { return angular_; }
 
  void set_wheel_params(
    const double wheel_radius, const double wheelbase = 0.0, const double wheel_track = 0.0);
 
  void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size);
 
  std::tuple<std::vector<double>, std::vector<double>> get_commands(
    const double v_bx, const double omega_bz, const bool open_loop = true,
    const bool reduce_wheel_speed_until_steering_reached = false);
 
  void reset_odometry();
 
private:
  bool update_odometry(const double v_bx, const double omega_bz, const double dt);
 
  void integrate_runge_kutta_2(const double v_bx, const double omega_bz, const double dt);
 
  void integrate_fk(const double v_bx, const double omega_bz, const double dt);
 
  double convert_twist_to_steering_angle(const double v_bx, const double omega_bz);
 
  double get_linear_velocity_double_traction_axle(
    const double right_traction_wheel_vel, const double left_traction_wheel_vel,
    const double steer_pos);
 
  void reset_accumulators();
 
  rclcpp::Time timestamp_;
 
  double x_;          //   [m]
  double y_;          //   [m]
  double steer_pos_;  // [rad]
  double heading_;    // [rad]
 
  double linear_;   //   [m/s]
  double angular_;  // [rad/s]
 
  double wheel_track_;   // [m]
  double wheelbase_;     // [m]
  double wheel_radius_;  // [m]
 
  int config_type_ = -1;
 
  double traction_wheel_old_pos_;
  double traction_right_wheel_old_pos_;
  double traction_left_wheel_old_pos_;
  size_t velocity_rolling_window_size_;
  rcppmath::RollingMeanAccumulator<double> linear_acc_;
  rcppmath::RollingMeanAccumulator<double> angular_acc_;
};
}  // namespace steering_odometry
 
#endif  // STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_