feat(obstacle_cruise_planner): replace interpolation function with th…

…e utils function (autowarefoundation#1798) * feat(obstacle_cruise_planner): replace interpolation function with the util function Signed-off-by: yutaka <purewater0901@gmail.com> * add dist and yaw threshold Signed-off-by: yutaka <purewater0901@gmail.com> Signed-off-by: yutaka <purewater0901@gmail.com>
boyali · Oct 3, 2022 · 68f8fb6 · 68f8fb6
1 parent 8016a94
commit 68f8fb6
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 69 deletions.
diff --git a/...include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/...include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp
@@ -56,8 +56,6 @@ class OptimizationBasedPlanner : public PlannerInterface
   std::tuple<double, double> calcInitialMotion(
     const ObstacleCruisePlannerData & planner_data, const Trajectory & prev_traj);
 
-  TrajectoryPoint calcInterpolatedTrajectoryPoint(
-    const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose);
   bool checkHasReachedGoal(const ObstacleCruisePlannerData & planner_data);
 
   boost::optional<SBoundaries> getSBoundaries(

diff --git a/...ing/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/...ing/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp
@@ -18,6 +18,7 @@
 #include "interpolation/spline_interpolation.hpp"
 #include "interpolation/zero_order_hold.hpp"
 #include "motion_utils/resample/resample.hpp"
+#include "motion_utils/trajectory/interpolation.hpp"
 #include "motion_utils/trajectory/trajectory.hpp"
 #include "obstacle_cruise_planner/utils.hpp"
 #include "tier4_autoware_utils/tier4_autoware_utils.hpp"
@@ -33,27 +34,6 @@
 constexpr double ZERO_VEL_THRESHOLD = 0.01;
 constexpr double CLOSE_S_DIST_THRESHOLD = 1e-3;
 
-namespace
-{
-inline void convertEulerAngleToMonotonic(std::vector<double> & a)
-{
-  for (unsigned int i = 1; i < a.size(); ++i) {
-    const double da = a[i] - a[i - 1];
-    a[i] = a[i - 1] + tier4_autoware_utils::normalizeRadian(da);
-  }
-}
-
-inline tf2::Vector3 getTransVector3(
-  const geometry_msgs::msg::Pose & from, const geometry_msgs::msg::Pose & to)
-{
-  double dx = to.position.x - from.position.x;
-  double dy = to.position.y - from.position.y;
-  double dz = to.position.z - from.position.z;
-  return tf2::Vector3(dx, dy, dz);
-}
-
-}  // namespace
-
 OptimizationBasedPlanner::OptimizationBasedPlanner(
   rclcpp::Node & node, const LongitudinalInfo & longitudinal_info,
   const vehicle_info_util::VehicleInfo & vehicle_info)
@@ -312,8 +292,9 @@ std::tuple<double, double> OptimizationBasedPlanner::calcInitialMotion(
   const auto & current_pose = planner_data.current_pose;
   const auto & input_traj = planner_data.traj;
   const double vehicle_speed{std::abs(current_vel)};
-  const auto current_closest_point =
-    calcInterpolatedTrajectoryPoint(input_traj, planner_data.current_pose);
+  const auto current_closest_point = motion_utils::calcInterpolatedPoint(
+    input_traj, planner_data.current_pose, nearest_dist_deviation_threshold_,
+    nearest_yaw_deviation_threshold_);
   const double target_vel{std::abs(current_closest_point.longitudinal_velocity_mps)};
 
   double initial_vel{};
@@ -328,10 +309,12 @@ std::tuple<double, double> OptimizationBasedPlanner::calcInitialMotion(
 
   TrajectoryPoint prev_output_closest_point;
   if (smoothed_trajectory_ptr_) {
-    prev_output_closest_point =
-      calcInterpolatedTrajectoryPoint(*smoothed_trajectory_ptr_, current_pose);
+    prev_output_closest_point = motion_utils::calcInterpolatedPoint(
+      *smoothed_trajectory_ptr_, current_pose, nearest_dist_deviation_threshold_,
+      nearest_yaw_deviation_threshold_);
   } else {
-    prev_output_closest_point = calcInterpolatedTrajectoryPoint(prev_traj, current_pose);
+    prev_output_closest_point = motion_utils::calcInterpolatedPoint(
+      prev_traj, current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_);
   }
 
   // when velocity tracking deviation is large
@@ -385,47 +368,6 @@ std::tuple<double, double> OptimizationBasedPlanner::calcInitialMotion(
   return std::make_tuple(initial_vel, initial_acc);
 }
 
-TrajectoryPoint OptimizationBasedPlanner::calcInterpolatedTrajectoryPoint(
-  const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose)
-{
-  TrajectoryPoint traj_p{};
-  traj_p.pose = target_pose;
-
-  if (trajectory.points.empty()) {
-    traj_p.longitudinal_velocity_mps = 0.0;
-    traj_p.acceleration_mps2 = 0.0;
-    return traj_p;
-  }
-
-  if (trajectory.points.size() == 1) {
-    traj_p.longitudinal_velocity_mps = trajectory.points.at(0).longitudinal_velocity_mps;
-    traj_p.acceleration_mps2 = trajectory.points.at(0).acceleration_mps2;
-    return traj_p;
-  }
-
-  const size_t segment_idx =
-    motion_utils::findNearestSegmentIndex(trajectory.points, target_pose.position);
-
-  const auto v1 = getTransVector3(
-    trajectory.points.at(segment_idx).pose, trajectory.points.at(segment_idx + 1).pose);
-  const auto v2 = getTransVector3(trajectory.points.at(segment_idx).pose, target_pose);
-
-  // Calc interpolation ratio
-  const double prop{std::clamp(v1.dot(v2) / v1.length2(), 0.0, 1.0)};
-
-  {
-    const auto & curr_pt = trajectory.points.at(segment_idx);
-    const auto & next_pt = trajectory.points.at(segment_idx + 1);
-    traj_p.pose = tier4_autoware_utils::calcInterpolatedPose(curr_pt, next_pt, prop);
-    traj_p.longitudinal_velocity_mps = interpolation::lerp(
-      curr_pt.longitudinal_velocity_mps, next_pt.longitudinal_velocity_mps, prop);
-    traj_p.acceleration_mps2 =
-      interpolation::lerp(curr_pt.acceleration_mps2, next_pt.acceleration_mps2, prop);
-  }
-
-  return traj_p;
-}
-
 bool OptimizationBasedPlanner::checkHasReachedGoal(const ObstacleCruisePlannerData & planner_data)
 {
   // If goal is close and current velocity is low, we don't optimize trajectory