add unir test to Ackerman motion model and update the interface of all models

Signed-off-by: fbattocchia <florencia.battochia@creativa77.com.ar>

Author: fbattocchia-ekumen

beluga/include/beluga/algorithm/amcl_core.hpp

@@ -162,12 +162,12 @@ class Amcl {
    * \return An optional pair containing the estimated pose and covariance after the update,
    *         or std::nullopt if no update was performed.
    */
-  auto update(state_type control_action, measurement_type measurement) -> std::optional<estimation_type> {
+  auto update(TimeStamped<state_type> control_action, measurement_type measurement) -> std::optional<estimation_type> {
     if (particles_.empty()) {
       return std::nullopt;
     }
 
-    if (!update_policy_(control_action) && !force_update_) {
+    if (!update_policy_(control_action.value) && !force_update_) {
       return std::nullopt;
     }
 
@@ -227,7 +227,7 @@ class Amcl {
 
   random_state_generator_type random_state_generator_;
 
-  beluga::RollingWindow<state_type, 2> control_action_window_;
+  beluga::RollingWindow<TimeStamped<state_type>, 2> control_action_window_;
 
   bool force_update_{true};
 };

beluga/include/beluga/motion.hpp

@@ -15,13 +15,18 @@
 #ifndef BELUGA_MOTION_HPP
 #define BELUGA_MOTION_HPP
 
+#include <beluga/motion/ackerman_drive_model.hpp>
 #include <beluga/motion/differential_drive_model.hpp>
 #include <beluga/motion/omnidirectional_drive_model.hpp>
 #include <beluga/motion/stationary_model.hpp>
 
 /**
  * \file
  * \brief Includes all Beluga motion models.
+ *
+ * Motion models in Beluga can be classified into two categories:
+ * - Position-based models: Use pose differences (DifferentialDriveModel, OmnidirectionalDriveModel, StationaryModel)
+ * - Velocity-based models: Use timestamped poses to calculate velocities (VelocityDriveModel)
  */
 
 /**
@@ -61,6 +66,7 @@
  * - beluga::DifferentialDriveModel
  * - beluga::OmnidirectionalDriveModel
  * - beluga::StationaryModel
+ * - beluga::VelocityDriveModel
  */
 
 #endif

beluga/include/beluga/motion/ackerman_drive_model.hpp

@@ -21,6 +21,7 @@
 #include <tuple>
 
 #include <beluga/type_traits/tuple_traits.hpp>
+#include <beluga/utility/time_stamped.hpp>
 
 #include <beluga/3d_embedding.hpp>
 #include <sophus/se2.hpp>
@@ -39,33 +40,42 @@ namespace beluga {
  * See Probabilistic Robotics \cite thrun2005probabilistic Chapter 5.3, particularly table 5.3.
  */
 struct VelocityDriveModelParam {
+  /// Rotational noise from rotational velocity
+  /**
+   * How much rotational noise is generated by the rotational velocity.
+   * Also known as `alpha1 in the differential drive model param`.
+   */
+  double rotation_noise_from_rotation;
+  /// Rotational noise from translation velocity
+  /**
+   * How much rotational noise is generated by the linear velocity.
+   * Also known as `alpha2 in the differential drive model param`.
+   */
+  double rotation_noise_from_translation;
   /// Translational noise from translation velocity
   /**
-   * How much translational noise is generated by the translational velocity.
-   * Also known as `alpha1`.
+   * How much translational noise is generated by the linear velocity.
+   * Also known as `alpha3 in the differential drive model param`.
    */
   double translation_noise_from_translation;
   /// Translational noise from rotational velocity
   /**
    * How much translational noise is generated by the rotational velocity.
-   * Also known as `alpha2`.
+   * Also known as `alpha4 in the differential drive model param`.
    */
   double translation_noise_from_rotation;
-  /// Rotational noise from translation velocity
+  /// Additional orientation noise from translational velocity
   /**
-   * How much rotational noise is generated by the translational velocity.
-   * Also known as `alpha3`.
+   * How much extra orientation noise is generated by the linear velocity.
+   * Also known as `alpha6`.
    */
-  double rotation_noise_from_translation;
-  /// Rotational noise from rotational velocity
+  double orientation_noise_from_translation;
+  /// Additional orientation noise from rotational velocity
   /**
-   * How much rotational noise is generated by the rotational velocity.
-   * Also known as `alpha4`.
+   * How much extra orientation noise is generated by the rotational velocity.
+   * Also known as `alpha7`.
    */
-  double rotation_noise_from_rotation;
-
-  /// Distance threshold to detect in-place rotation.
-  double distance_threshold = 0.01;
+  double orientation_noise_from_rotation;
 };
 
 /// Sampled velocity model for a differential drive.
@@ -88,10 +98,10 @@ class VelocityDriveModel {
   using state_type = StateType;
 
   /// Time point type for motion model control actions.
-  using time_point_type = std::chrono::time_point<std::chrono::steady_clock>;
+  using timestamped_state_type = TimeStamped<state_type>;
 
   /// Current and previous pose estimates and time points as motion model control action.
-  using control_type = std::tuple<state_type, state_type, time_point_type, time_point_type>;
+  using control_type = std::tuple<timestamped_state_type, timestamped_state_type>;
 
   /// Parameter type that the constructor uses to configure the motion model.
   using param_type = VelocityDriveModelParam;
@@ -111,7 +121,12 @@ class VelocityDriveModel {
    */
   template <class Control, typename = common_tuple_type_t<Control, control_type>>
   [[nodiscard]] auto operator()(const Control& action) const {
-    const auto& [pose, previous_pose, time, previous_time] = action;
+    const auto& [timestamped, previous_timestamped] = action;
+    const auto& pose = timestamped.value;
+    const auto& previous_pose = previous_timestamped.value;
+
+    auto time = timestamped.timestamp;
+    auto previous_time = previous_timestamped.timestamp;
     const auto delta_time = std::chrono::duration<double>(time - previous_time).count();
     if constexpr (std::is_same_v<state_type, Sophus::SE2d>) {
       return sampling_fn_2d(pose, previous_pose, delta_time);
@@ -121,8 +136,8 @@ class VelocityDriveModel {
   }
 
  private:
-  using control_type_2d = std::tuple<Sophus::SE2d, Sophus::SE2d, double>;
-  using control_type_3d = std::tuple<Sophus::SE3d, Sophus::SE3d, double>;
+  using control_type_2d = std::tuple<Sophus::SE2d, Sophus::SE2d>;
+  using control_type_3d = std::tuple<Sophus::SE3d, Sophus::SE3d>;
 
   [[nodiscard]] auto sampling_fn_3d(const Sophus::SE3d& pose, const Sophus::SE3d& previous_pose, double delta_time)
       const {
@@ -154,8 +169,8 @@ class VelocityDriveModel {
     const auto gamma_params = DistributionParam{
         0.0,  // zero mean
         std::sqrt(
-            params_.rotation_noise_from_translation * std::abs(linear_velocity) +
-            params_.rotation_noise_from_rotation * std::abs(angular_velocity))};
+            params_.orientation_noise_from_translation * std::abs(linear_velocity) +
+            params_.orientation_noise_from_rotation * std::abs(angular_velocity))};
 
     return [=](const auto& state, auto& gen) {
       static thread_local auto distribution = std::normal_distribution<double>{};
@@ -173,24 +188,38 @@ class VelocityDriveModel {
   /// Calculate linear and angular velocities from two poses and delta time
   std::pair<double, double>
   calculate_velocities(const Sophus::SE2d& pose, const Sophus::SE2d& previous_pose, double delta_time) const {
-    // Distancia euclidiana
+    // Euclidean distance (chord length between poses)
     const auto translation = pose.translation() - previous_pose.translation();
-    const double distance = translation.norm();
+    const double chord_distance = translation.norm();
 
-    // Velocidad angular
+    // Angular velocity from orientation change
     const auto angular_change = pose.so2() * previous_pose.so2().inverse();
     const double angle_change = angular_change.log();
     const double angular_velocity = angle_change / delta_time;
 
-    // Velocidad lineal
+    // Determine direction sign (forward/backward motion)
+    const auto forward_direction =
+        Eigen::Vector2d{std::cos(previous_pose.so2().log()), std::sin(previous_pose.so2().log())};
+    const double dot_product = translation.dot(forward_direction);
+    const double sign = (dot_product >= 0) ? 1.0 : -1.0;
+
+    // Linear velocity calculation
     double linear_velocity = 0.0;
+
     if (std::abs(angle_change) > 1e-6) {
-      // v = ω · r
-      const double radius = distance / std::abs(angle_change);
-      linear_velocity = std::abs(angular_velocity) * radius;
+      // Circular motion: calculate radius from chord and angle
+      // For an arc: chord = 2r·sin(θ/2), therefore r = chord / (2·sin(θ/2))
+      const double radius = chord_distance / (2.0 * std::sin(std::abs(angle_change) / 2.0));
+
+      // Arc length: s = r · θ
+      const double arc_distance = radius * std::abs(angle_change);
+
+      // Linear velocity with direction sign
+      linear_velocity = sign * arc_distance / delta_time;
+
     } else {
-      // Movimiento rectilíneo: v = distancia/tiempo
-      linear_velocity = distance / delta_time;
+      // Straight line motion: v = distance / time
+      linear_velocity = sign * chord_distance / delta_time;
     }
 
     return {linear_velocity, angular_velocity};
@@ -206,7 +235,7 @@ class VelocityDriveModel {
 
     Sophus::SE2d new_pose;
 
-    if (std::abs(omega_hat) < 1e-6) {
+    if (std::abs(omega_hat) < 1e-4) {
       // Nearly straight line motion
       const auto translation =
           Eigen::Vector2d{v_hat * delta_time * std::cos(current_theta), v_hat * delta_time * std::sin(current_theta)};

beluga/include/beluga/motion/differential_drive_model.hpp

@@ -20,6 +20,7 @@
 #include <tuple>
 
 #include <beluga/type_traits/tuple_traits.hpp>
+#include <beluga/utility/time_stamped.hpp>
 
 #include <beluga/3d_embedding.hpp>
 #include <sophus/se2.hpp>
@@ -86,8 +87,11 @@ class DifferentialDriveModel {
   /// 2D or flattened 3D pose as motion model state (to match that of the particles).
   using state_type = StateType;
 
+  /// Time point type for motion model control actions.
+  using timestamped_state_type = TimeStamped<state_type>;
+
   /// Current and previous odometry estimates as motion model control action.
-  using control_type = std::tuple<state_type, state_type>;
+  using control_type = std::tuple<timestamped_state_type, timestamped_state_type>;
 
   /// Parameter type that the constructor uses to configure the motion model.
   using param_type = DifferentialDriveModelParam;
@@ -107,7 +111,9 @@ class DifferentialDriveModel {
    */
   template <class Control, typename = common_tuple_type_t<Control, control_type>>
   [[nodiscard]] auto operator()(const Control& action) const {
-    const auto& [pose, previous_pose] = action;
+    const auto& [timestamped, previous_timestamped] = action;
+    const auto& pose = timestamped.value;
+    const auto& previous_pose = previous_timestamped.value;
     if constexpr (std::is_same_v<state_type, Sophus::SE2d>) {
       return sampling_fn_2d(pose, previous_pose);
     } else {

beluga/include/beluga/motion/omnidirectional_drive_model.hpp

@@ -21,6 +21,7 @@
 #include <type_traits>
 
 #include <beluga/type_traits/tuple_traits.hpp>
+#include <beluga/utility/time_stamped.hpp>
 
 #include <sophus/se2.hpp>
 #include <sophus/so2.hpp>
@@ -77,11 +78,12 @@ struct OmnidirectionalDriveModelParam {
  */
 class OmnidirectionalDriveModel {
  public:
-  /// Current and previous odometry estimates as motion model control action.
-  using control_type = std::tuple<Sophus::SE2d, Sophus::SE2d>;
   /// 2D pose as motion model state (to match that of the particles).
   using state_type = Sophus::SE2d;
-
+  /// Time point type for motion model control actions.
+  using timestamped_state_type = TimeStamped<state_type>;
+  /// Current and previous odometry estimates as motion model control action.
+  using control_type = std::tuple<timestamped_state_type, timestamped_state_type>;
   /// Parameter type that the constructor uses to configure the motion model.
   using param_type = OmnidirectionalDriveModelParam;
 
@@ -100,7 +102,9 @@ class OmnidirectionalDriveModel {
    */
   template <class Control, typename = common_tuple_type_t<Control, control_type>>
   [[nodiscard]] auto operator()(Control&& action) const {
-    const auto& [pose, previous_pose] = action;
+    const auto& [timestamped, previous_timestamped] = action;
+    const auto& pose = timestamped.value;
+    const auto& previous_pose = previous_timestamped.value;
 
     const auto translation = pose.translation() - previous_pose.translation();
     const double distance = translation.norm();

beluga/include/beluga/motion/stationary_model.hpp

@@ -38,11 +38,12 @@ namespace beluga {
  */
 class StationaryModel {
  public:
-  /// Current and previous odometry estimates as motion model control action.
-  using control_type = std::tuple<Sophus::SE2d, Sophus::SE2d>;
   /// 2D pose as motion model state (to match that of the particles).
   using state_type = Sophus::SE2d;
-
+  /// Time point type for motion model control actions.
+  using timestamped_state_type = TimeStamped<state_type>;
+  /// Current and previous odometry estimates as motion model control action.
+  using control_type = std::tuple<timestamped_state_type, timestamped_state_type>;
   /// Computes a state sampling function conditioned on a given control action.
   /**
    * The updated state will be centered around `state` with some covariance.

beluga/include/beluga/utility/time_stamped.hpp

@@ -0,0 +1,82 @@
+// Copyright 2023-2024 Ekumen, Inc.
+//
+// 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.
+
+#ifndef BELUGA_UTILITY_TIME_STAMPED_HPP
+#define BELUGA_UTILITY_TIME_STAMPED_HPP
+
+#include <chrono>
+
+/**
+ * \file
+ * \brief Implementation of a time-stamped wrapper for values.
+ */
+
+namespace beluga {
+
+/// A wrapper that associates a value with a timestamp.
+/**
+ * This structure provides a way to bundle any value with its associated timestamp,
+ * which is commonly needed in motion models and sensor processing where timing
+ * information is crucial for proper calculations.
+ *
+ * \tparam T The type of the value to be time-stamped.
+ * \tparam ClockT The clock type to use for timestamps. Defaults to system_clock.
+ */
+template <typename T, typename ClockT = std::chrono::system_clock>
+struct TimeStamped {
+  /// The wrapped value.
+  T value;
+
+  /// The timestamp associated with the value.
+  std::chrono::time_point<ClockT> timestamp;
+  /// Default constructor.
+  /**
+   * Initializes value with default construction and timestamp to epoch.
+   */
+  TimeStamped() : value{}, timestamp{} {}
+
+  /// Constructs a TimeStamped with the current time.
+  /**
+   * \param val The value to be time-stamped.
+   */
+  explicit TimeStamped(const T& val) : value(val), timestamp{} {}
+
+  /// Constructs a TimeStamped with a specific timestamp.
+  /**
+   * \param val The value to be time-stamped.
+   * \param ts The timestamp to associate with the value.
+   */
+  TimeStamped(const T& val, std::chrono::time_point<ClockT> ts) : value(val), timestamp(ts) {}
+};
+
+/// Helper function to create a control action tuple with TimeStamped values
+template <typename T>
+auto make_control_action(const T& current, const T& previous) {
+  return std::make_tuple(TimeStamped{current}, TimeStamped{previous});
+}
+
+/// Helper function to create a control action tuple with TimeStamped values and explicit timestamps
+template <typename T, typename ClockT = std::chrono::system_clock>
+auto make_control_action(
+    const T& current,
+    const T& previous,
+    std::chrono::time_point<ClockT> current_timestamp,
+    std::chrono::time_point<ClockT> previous_timestamp) {
+  return std::make_tuple(
+      TimeStamped<T, ClockT>{current, current_timestamp}, TimeStamped<T, ClockT>{previous, previous_timestamp});
+}
+
+}  // namespace beluga
+
+#endif

beluga/test/beluga/CMakeLists.txt

@@ -33,6 +33,7 @@ add_executable(
   algorithm/test_unscented_transform.cpp
   containers/test_circular_array.cpp
   containers/test_tuple_vector.cpp
+  motion/test_ackerman_drive_model.cpp
   motion/test_differential_drive_model.cpp
   motion/test_omnidirectional_drive_model.cpp
   policies/test_every_n.cpp