migrate lerp function from dwa_planner to wheel_stuck_utils

Signed-off-by: Autumn60 <[email protected]>

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

@@ -0,0 +1,15 @@
+#ifndef WHEEL_STUCK_UTILS__MATH__MATH_HPP_
+#define WHEEL_STUCK_UTILS__MATH__MATH_HPP_
+
+#include <algorithm>
+
+namespace wheel_stuck_utils::math
+{
+inline 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);
+}
+}  // 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_);