diff --git a/src/common/wheel_stuck_utils/include/wheel_stuck_utils/math/math.hpp b/src/common/wheel_stuck_utils/include/wheel_stuck_utils/math/math.hpp deleted file mode 100644 index d784581..0000000 --- a/src/common/wheel_stuck_utils/include/wheel_stuck_utils/math/math.hpp +++ /dev/null @@ -1,70 +0,0 @@ -#ifndef WHEEL_STUCK_UTILS__MATH__MATH_HPP_ -#define WHEEL_STUCK_UTILS__MATH__MATH_HPP_ - -#include - -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 -inline T min(T a) -{ - return a; -} - -template -inline T min(T a, Args... args) -{ - return std::min(a, min(args...)); -} - -// 可変長引数を受け取り最大値を返す -template -inline T max(T b) -{ - return b; -} - -template -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_ diff --git a/src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp b/src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp index efbddd5..d51a49a 100644 --- a/src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp +++ b/src/planning/dwa_planner/include/dwa_planner/dwa_planner.hpp @@ -9,6 +9,7 @@ #include #include +#include #include 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_; diff --git a/src/planning/dwa_planner/src/dwa_planner.cpp b/src/planning/dwa_planner/src/dwa_planner.cpp index 6070a1b..396d257 100644 --- a/src/planning/dwa_planner/src/dwa_planner.cpp +++ b/src/planning/dwa_planner/src/dwa_planner.cpp @@ -1,7 +1,5 @@ #include "dwa_planner/dwa_planner.hpp" -#include - #include 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_);