feat: migrate lerp function from dwa_planner to wheel_stuck_utils (#39)

* migrate lerp function from dwa_planner to wheel_stuck_utils

Signed-off-by: Autumn60 <harada.akiro@gmail.com>

* chore: Use std::clamp instead of std::min and std::max for lerp function

Signed-off-by: Autumn60 <harada.akiro@gmail.com>

* Feat/common/add math utils (#44)

* add clamp function

* add clamp01 function

* add clamp01 function

* add clamp01 function

* add clamp01 function

* fix lerp function

* fix lerp function

* Feat/common/add math utils (#46)

---------

Signed-off-by: Autumn60 <harada.akiro@gmail.com>
Co-authored-by: Bey9434 <134945706+Bey9434@users.noreply.github.com>

src/common/wheel_stuck_utils/include/wheel_stuck_utils/math/math.hpp

@@ -0,0 +1,70 @@
+#ifndef WHEEL_STUCK_UTILS__MATH__MATH_HPP_
+#define WHEEL_STUCK_UTILS__MATH__MATH_HPP_
+
+#include <algorithm>
+
+namespace wheel_stuck_utils::math
+{
+inline double clamp01(const double value)
+{
+  return std::clamp(value, 0.0, 1.0);
+}
+
+inline double lerp(const double a, const double b, const double t)
+{
+  return a + clamp01(t) * (b - a);
+}
+
+// lerpの逆関数
+inline double inverse_lerp(const double a, const double b, const double value)
+{
+  if (a != b) {
+    return clamp01((value - a) / (b - a));
+  } else {
+    return 0;  // ゼロ除算のため値を定義し得ないが、便宜上0を返す。
+  }
+}
+
+// 符号(-1, 0, 1)を返す
+inline int sign(const double i)
+{
+  return (i > 0) ? +1 : (i < 0) ? -1 : 0;
+  /*
+  if(i > 0)
+    return 1;
+  else if(i < 0)
+    return -1;
+  else
+    return 0;
+  */
+}
+
+// 可変長引数を受け取り最小値を返す
+template <typename T>
+inline T min(T a)
+{
+  return a;
+}
+
+template <typename T, typename... Args>
+inline T min(T a, Args... args)
+{
+  return std::min(a, min(args...));
+}
+
+// 可変長引数を受け取り最大値を返す
+template <typename T>
+inline T max(T b)
+{
+  return b;
+}
+
+template <typename T, typename... Args>
+inline T max(T b, Args... args)
+{
+  return std::max(b, max(args...));
+}
+
+}  // namespace wheel_stuck_utils::math
+
+#endif  // WHEEL_STUCK_UTILS__MATH__MATH_HPP_

src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp

@@ -9,7 +9,6 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
-#include <algorithm>
 #include <vector>
 
 namespace dwa_planner
@@ -75,12 +74,6 @@ class DWAPlanner : public rclcpp::Node
   double calculate_stage_cost(const State & state);
   double calculate_terminal_cost(const State & state);
 
-  inline static double lerp(const double a, const double b, double t)
-  {
-    t = std::max(0.0, std::min(1.0, t));
-    return a + t * (b - a);
-  }
-
   OccupancyGridSubscription::SharedPtr map_sub_;
   OdometrySubscription::SharedPtr odom_sub_;
   PoseStampedSubscription::SharedPtr goal_sub_;

src/planning/dwa_planner/src/dwa_planner.cpp

@@ -1,5 +1,7 @@
 #include "dwa_planner/dwa_planner.hpp"
 
+#include <wheel_stuck_utils/math/math.hpp>
+
 #include <limits>
 
 namespace dwa_planner
@@ -183,9 +185,10 @@ DWAPlanner::Trajectory DWAPlanner::planning()
   Trajectory best_trajectory;
 
   for (int v = 0; v < velocity_resolution_; v++) {
-    double velocity = lerp(window.min_velocity, window.max_velocity, v * velocity_resolution_inv_);
+    double velocity = wheel_stuck_utils::math::lerp(
+      window.min_velocity, window.max_velocity, v * velocity_resolution_inv_);
     for (int a = 0; a < angular_velocity_resolution_; a++) {
-      double angular_velocity = lerp(
+      double angular_velocity = wheel_stuck_utils::math::lerp(
         window.min_angular_velocity, window.max_angular_velocity,
         a * angular_velocity_resolution_inv_);