From d81bf4ea47e8b2fcd6f098b5af2be2d1eec17c26 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 27 Mar 2023 11:42:22 +0900 Subject: [PATCH] refactor(mpc): remove old library Signed-off-by: Takamasa Horibe --- .../include/mpc_lateral_controller/mpc.hpp | 2 -- .../mpc_lateral_controller/mpc_utils.hpp | 1 - control/mpc_lateral_controller/package.xml | 1 - control/mpc_lateral_controller/src/mpc.cpp | 4 ++-- .../mpc_lateral_controller/src/mpc_utils.cpp | 18 +++++++++--------- 5 files changed, 11 insertions(+), 15 deletions(-) diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index c6a920e347cab..323a23ab3e80a 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -15,8 +15,6 @@ #ifndef MPC_LATERAL_CONTROLLER__MPC_HPP_ #define MPC_LATERAL_CONTROLLER__MPC_HPP_ -#include "geometry/common_2d.hpp" -#include "helper_functions/angle_utils.hpp" #include "mpc_lateral_controller/interpolate.hpp" #include "mpc_lateral_controller/lowpass_filter.hpp" #include "mpc_lateral_controller/mpc_trajectory.hpp" diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp index d9fce24f4185f..64cb2c134115c 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc_utils.hpp @@ -16,7 +16,6 @@ #define MPC_LATERAL_CONTROLLER__MPC_UTILS_HPP_ #include "eigen3/Eigen/Core" -#include "geometry/common_2d.hpp" #include "helper_functions/angle_utils.hpp" #include "interpolation/linear_interpolation.hpp" #include "interpolation/spline_interpolation.hpp" diff --git a/control/mpc_lateral_controller/package.xml b/control/mpc_lateral_controller/package.xml index c805abe341bef..04a80ef9d9080 100644 --- a/control/mpc_lateral_controller/package.xml +++ b/control/mpc_lateral_controller/package.xml @@ -19,7 +19,6 @@ autoware_cmake autoware_auto_control_msgs - autoware_auto_geometry autoware_auto_planning_msgs autoware_auto_vehicle_msgs diagnostic_msgs diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index fb4353d142ee0..9372bdee2b894 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -284,8 +284,8 @@ bool MPC::getData( *m_steer_prediction_prev = data->predicted_steer; /* check error limit */ - const double dist_err = autoware::common::geometry::distance_2d( - current_pose.position, data->nearest_pose.position); + const double dist_err = + tier4_autoware_utils::calcDistance2d(current_pose.position, data->nearest_pose.position); if (dist_err > m_admissible_position_error) { RCLCPP_WARN_SKIPFIRST_THROTTLE( m_logger, *m_clock, duration, "position error is over limit. error = %fm, limit: %fm", diff --git a/control/mpc_lateral_controller/src/mpc_utils.cpp b/control/mpc_lateral_controller/src/mpc_utils.cpp index 41b15f17bf075..4460e389d80b9 100644 --- a/control/mpc_lateral_controller/src/mpc_utils.cpp +++ b/control/mpc_lateral_controller/src/mpc_utils.cpp @@ -26,7 +26,7 @@ namespace autoware::motion::control::mpc_lateral_controller { namespace MPCUtils { -using autoware::common::geometry::distance_2d; +using tier4_autoware_utils::calcDistance2d; geometry_msgs::msg::Quaternion getQuaternionFromYaw(const double & yaw) { @@ -200,12 +200,12 @@ std::vector calcTrajectoryCurvature( p1.y = traj.y[prev_idx]; p2.y = traj.y[curr_idx]; p3.y = traj.y[next_idx]; - const double den = std::max( - distance_2d(p1, p2) * distance_2d(p2, p3) * distance_2d(p3, p1), - std::numeric_limits::epsilon()); - const double curvature = - 2.0 * ((p2.x - p1.x) * (p3.y - p1.y) - (p2.y - p1.y) * (p3.x - p1.x)) / den; - curvature_vec.at(curr_idx) = curvature; + try { + curvature_vec.at(curr_idx) = tier4_autoware_utils::calcCurvature(p1, p2, p3); + } catch (...) { + std::cerr << "[MPC] 2 points are too close to calculate curvature." << std::endl; + curvature_vec.at(curr_idx) = 0.0; + } } /* first and last curvature is copied from next value */ @@ -380,7 +380,7 @@ double calcStopDistance( for (int i = origin + 1; i < static_cast(current_trajectory.points.size()) - 1; ++i) { const auto & p0 = current_trajectory.points.at(static_cast(i)); const auto & p1 = current_trajectory.points.at(static_cast(i - 1)); - stop_dist += distance_2d(p0, p1); + stop_dist += calcDistance2d(p0, p1); if (std::fabs(p0.longitudinal_velocity_mps) < zero_velocity) { break; } @@ -395,7 +395,7 @@ double calcStopDistance( if (std::fabs(p0.longitudinal_velocity_mps) > zero_velocity) { break; } - stop_dist -= distance_2d(p0, p1); + stop_dist -= calcDistance2d(p0, p1); } return stop_dist; }