diff --git a/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/planning_utils.hpp b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/planning_utils.hpp index 15a67e0e6e7fd..2e3c64c9c4315 100644 --- a/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/planning_utils.hpp +++ b/control/autoware_pure_pursuit/include/autoware/pure_pursuit/util/planning_utils.hpp @@ -74,55 +74,6 @@ std::pair findClosestIdxWithDistAngThr( int8_t getLaneDirection(const std::vector & poses, double th_dist = 0.5); -// cspell: ignore pointinpoly -// refer from apache's pointinpoly in http://www.visibone.com/inpoly/ -template -bool isInPolygon(const std::vector & polygon, const T & point) -{ - // polygons with fewer than 3 sides are excluded - if (polygon.size() < 3) { - return false; - } - - bool in_poly = false; - double x1, x2, y1, y2; - - uint32_t nr_poly_points = polygon.size(); - // start with the last point to make the check last point<->first point the first one - double xold = polygon.at(nr_poly_points - 1).x(); - double yold = polygon.at(nr_poly_points - 1).y(); - for (const auto & poly_p : polygon) { - double xnew = poly_p.x(); - double ynew = poly_p.y(); - if (xnew > xold) { - x1 = xold; - x2 = xnew; - y1 = yold; - y2 = ynew; - } else { - x1 = xnew; - x2 = xold; - y1 = ynew; - y2 = yold; - } - - if ( - (xnew < point.x()) == (point.x() <= xold) && - (point.y() - y1) * (x2 - x1) < (y2 - y1) * (point.x() - x1)) { - in_poly = !in_poly; - } - xold = xnew; - yold = ynew; - } - - return in_poly; -} - -template <> -bool isInPolygon( - const std::vector & polygon, const geometry_msgs::msg::Point & point); - -double kmph2mps(const double velocity_kmph); double normalizeEulerAngle(const double euler); geometry_msgs::msg::Point transformToAbsoluteCoordinate2D( diff --git a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp index c228f824970a8..f91e8379b0f71 100644 --- a/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp +++ b/control/autoware_pure_pursuit/src/autoware_pure_pursuit_core/planning_utils.cpp @@ -164,25 +164,6 @@ int8_t getLaneDirection(const std::vector & poses, dou return 2; } -template <> -bool isInPolygon( - const std::vector & polygon, const geometry_msgs::msg::Point & point) -{ - std::vector polygon_conv; - for (const auto & el : polygon) { - polygon_conv.emplace_back(el.x, el.y, el.z); - } - - tf2::Vector3 point_conv = tf2::Vector3(point.x, point.y, point.z); - - return isInPolygon(polygon_conv, point_conv); -} - -double kmph2mps(const double velocity_kmph) -{ - return (velocity_kmph * 1000) / (60 * 60); -} - double normalizeEulerAngle(const double euler) { double res = euler;