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_

#ifdef _WIN32
#pragma message( \
  "This header is deprecated. Please update your code to use 'steering_kinematics.hpp' header.")  // NOLINT
#else
#warning \
  "This header is deprecated. Please update your code to use 'steering_kinematics.hpp' header." //NOLINT
#endif

#include "steering_controllers_library/steering_kinematics.hpp"

#include <cmath>
#include <tuple>
#include <vector>

#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 steering_odometry
{
[[deprecated("Use steering_kinematics::BICYCLE_CONFIG")]] const unsigned int BICYCLE_CONFIG =
  steering_kinematics::BICYCLE_CONFIG;
[[deprecated("Use steering_kinematics::TRICYCLE_CONFIG")]] const unsigned int TRICYCLE_CONFIG =
  steering_kinematics::TRICYCLE_CONFIG;
[[deprecated("Use steering_kinematics::ACKERMANN_CONFIG")]] const unsigned int ACKERMANN_CONFIG =
  steering_kinematics::ACKERMANN_CONFIG;

inline bool is_close_to_zero(double val) { return steering_kinematics::is_close_to_zero(val); }

class [[deprecated("Use steering_kinematics::SteeringKinematics instead")]] 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);

  unsigned int get_odometry_type() const;
  double get_heading() const;

  double get_x() const;

  double get_y() const;

  double get_linear() const;

  double get_angular() const;

  void set_wheel_params(
    const double wheel_radius, const double wheel_base, const double wheel_track);

  void set_wheel_params(
    const double wheel_radius, const double wheel_base, const double wheel_track_steering,
    const double wheel_track_traction);

  void set_velocity_rolling_window_size(const size_t velocity_rolling_window_size);

  std::tuple<std::vector<double>, std::vector<double>> get_commands(
    double v_bx, double omega_bz, bool open_loop = true,
    bool reduce_wheel_speed_until_steering_reached = false);

  void reset_odometry();

private:
  steering_kinematics::SteeringKinematics sk_impl_;
};
}  // namespace steering_odometry

#endif  // STEERING_CONTROLLERS_LIBRARY__STEERING_ODOMETRY_HPP_