Program Listing for File steering_odometry.hpp

↰ Return to documentation for file (include/steering_controllers_library/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 <tuple>
#include <vector>

#include "realtime_tools/realtime_buffer.h"
#include "realtime_tools/realtime_publisher.h"

#include "rcpputils/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;
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 linear, const double angular, 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(double wheel_radius, double wheelbase = 0.0, double wheel_track = 0.0);

  void set_velocity_rolling_window_size(size_t velocity_rolling_window_size);

  std::tuple<std::vector<double>, std::vector<double>> get_commands(
    const double Vx, const double theta_dot);

  void reset_odometry();

private:
  bool update_odometry(const double linear_velocity, const double angular, const double dt);

  void integrate_runge_kutta_2(double linear, double angular);

  void integrate_exact(double linear, double angular);

  double convert_trans_rot_vel_to_steering_angle(double Vx, double theta_dot);

  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_;
  rcpputils::RollingMeanAccumulator<double> linear_acc_;
  rcpputils::RollingMeanAccumulator<double> angular_acc_;
};
}  // namespace steering_odometry

#endif  // STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_