From 588760bf00ca1e0c93986409d2f0c0388e7188f7 Mon Sep 17 00:00:00 2001 From: yutaka Date: Fri, 1 Jul 2022 16:29:06 +0900 Subject: [PATCH] feat(obstacle_cruise_planner): separate data Signed-off-by: yutaka --- .../include/obstacle_cruise_planner/node.hpp | 10 ++- .../optimization_based_planner.hpp | 11 ++- .../pid_based_planner/pid_based_planner.hpp | 4 +- .../planner_interface.hpp | 4 +- planning/obstacle_cruise_planner/src/node.cpp | 79 ++++++++++++++----- .../optimization_based_planner.cpp | 72 ++++++++--------- .../pid_based_planner/pid_based_planner.cpp | 6 +- 7 files changed, 117 insertions(+), 69 deletions(-) diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp index 7e61ceafab19c..e8d49cd103837 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -71,10 +71,16 @@ class ObstacleCruisePlannerNode : public rclcpp::Node void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg); // member Functions - ObstacleCruisePlannerData createPlannerData( + ObstacleCruisePlannerData createCruiseData( const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, - DebugData & debug_data); + const std::vector & obstacles); + ObstacleCruisePlannerData createStopData( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, + const std::vector & obstacles); double calcCurrentAccel() const; + std::vector getTargetObstacles( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, + const double current_vel, DebugData & debug_data); std::vector filterObstacles( const PredictedObjects & predicted_objects, const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose, const double current_vel, 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 1498230724554..fda3495a2547f 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 @@ -49,8 +49,8 @@ class OptimizationBasedPlanner : public PlannerInterface const vehicle_info_util::VehicleInfo & vehicle_info); Trajectory generateCruiseTrajectory( - const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, - boost::optional & vel_limit, DebugData & debug_data) override; + const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, + DebugData & debug_data) override; private: struct TrajectoryData @@ -72,13 +72,12 @@ class OptimizationBasedPlanner : public PlannerInterface // Member Functions std::vector createTimeVector(); std::tuple calcInitialMotion( - const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, - const size_t input_closest, const Trajectory & prev_traj); + const ObstacleCruisePlannerData & planner_data, const size_t input_closest, + const Trajectory & prev_traj); TrajectoryPoint calcInterpolatedTrajectoryPoint( const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose); - bool checkHasReachedGoal( - const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj); + bool checkHasReachedGoal(const ObstacleCruisePlannerData & planner_data); TrajectoryData getTrajectoryData( const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose); diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp index 9ad2e7f2e59e8..58e38c3174b45 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -56,8 +56,8 @@ class PIDBasedPlanner : public PlannerInterface const vehicle_info_util::VehicleInfo & vehicle_info); Trajectory generateCruiseTrajectory( - const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, - boost::optional & vel_limit, DebugData & debug_data) override; + const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, + DebugData & debug_data) override; void updateParam(const std::vector & parameters) override; diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp index 30e8934c53a13..e66825425c40c 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/planner_interface.hpp @@ -66,8 +66,8 @@ class PlannerInterface const ObstacleCruisePlannerData & planner_data, DebugData & debug_data); virtual Trajectory generateCruiseTrajectory( - const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, - boost::optional & vel_limit, DebugData & debug_data) = 0; + const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, + DebugData & debug_data) = 0; void updateCommonParam(const std::vector & parameters) { diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index fb17e48752d20..e76ec8d279b77 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -482,17 +482,24 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms stop_watch_.tic(__func__); - // create algorithmic data + // Get Target Obstacles DebugData debug_data; - const auto planner_data = createPlannerData(*msg, current_pose_ptr->pose, debug_data); + const auto target_obstacles = getTargetObstacles( + *msg, current_pose_ptr->pose, current_twist_ptr_->twist.linear.x, debug_data); + + // create data for stop + const auto stop_data = createStopData(*msg, current_pose_ptr->pose, target_obstacles); // stop planning - const auto stop_traj = planner_ptr_->generateStopTrajectory(planner_data, debug_data); + const auto stop_traj = planner_ptr_->generateStopTrajectory(stop_data, debug_data); + + // create data for cruise + const auto cruise_data = createCruiseData(stop_traj, current_pose_ptr->pose, target_obstacles); // cruise planning boost::optional vel_limit; const auto output_traj = - planner_ptr_->generateCruiseTrajectory(planner_data, stop_traj, vel_limit, debug_data); + planner_ptr_->generateCruiseTrajectory(cruise_data, vel_limit, debug_data); // publisher external velocity limit if required publishVelocityLimit(vel_limit); @@ -523,33 +530,50 @@ bool ObstacleCruisePlannerNode::isStopObstacle(const uint8_t label) return std::find(types.begin(), types.end(), label) != types.end(); } -ObstacleCruisePlannerData ObstacleCruisePlannerNode::createPlannerData( +ObstacleCruisePlannerData ObstacleCruisePlannerNode::createStopData( const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, - DebugData & debug_data) + const std::vector & obstacles) { - stop_watch_.tic(__func__); - const auto current_time = now(); const double current_vel = current_twist_ptr_->twist.linear.x; const double current_accel = calcCurrentAccel(); - auto target_obstacles = - filterObstacles(*in_objects_ptr_, trajectory, current_pose, current_vel, debug_data); - updateHasStopped(target_obstacles); - // create planner_data + // create planner_stop data ObstacleCruisePlannerData planner_data; planner_data.current_time = current_time; planner_data.traj = trajectory; planner_data.current_pose = current_pose; planner_data.current_vel = current_vel; planner_data.current_acc = current_accel; - planner_data.target_obstacles = target_obstacles; + for (const auto & obstacle : obstacles) { + if (obstacle.has_stopped) { + planner_data.target_obstacles.push_back(obstacle); + } + } - // print calculation time - const double calculation_time = stop_watch_.toc(__func__); - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("ObstacleCruisePlanner"), is_showing_debug_info_, " %s := %f [ms]", - __func__, calculation_time); + return planner_data; +} + +ObstacleCruisePlannerData ObstacleCruisePlannerNode::createCruiseData( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, + const std::vector & obstacles) +{ + const auto current_time = now(); + const double current_vel = current_twist_ptr_->twist.linear.x; + const double current_accel = calcCurrentAccel(); + + // create planner_stop data + ObstacleCruisePlannerData planner_data; + planner_data.current_time = current_time; + planner_data.traj = trajectory; + planner_data.current_pose = current_pose; + planner_data.current_vel = current_vel; + planner_data.current_acc = current_accel; + for (const auto & obstacle : obstacles) { + if (!obstacle.has_stopped) { + planner_data.target_obstacles.push_back(obstacle); + } + } return planner_data; } @@ -567,6 +591,25 @@ double ObstacleCruisePlannerNode::calcCurrentAccel() const return lpf_acc_ptr_->filter(accel); } +std::vector ObstacleCruisePlannerNode::getTargetObstacles( + const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, + const double current_vel, DebugData & debug_data) +{ + stop_watch_.tic(__func__); + + auto target_obstacles = + filterObstacles(*in_objects_ptr_, trajectory, current_pose, current_vel, debug_data); + updateHasStopped(target_obstacles); + + // print calculation time + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), is_showing_debug_info_, " %s := %f [ms]", + __func__, calculation_time); + + return target_obstacles; +} + std::vector ObstacleCruisePlannerNode::filterObstacles( const PredictedObjects & predicted_objects, const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose, const double current_vel, DebugData & debug_data) 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 a41b5665f7e03..9dc46db89870a 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 @@ -113,7 +113,7 @@ OptimizationBasedPlanner::OptimizationBasedPlanner( } Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( - const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, + const ObstacleCruisePlannerData & planner_data, [[maybe_unused]] boost::optional & vel_limit, [[maybe_unused]] DebugData & debug_data) { @@ -123,57 +123,57 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( RCLCPP_ERROR( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "Resolution size is not enough"); - prev_output_ = stop_traj; - return stop_traj; + prev_output_ = planner_data.traj; + return planner_data.traj; } // Get the nearest point on the trajectory const auto closest_idx = tier4_autoware_utils::findNearestIndex( - stop_traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, + planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); if (!closest_idx) { // Check validity of the closest index RCLCPP_ERROR( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "Closest Index is Invalid"); - prev_output_ = stop_traj; - return stop_traj; + prev_output_ = planner_data.traj; + return planner_data.traj; } // Transform original trajectory to TrajectoryData - const auto base_traj_data = getTrajectoryData(stop_traj, planner_data.current_pose); + const auto base_traj_data = getTrajectoryData(planner_data.traj, planner_data.current_pose); if (base_traj_data.traj.points.size() < 2) { RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "The number of points on the trajectory data is too small"); - prev_output_ = stop_traj; - return stop_traj; + prev_output_ = planner_data.traj; + return planner_data.traj; } // Compute maximum velocity double v_max = 0.0; - for (const auto & point : stop_traj.points) { + for (const auto & point : planner_data.traj.points) { v_max = std::max(v_max, static_cast(point.longitudinal_velocity_mps)); } // Get Current Velocity double v0; double a0; - std::tie(v0, a0) = calcInitialMotion(planner_data, stop_traj, *closest_idx, prev_output_); + std::tie(v0, a0) = calcInitialMotion(planner_data, *closest_idx, prev_output_); a0 = std::min(longitudinal_info_.max_accel, std::max(a0, longitudinal_info_.min_accel)); // Check trajectory size - if (stop_traj.points.size() - *closest_idx <= 2) { + if (planner_data.traj.points.size() - *closest_idx <= 2) { RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "The number of points on the trajectory is too small"); - prev_output_ = stop_traj; - return stop_traj; + prev_output_ = planner_data.traj; + return planner_data.traj; } // Check if reached goal - if (checkHasReachedGoal(planner_data, stop_traj)) { - prev_output_ = stop_traj; - return stop_traj; + if (checkHasReachedGoal(planner_data)) { + prev_output_ = planner_data.traj; + return planner_data.traj; } // Resample base trajectory data by time @@ -183,8 +183,8 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "The number of points on the resampled trajectory data is too small"); - prev_output_ = stop_traj; - return stop_traj; + prev_output_ = planner_data.traj; + return planner_data.traj; } // Get S Boundaries from the obstacle @@ -193,8 +193,8 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "No Dangerous Objects around the ego vehicle"); - prev_output_ = stop_traj; - return stop_traj; + prev_output_ = planner_data.traj; + return planner_data.traj; } // Optimization @@ -232,7 +232,8 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( // Publish Debug trajectories publishDebugTrajectory( - planner_data.current_time, stop_traj, *closest_idx, time_vec, *s_boundaries, optimized_result); + planner_data.current_time, planner_data.traj, *closest_idx, time_vec, *s_boundaries, + optimized_result); // Transformation from t to s const auto processed_result = processOptimizedResult(data.v0, optimized_result); @@ -240,15 +241,15 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "Processed Result is empty"); - prev_output_ = stop_traj; - return stop_traj; + prev_output_ = planner_data.traj; + return planner_data.traj; } const auto & opt_position = processed_result->s; const auto & opt_velocity = processed_result->v; // Check Size if (opt_position.size() == 1 && opt_velocity.front() < ZERO_VEL_THRESHOLD) { - auto output = stop_traj; + auto output = planner_data.traj; output.points.at(*closest_idx).longitudinal_velocity_mps = data.v0; for (size_t i = *closest_idx + 1; i < output.points.size(); ++i) { output.points.at(i).longitudinal_velocity_mps = 0.0; @@ -259,8 +260,8 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "Optimized Trajectory is too small"); - prev_output_ = stop_traj; - return stop_traj; + prev_output_ = planner_data.traj; + return planner_data.traj; } // Get Zero Velocity Position @@ -273,7 +274,7 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( } } const auto traj_stop_dist = tier4_autoware_utils::calcDistanceToForwardStopPoint( - stop_traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, + planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); if (traj_stop_dist) { closest_stop_dist = std::min(*traj_stop_dist, closest_stop_dist); @@ -332,9 +333,9 @@ Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( } Trajectory output; - output.header = stop_traj.header; + output.header = planner_data.traj.header; for (size_t i = 0; i < *closest_idx; ++i) { - auto point = stop_traj.points.at(i); + auto point = planner_data.traj.points.at(i); point.longitudinal_velocity_mps = data.v0; output.points.push_back(point); } @@ -385,11 +386,11 @@ std::vector OptimizationBasedPlanner::createTimeVector() // v0, a0 std::tuple OptimizationBasedPlanner::calcInitialMotion( - const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, - const size_t input_closest, const Trajectory & prev_traj) + const ObstacleCruisePlannerData & planner_data, const size_t input_closest, + const Trajectory & prev_traj) { const auto & current_vel = planner_data.current_vel; - const auto & input_traj = stop_traj; + const auto & input_traj = planner_data.traj; const double vehicle_speed{std::abs(current_vel)}; const double target_vel{std::abs(input_traj.points.at(input_closest).longitudinal_velocity_mps)}; @@ -504,12 +505,11 @@ TrajectoryPoint OptimizationBasedPlanner::calcInterpolatedTrajectoryPoint( return traj_p; } -bool OptimizationBasedPlanner::checkHasReachedGoal( - const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj) +bool OptimizationBasedPlanner::checkHasReachedGoal(const ObstacleCruisePlannerData & planner_data) { // If goal is close and current velocity is low, we don't optimize trajectory const auto closest_stop_dist = tier4_autoware_utils::calcDistanceToForwardStopPoint( - stop_traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, + planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, nearest_yaw_deviation_threshold_); if (closest_stop_dist && *closest_stop_dist < 0.5 && planner_data.current_vel < 0.6) { return true; diff --git a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp index 92d35568f52f1..8062a3a3c85a7 100644 --- a/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp +++ b/planning/obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -87,8 +87,8 @@ PIDBasedPlanner::PIDBasedPlanner( } Trajectory PIDBasedPlanner::generateCruiseTrajectory( - const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, - boost::optional & vel_limit, DebugData & debug_data) + const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, + DebugData & debug_data) { stop_watch_.tic(__func__); debug_values_.resetValues(); @@ -108,7 +108,7 @@ Trajectory PIDBasedPlanner::generateCruiseTrajectory( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, " %s := %f [ms]", __func__, calculation_time); - return stop_traj; + return planner_data.traj; } void PIDBasedPlanner::calcObstaclesToCruise(