From 505f97586316d1b2bd45afdf903c27c6cb00df0a Mon Sep 17 00:00:00 2001 From: Yutaka Shimizu <43805014+purewater0901@users.noreply.github.com> Date: Wed, 7 Sep 2022 09:22:27 +0900 Subject: [PATCH] feat(obstacle_cruise_planner): replace interpolation function with the utils function (#1798) * feat(obstacle_cruise_planner): replace interpolation function with the util function Signed-off-by: yutaka * add dist and yaw threshold Signed-off-by: yutaka Signed-off-by: yutaka --- .../optimization_based_planner.hpp | 2 - .../optimization_based_planner.cpp | 76 +++---------------- 2 files changed, 9 insertions(+), 69 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp index ffdae2edc1c46..9b174e09357e1 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -56,8 +56,6 @@ class OptimizationBasedPlanner : public PlannerInterface std::tuple 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 getSBoundaries( diff --git a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp index e50063ce3c7bc..e58df2d0b884a 100644 --- a/planning/obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp +++ b/planning/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 & 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 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 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 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