Revert "Merge branch 'main' into feat/planning/fix_dwaP_member_scope"

This reverts commit ac6629f, reversing
changes made to d118d11.

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

@@ -9,6 +9,7 @@
 #include <nav_msgs/msg/occupancy_grid.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 
+#include <algorithm>
 #include <vector>
 
 namespace dwa_planner
@@ -72,6 +73,14 @@ 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);
+  }
+
+  rclcpp::TimerBase::SharedPtr update_timer_;
+
   OccupancyGridSubscription::SharedPtr map_sub_;
   OdometrySubscription::SharedPtr odom_sub_;
   PoseStampedSubscription::SharedPtr goal_sub_;

src/planning/dwa_planner/src/dwa_planner.cpp

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