refactor(motion_velocity_planner_common): separation of cache and polygon functions (autowarefoundation#628)

refactor cache

Signed-off-by: yuki-takagi-66 <yuki.takagi@tier4.jp>

Author: yuki-takagi-66

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/include/autoware/motion_velocity_planner_common/utils.hpp

@@ -172,6 +172,13 @@ double get_dist_to_traj_poly(
   const geometry_msgs::msg::Point & point,
   const std::vector<autoware_utils::Polygon2d> & decimated_traj_polys);
 
+/*
+ * @brief return the  distance from `predicted_object` to `decimated_traj_polys`
+ */
+double calc_dist_to_traj_poly(
+  const autoware_perception_msgs::msg::PredictedObject & predicted_object,
+  const std::vector<autoware_utils_geometry::Polygon2d> & decimated_traj_polys);
+
 /**
  * @brief append the `input_points` up to `extend_length` every `step_length`, in the direction of
  * the last point of `input_points`, keeping its vel/acc

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/planner_data.cpp

@@ -267,13 +267,7 @@ double PlannerData::Object::get_dist_to_traj_poly(
   const std::vector<autoware_utils_geometry::Polygon2d> & decimated_traj_polys) const
 {
   if (!dist_to_traj_poly) {
-    const auto & obj_pose = predicted_object.kinematics.initial_pose_with_covariance.pose;
-    const auto obj_poly = autoware_utils_geometry::to_polygon2d(obj_pose, predicted_object.shape);
-    dist_to_traj_poly = std::numeric_limits<double>::max();
-    for (const auto & traj_poly : decimated_traj_polys) {
-      const double current_dist_to_traj_poly = bg::distance(traj_poly, obj_poly);
-      dist_to_traj_poly = std::min(*dist_to_traj_poly, current_dist_to_traj_poly);
-    }
+    dist_to_traj_poly = utils::calc_dist_to_traj_poly(predicted_object, decimated_traj_polys);
   }
   return *dist_to_traj_poly;
 }

planning/motion_velocity_planner/autoware_motion_velocity_planner_common/src/utils.cpp

@@ -226,4 +226,18 @@ double get_dist_to_traj_poly(
   return dist_to_traj_poly;
 }
 
+double calc_dist_to_traj_poly(
+  const autoware_perception_msgs::msg::PredictedObject & predicted_object,
+  const std::vector<autoware_utils_geometry::Polygon2d> & decimated_traj_polys)
+{
+  const auto obj_poly = autoware_utils_geometry::to_polygon2d(
+    predicted_object.kinematics.initial_pose_with_covariance.pose, predicted_object.shape);
+  double dist_to_traj_poly = std::numeric_limits<double>::max();
+  for (const auto & traj_poly : decimated_traj_polys) {
+    const double current_dist_to_traj_poly = boost::geometry::distance(traj_poly, obj_poly);
+    dist_to_traj_poly = std::min(dist_to_traj_poly, current_dist_to_traj_poly);
+  }
+  return dist_to_traj_poly;
+}
+
 }  // namespace autoware::motion_velocity_planner::utils