From 3ef1663e315fe734ec488cdb66618a1084e6485a Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Thu, 30 Jun 2022 14:30:35 +0900 Subject: [PATCH] feat(obstacle_cruise_planner): implemented common obstacle stop (#1185) * feat(obstacle_cruise_planner): implemented common obstacle stop Signed-off-by: Takayuki Murooka * fix some implementation Signed-off-by: Takayuki Murooka * minor changes Signed-off-by: Takayuki Murooka * use min_ego_accel_for_rss Signed-off-by: Takayuki Murooka * remove unnecessary code Signed-off-by: Takayuki Murooka * fix CI error Signed-off-by: Takayuki Murooka * fix typo Signed-off-by: Takayuki Murooka --- .../obstacle_cruise_planner.param.yaml | 18 +- .../obstacle_cruise_planner/CMakeLists.txt | 1 + .../config/obstacle_cruise_planner.param.yaml | 18 +- .../include/obstacle_cruise_planner/node.hpp | 12 + .../optimization_based_planner.hpp | 24 +- .../pid_based_planner/pid_based_planner.hpp | 32 +- .../planner_interface.hpp | 150 +------- .../include/obstacle_cruise_planner/utils.hpp | 64 +++- .../obstacle_cruise_planner-design.md | 1 + planning/obstacle_cruise_planner/src/node.cpp | 146 +++++++- .../optimization_based_planner.cpp | 324 +++++------------- .../pid_based_planner/pid_based_planner.cpp | 264 +------------- .../src/planner_interface.cpp | 183 ++++++++++ .../obstacle_cruise_planner/src/utils.cpp | 107 +++--- 14 files changed, 604 insertions(+), 740 deletions(-) create mode 100644 planning/obstacle_cruise_planner/src/planner_interface.cpp diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml index 4690a524788f6..9b50aba0e27cf 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/motion_planning/obstacle_cruise_planner/obstacle_cruise_planner.param.yaml @@ -6,7 +6,7 @@ is_showing_debug_info: true # longitudinal info - idling_time: 4.0 # idling time to detect front vehicle starting deceleration [s] + idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s] min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] @@ -17,6 +17,7 @@ nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] cruise_obstacle_type: unknown: true @@ -39,7 +40,7 @@ pedestrian: true obstacle_filtering: - rough_detection_area_expand_width : 5.0 # rough lateral margin for rough detection area expansion [m] + rough_detection_area_expand_width : 3.0 # rough lateral margin for rough detection area expansion [m] detection_area_expand_width : 0.0 # lateral margin for precise detection area expansion decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking @@ -48,6 +49,7 @@ crossing_obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] + outside_rough_detection_area_expand_width : 0.5 # rough lateral margin for rough detection area expansion for obstacles outside the trajectory [m] ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego @@ -65,14 +67,14 @@ # use_predicted_obstacle_pose: false # PID gains to keep safe distance with the front vehicle - kp: 0.6 + kp: 2.5 ki: 0.0 - kd: 0.5 + kd: 2.3 min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] - output_ratio_during_accel: 2.5 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - vel_to_acc_weight: 8.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] @@ -87,11 +89,11 @@ max_trajectory_length: 200.0 velocity_margin: 0.1 #[m/s] - # Parameters for safety distance limit time + # Parameters for safe distance t_dangerous: 0.5 # For initial Motion - replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s] + replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s] engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. diff --git a/planning/obstacle_cruise_planner/CMakeLists.txt b/planning/obstacle_cruise_planner/CMakeLists.txt index 7ba8bd9c000ed..81958463871c4 100644 --- a/planning/obstacle_cruise_planner/CMakeLists.txt +++ b/planning/obstacle_cruise_planner/CMakeLists.txt @@ -15,6 +15,7 @@ ament_auto_add_library(obstacle_cruise_planner_core SHARED src/optimization_based_planner/velocity_optimizer.cpp src/optimization_based_planner/optimization_based_planner.cpp src/pid_based_planner/pid_based_planner.cpp + src/planner_interface.cpp ) rclcpp_components_register_node(obstacle_cruise_planner_core diff --git a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 4690a524788f6..9b50aba0e27cf 100644 --- a/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -6,7 +6,7 @@ is_showing_debug_info: true # longitudinal info - idling_time: 4.0 # idling time to detect front vehicle starting deceleration [s] + idling_time: 1.5 # idling time to detect front vehicle starting deceleration [s] min_ego_accel_for_rss: -1.0 # ego's acceleration to calculate RSS distance [m/ss] min_object_accel_for_rss: -1.0 # front obstacle's acceleration to calculate RSS distance [m/ss] safe_distance_margin : 6.0 # This is also used as a stop margin [m] @@ -17,6 +17,7 @@ nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index min_behavior_stop_margin: 3.0 # [m] obstacle_velocity_threshold_from_cruise_to_stop : 3.0 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] + obstacle_velocity_threshold_from_stop_to_cruise : 3.5 # stop planning is executed to the obstacle whose velocity is less than this value [m/s] cruise_obstacle_type: unknown: true @@ -39,7 +40,7 @@ pedestrian: true obstacle_filtering: - rough_detection_area_expand_width : 5.0 # rough lateral margin for rough detection area expansion [m] + rough_detection_area_expand_width : 3.0 # rough lateral margin for rough detection area expansion [m] detection_area_expand_width : 0.0 # lateral margin for precise detection area expansion decimate_trajectory_step_length : 2.0 # longitudinal step length to calculate trajectory polygon for collision checking @@ -48,6 +49,7 @@ crossing_obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop collision_time_margin : 8.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] + outside_rough_detection_area_expand_width : 0.5 # rough lateral margin for rough detection area expansion for obstacles outside the trajectory [m] ego_obstacle_overlap_time_threshold : 1.0 # time threshold to decide cut-in obstacle for cruise or stop [s] max_prediction_time_for_collision_check : 20.0 # prediction time to check collision between obstacle and ego @@ -65,14 +67,14 @@ # use_predicted_obstacle_pose: false # PID gains to keep safe distance with the front vehicle - kp: 0.6 + kp: 2.5 ki: 0.0 - kd: 0.5 + kd: 2.3 min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] - output_ratio_during_accel: 2.5 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] - vel_to_acc_weight: 8.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + output_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + vel_to_acc_weight: 3.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] @@ -87,11 +89,11 @@ max_trajectory_length: 200.0 velocity_margin: 0.1 #[m/s] - # Parameters for safety distance limit time + # Parameters for safe distance t_dangerous: 0.5 # For initial Motion - replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s] + replan_vel_deviation: 5.0 # velocity deviation to replan initial velocity [m/s] engage_velocity: 0.25 # engage velocity threshold [m/s] (if the trajectory velocity is higher than this value, use this velocity for engage vehicle speed) engage_acceleration: 0.1 # engage acceleration [m/ss] (use this acceleration when engagement) engage_exit_ratio: 0.5 # exit engage sequence to normal velocity planning when the velocity exceeds engage_exit_ratio x engage_velocity. 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 98702082550ab..7e61ceafab19c 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/node.hpp @@ -79,6 +79,7 @@ class ObstacleCruisePlannerNode : public rclcpp::Node const PredictedObjects & predicted_objects, const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose, const double current_vel, DebugData & debug_data); + void updateHasStopped(std::vector & target_obstacles); geometry_msgs::msg::Point calcNearestCollisionPoint( const size_t & first_within_idx, const std::vector & collision_points, @@ -93,6 +94,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node void publishDebugData(const DebugData & debug_data) const; void publishCalculationTime(const double calculation_time) const; + bool isCruiseObstacle(const uint8_t label); + bool isStopObstacle(const uint8_t label); + bool is_showing_debug_info_; double min_behavior_stop_margin_; double nearest_dist_deviation_threshold_; @@ -100,6 +104,9 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double obstacle_velocity_threshold_from_cruise_to_stop_; double obstacle_velocity_threshold_from_stop_to_cruise_; + std::vector cruise_obstacle_types_; + std::vector stop_obstacle_types_; + // parameter callback result OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -151,8 +158,11 @@ class ObstacleCruisePlannerNode : public rclcpp::Node double rough_detection_area_expand_width; double detection_area_expand_width; double decimate_trajectory_step_length; + // inside double crossing_obstacle_velocity_threshold; double collision_time_margin; + // outside + double outside_rough_detection_area_expand_width; double ego_obstacle_overlap_time_threshold; double max_prediction_time_for_collision_check; double crossing_obstacle_traj_angle_threshold; @@ -161,6 +171,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node ObstacleFilteringParam obstacle_filtering_param_; bool need_to_clear_vel_limit_{false}; + + std::vector prev_target_obstacles_; }; } // namespace motion_planning 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 1f8834a0e6a4e..1498230724554 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 @@ -48,9 +48,9 @@ class OptimizationBasedPlanner : public PlannerInterface rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, const vehicle_info_util::VehicleInfo & vehicle_info); - Trajectory generateTrajectory( - const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, - DebugData & debug_data) override; + Trajectory generateCruiseTrajectory( + const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, + boost::optional & vel_limit, DebugData & debug_data) override; private: struct TrajectoryData @@ -71,26 +71,20 @@ class OptimizationBasedPlanner : public PlannerInterface // Member Functions std::vector createTimeVector(); - - double getClosestStopDistance( - const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data); - std::tuple calcInitialMotion( - const double current_vel, const Trajectory & input_traj, const size_t input_closest, - const Trajectory & prev_traj, const double closest_stop_dist); + const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, + const size_t input_closest, const Trajectory & prev_traj); TrajectoryPoint calcInterpolatedTrajectoryPoint( const Trajectory & trajectory, const geometry_msgs::msg::Pose & target_pose); - - bool checkHasReachedGoal(const Trajectory & traj, const size_t closest_idx, const double v0); - + bool checkHasReachedGoal( + const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj); TrajectoryData getTrajectoryData( const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose); TrajectoryData resampleTrajectoryData( const TrajectoryData & base_traj_data, const double resampling_s_interval, - const double max_traj_length, const double stop_dist); - + const double max_traj_length); Trajectory resampleTrajectory( const std::vector & base_index, const Trajectory & base_trajectory, const std::vector & query_index, const bool use_spline_for_pose = false); @@ -137,7 +131,6 @@ class OptimizationBasedPlanner : public PlannerInterface const rclcpp::Time & current_time, const Trajectory & traj, const size_t closest_idx, const std::vector & time_vec, const SBoundaries & s_boundaries, const VelocityOptimizer::OptimizationResult & opt_result); - // Calculation time watcher tier4_autoware_utils::StopWatch stop_watch_; @@ -150,7 +143,6 @@ class OptimizationBasedPlanner : public PlannerInterface rclcpp::Publisher::SharedPtr boundary_pub_; rclcpp::Publisher::SharedPtr optimized_sv_pub_; rclcpp::Publisher::SharedPtr optimized_st_graph_pub_; - rclcpp::Publisher::SharedPtr distance_to_closest_obj_pub_; rclcpp::Publisher::SharedPtr debug_calculation_time_; rclcpp::Publisher::SharedPtr debug_wall_marker_pub_; 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 1895fef50dc02..9ad2e7f2e59e8 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 @@ -22,8 +22,6 @@ #include "tier4_autoware_utils/system/stop_watch.hpp" #include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" -#include "tier4_planning_msgs/msg/stop_reason_array.hpp" -#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" #include "visualization_msgs/msg/marker_array.hpp" #include @@ -32,7 +30,6 @@ #include using tier4_debug_msgs::msg::Float32MultiArrayStamped; -using tier4_planning_msgs::msg::StopSpeedExceeded; class PIDBasedPlanner : public PlannerInterface { @@ -54,30 +51,19 @@ class PIDBasedPlanner : public PlannerInterface double dist_to_obstacle; }; - struct StopObstacleInfo - { - StopObstacleInfo(const TargetObstacle & obstacle_arg, const double dist_to_stop_arg) - : obstacle(obstacle_arg), dist_to_stop(dist_to_stop_arg) - { - } - TargetObstacle obstacle; - double dist_to_stop; - }; - PIDBasedPlanner( rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, const vehicle_info_util::VehicleInfo & vehicle_info); - Trajectory generateTrajectory( - const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, - DebugData & debug_data) override; + Trajectory generateCruiseTrajectory( + const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, + boost::optional & vel_limit, DebugData & debug_data) override; void updateParam(const std::vector & parameters) override; private: - void calcObstaclesToCruiseAndStop( + void calcObstaclesToCruise( const ObstacleCruisePlannerData & planner_data, - boost::optional & stop_obstacle_info, boost::optional & cruise_obstacle_info); double calcDistanceToObstacle( const ObstacleCruisePlannerData & planner_data, const TargetObstacle & obstacle); @@ -90,14 +76,6 @@ class PIDBasedPlanner : public PlannerInterface std::vector & debug_obstacles_to_cruise, visualization_msgs::msg::MarkerArray & debug_walls_marker); - Trajectory planStop( - const ObstacleCruisePlannerData & planner_data, - const boost::optional & stop_obstacle_info, DebugData & debug_data); - boost::optional doStop( - const ObstacleCruisePlannerData & planner_data, const StopObstacleInfo & stop_obstacle_info, - std::vector & debug_obstacles_to_stop, - visualization_msgs::msg::MarkerArray & debug_walls_marker) const; - void publishDebugValues(const ObstacleCruisePlannerData & planner_data) const; size_t findExtendedNearestIndex( @@ -138,8 +116,6 @@ class PIDBasedPlanner : public PlannerInterface stop_watch_; // publisher - rclcpp::Publisher::SharedPtr stop_reasons_pub_; - rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; rclcpp::Publisher::SharedPtr debug_values_pub_; boost::optional prev_target_vel_; 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 326cbac7bbc73..30e8934c53a13 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 @@ -21,7 +21,10 @@ #include "vehicle_info_util/vehicle_info_util.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "tier4_planning_msgs/msg/stop_reason_array.hpp" +#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" #include "tier4_planning_msgs/msg/velocity_limit.hpp" +#include "visualization_msgs/msg/marker_array.hpp" #include @@ -30,6 +33,7 @@ using autoware_auto_perception_msgs::msg::ObjectClassification; using autoware_auto_planning_msgs::msg::Trajectory; +using tier4_planning_msgs::msg::StopSpeedExceeded; using tier4_planning_msgs::msg::VelocityLimit; class PlannerInterface @@ -40,96 +44,30 @@ class PlannerInterface const vehicle_info_util::VehicleInfo & vehicle_info) : longitudinal_info_(longitudinal_info), vehicle_info_(vehicle_info) { - { // cruise obstacle type - if (node.declare_parameter("common.cruise_obstacle_type.unknown")) { - cruise_obstacle_types_.push_back(ObjectClassification::UNKNOWN); - } - if (node.declare_parameter("common.cruise_obstacle_type.car")) { - cruise_obstacle_types_.push_back(ObjectClassification::CAR); - } - if (node.declare_parameter("common.cruise_obstacle_type.truck")) { - cruise_obstacle_types_.push_back(ObjectClassification::TRUCK); - } - if (node.declare_parameter("common.cruise_obstacle_type.bus")) { - cruise_obstacle_types_.push_back(ObjectClassification::BUS); - } - if (node.declare_parameter("common.cruise_obstacle_type.trailer")) { - cruise_obstacle_types_.push_back(ObjectClassification::TRAILER); - } - if (node.declare_parameter("common.cruise_obstacle_type.motorcycle")) { - cruise_obstacle_types_.push_back(ObjectClassification::MOTORCYCLE); - } - if (node.declare_parameter("common.cruise_obstacle_type.bicycle")) { - cruise_obstacle_types_.push_back(ObjectClassification::BICYCLE); - } - if (node.declare_parameter("common.cruise_obstacle_type.pedestrian")) { - cruise_obstacle_types_.push_back(ObjectClassification::PEDESTRIAN); - } - } - - { // stop obstacle type - if (node.declare_parameter("common.stop_obstacle_type.unknown")) { - stop_obstacle_types_.push_back(ObjectClassification::UNKNOWN); - } - if (node.declare_parameter("common.stop_obstacle_type.car")) { - stop_obstacle_types_.push_back(ObjectClassification::CAR); - } - if (node.declare_parameter("common.stop_obstacle_type.truck")) { - stop_obstacle_types_.push_back(ObjectClassification::TRUCK); - } - if (node.declare_parameter("common.stop_obstacle_type.bus")) { - stop_obstacle_types_.push_back(ObjectClassification::BUS); - } - if (node.declare_parameter("common.stop_obstacle_type.trailer")) { - stop_obstacle_types_.push_back(ObjectClassification::TRAILER); - } - if (node.declare_parameter("common.stop_obstacle_type.motorcycle")) { - stop_obstacle_types_.push_back(ObjectClassification::MOTORCYCLE); - } - if (node.declare_parameter("common.stop_obstacle_type.bicycle")) { - stop_obstacle_types_.push_back(ObjectClassification::BICYCLE); - } - if (node.declare_parameter("common.stop_obstacle_type.pedestrian")) { - stop_obstacle_types_.push_back(ObjectClassification::PEDESTRIAN); - } - } + stop_reasons_pub_ = + node.create_publisher("~/output/stop_reasons", 1); + stop_speed_exceeded_pub_ = + node.create_publisher("~/output/stop_speed_exceeded", 1); } PlannerInterface() = default; void setParams( const bool is_showing_debug_info, const double min_behavior_stop_margin, - const double nearest_dist_deviation_threshold, const double nearest_yaw_deviation_threshold, - const double obstacle_velocity_threshold_from_cruise_to_stop, - const double obstacle_velocity_threshold_from_stop_to_cruise) + const double nearest_dist_deviation_threshold, const double nearest_yaw_deviation_threshold) { is_showing_debug_info_ = is_showing_debug_info; min_behavior_stop_margin_ = min_behavior_stop_margin; nearest_dist_deviation_threshold_ = nearest_dist_deviation_threshold; nearest_yaw_deviation_threshold_ = nearest_yaw_deviation_threshold; - obstacle_velocity_threshold_from_cruise_to_stop_ = - obstacle_velocity_threshold_from_cruise_to_stop; - obstacle_velocity_threshold_from_stop_to_cruise_ = - obstacle_velocity_threshold_from_stop_to_cruise; } - /* - // two kinds of velocity planning is supported. - // 1. getZeroVelocityIndexWithVelocityLimit - // returns zero velocity index and velocity limit - // 2. generateTrajectory - // returns trajectory with planned velocity - virtual boost::optional getZeroVelocityIndexWithVelocityLimit( - [[maybe_unused]] const ObstacleCruisePlannerData & planner_data, - [[maybe_unused]] boost::optional & vel_limit) - { - return {}; - }; - */ + Trajectory generateStopTrajectory( + const ObstacleCruisePlannerData & planner_data, DebugData & debug_data); - virtual Trajectory generateTrajectory( - const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, - DebugData & debug_data) = 0; + virtual Trajectory generateCruiseTrajectory( + const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, + boost::optional & vel_limit, DebugData & debug_data) = 0; void updateCommonParam(const std::vector & parameters) { @@ -158,52 +96,6 @@ class PlannerInterface smoothed_trajectory_ptr_ = traj; } - bool isCruiseObstacle(const uint8_t label) - { - const auto & types = cruise_obstacle_types_; - return std::find(types.begin(), types.end(), label) != types.end(); - } - - bool isStopObstacle(const uint8_t label) - { - const auto & types = stop_obstacle_types_; - return std::find(types.begin(), types.end(), label) != types.end(); - } - - bool isStopRequired(const TargetObstacle & obstacle, TargetObstacle & modified_obstacle) - { - const bool is_cruise_obstacle = isCruiseObstacle(obstacle.classification.label); - const bool is_stop_obstacle = isStopObstacle(obstacle.classification.label); - - if (is_stop_obstacle && !is_cruise_obstacle) { - modified_obstacle.has_stopped = true; - return true; - } - - if (is_cruise_obstacle) { - const auto itr = std::find_if( - prev_target_obstacles_.begin(), prev_target_obstacles_.end(), - [&](const auto & prev_target_obstacle) { - return obstacle.uuid == prev_target_obstacle.uuid; - }); - const bool has_already_stopped = (itr != prev_target_obstacles_.end()) && itr->has_stopped; - if (has_already_stopped) { - if (std::abs(obstacle.velocity) < obstacle_velocity_threshold_from_stop_to_cruise_) { - modified_obstacle.has_stopped = true; - return true; - } - } else { - if (std::abs(obstacle.velocity) < obstacle_velocity_threshold_from_cruise_to_stop_) { - modified_obstacle.has_stopped = true; - return true; - } - } - } - - modified_obstacle.has_stopped = false; - return false; - } - protected: // Parameters bool is_showing_debug_info_{false}; @@ -211,8 +103,10 @@ class PlannerInterface double min_behavior_stop_margin_; double nearest_dist_deviation_threshold_; double nearest_yaw_deviation_threshold_; - double obstacle_velocity_threshold_from_cruise_to_stop_; - double obstacle_velocity_threshold_from_stop_to_cruise_; + + // Publishers + rclcpp::Publisher::SharedPtr stop_reasons_pub_; + rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; // Vehicle Parameters vehicle_info_util::VehicleInfo vehicle_info_; @@ -220,21 +114,15 @@ class PlannerInterface // TODO(shimizu) remove these parameters Trajectory::ConstSharedPtr smoothed_trajectory_ptr_; - std::vector prev_target_obstacles_; - double calcRSSDistance( const double ego_vel, const double obj_vel, const double margin = 0.0) const { const auto & i = longitudinal_info_; const double rss_dist_with_margin = - ego_vel * i.idling_time + std::pow(ego_vel, 2) * 0.5 / std::abs(i.min_accel) - + ego_vel * i.idling_time + std::pow(ego_vel, 2) * 0.5 / std::abs(i.min_ego_accel_for_rss) - std::pow(obj_vel, 2) * 0.5 / std::abs(i.min_object_accel_for_rss) + margin; return rss_dist_with_margin; } - -private: - std::vector cruise_obstacle_types_; - std::vector stop_obstacle_types_; }; #endif // OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ diff --git a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp index 9bdf9d0864d8a..3899d1f413ba5 100644 --- a/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp +++ b/planning/obstacle_cruise_planner/include/obstacle_cruise_planner/utils.hpp @@ -15,6 +15,8 @@ #ifndef OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ #define OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ +#include "tier4_autoware_utils/tier4_autoware_utils.hpp" + #include #include "autoware_auto_perception_msgs/msg/object_classification.hpp" @@ -26,6 +28,7 @@ #include +#include #include #include @@ -40,12 +43,9 @@ visualization_msgs::msg::Marker getObjectMarker( const double r, const double g, const double b); boost::optional calcForwardPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t nearest_idx, + const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t start_idx, const double target_length); -geometry_msgs::msg::Pose lerpByPose( - const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t); - boost::optional lerpByTimeStamp( const autoware_auto_perception_msgs::msg::PredictedPath & path, const rclcpp::Duration & rel_time); @@ -61,6 +61,62 @@ boost::optional getCurrentObjectPoseFromPredictedPaths geometry_msgs::msg::Pose getCurrentObjectPose( const autoware_auto_perception_msgs::msg::PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, const rclcpp::Time & current_time, const bool use_prediction); + +template +size_t getIndexWithLongitudinalOffset( + const T & points, const double longitudinal_offset, boost::optional start_idx) +{ + if (points.empty()) { + throw std::logic_error("points is empty."); + } + + if (start_idx) { + if (/*start_idx.get() < 0 || */ points.size() <= start_idx.get()) { + throw std::out_of_range("start_idx is out of range."); + } + } else { + if (longitudinal_offset > 0) { + start_idx = 0; + } else { + start_idx = points.size() - 1; + } + } + + double sum_length = 0.0; + if (longitudinal_offset > 0) { + for (size_t i = start_idx.get(); i < points.size() - 1; ++i) { + const double segment_length = + tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + sum_length += segment_length; + if (sum_length >= longitudinal_offset) { + const double front_length = segment_length; + const double back_length = sum_length - longitudinal_offset; + if (front_length < back_length) { + return i; + } else { + return i + 1; + } + } + } + return points.size() - 1; + } + + for (size_t i = start_idx.get(); i > 0; --i) { + const double segment_length = + tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); + sum_length += segment_length; + if (sum_length >= -longitudinal_offset) { + const double front_length = segment_length; + const double back_length = sum_length + longitudinal_offset; + if (front_length < back_length) { + return i; + } else { + return i + 1; + } + } + } + return 0; +} } // namespace obstacle_cruise_utils #endif // OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ diff --git a/planning/obstacle_cruise_planner/obstacle_cruise_planner-design.md b/planning/obstacle_cruise_planner/obstacle_cruise_planner-design.md index 3a78af3827b2e..deb41e5267f41 100644 --- a/planning/obstacle_cruise_planner/obstacle_cruise_planner-design.md +++ b/planning/obstacle_cruise_planner/obstacle_cruise_planner-design.md @@ -322,5 +322,6 @@ Red wall which means a safe distance to stop if the ego's front meets the wall i - Common - When the obstacle pose or velocity estimation has a delay, the ego sometimes will go close to the front vehicle keeping deceleration. - Current implementation only uses predicted objects message for static/dynamic obstacles and does not use pointcloud. Therefore, if object recognition is lost, the ego cannot deal with the lost obstacle. + - The current predicted paths for obstacle's lane change does not have enough precision for obstacle_cruise_planner. Therefore, we set `rough_detection_area` a small value. - PID-based planner - The algorithm strongly depends on the velocity smoothing package (`motion_velocity_smoother` by default) whether or not the ego realizes the designated target speed. If the velocity smoothing package is updated, please take care of the vehicle's behavior as much as possible. diff --git a/planning/obstacle_cruise_planner/src/node.cpp b/planning/obstacle_cruise_planner/src/node.cpp index 4a36e6510ec10..fb17e48752d20 100644 --- a/planning/obstacle_cruise_planner/src/node.cpp +++ b/planning/obstacle_cruise_planner/src/node.cpp @@ -255,6 +255,8 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & declare_parameter("obstacle_filtering.crossing_obstacle_velocity_threshold"); obstacle_filtering_param_.collision_time_margin = declare_parameter("obstacle_filtering.collision_time_margin"); + obstacle_filtering_param_.outside_rough_detection_area_expand_width = + declare_parameter("obstacle_filtering.outside_rough_detection_area_expand_width"); obstacle_filtering_param_.ego_obstacle_overlap_time_threshold = declare_parameter("obstacle_filtering.ego_obstacle_overlap_time_threshold"); obstacle_filtering_param_.max_prediction_time_for_collision_check = @@ -323,8 +325,61 @@ ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & declare_parameter("common.obstacle_velocity_threshold_from_stop_to_cruise"); planner_ptr_->setParams( is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_, obstacle_velocity_threshold_from_cruise_to_stop_, - obstacle_velocity_threshold_from_stop_to_cruise_); + nearest_yaw_deviation_threshold_); + } + + { // cruise obstacle type + if (declare_parameter("common.cruise_obstacle_type.unknown")) { + cruise_obstacle_types_.push_back(ObjectClassification::UNKNOWN); + } + if (declare_parameter("common.cruise_obstacle_type.car")) { + cruise_obstacle_types_.push_back(ObjectClassification::CAR); + } + if (declare_parameter("common.cruise_obstacle_type.truck")) { + cruise_obstacle_types_.push_back(ObjectClassification::TRUCK); + } + if (declare_parameter("common.cruise_obstacle_type.bus")) { + cruise_obstacle_types_.push_back(ObjectClassification::BUS); + } + if (declare_parameter("common.cruise_obstacle_type.trailer")) { + cruise_obstacle_types_.push_back(ObjectClassification::TRAILER); + } + if (declare_parameter("common.cruise_obstacle_type.motorcycle")) { + cruise_obstacle_types_.push_back(ObjectClassification::MOTORCYCLE); + } + if (declare_parameter("common.cruise_obstacle_type.bicycle")) { + cruise_obstacle_types_.push_back(ObjectClassification::BICYCLE); + } + if (declare_parameter("common.cruise_obstacle_type.pedestrian")) { + cruise_obstacle_types_.push_back(ObjectClassification::PEDESTRIAN); + } + } + + { // stop obstacle type + if (declare_parameter("common.stop_obstacle_type.unknown")) { + stop_obstacle_types_.push_back(ObjectClassification::UNKNOWN); + } + if (declare_parameter("common.stop_obstacle_type.car")) { + stop_obstacle_types_.push_back(ObjectClassification::CAR); + } + if (declare_parameter("common.stop_obstacle_type.truck")) { + stop_obstacle_types_.push_back(ObjectClassification::TRUCK); + } + if (declare_parameter("common.stop_obstacle_type.bus")) { + stop_obstacle_types_.push_back(ObjectClassification::BUS); + } + if (declare_parameter("common.stop_obstacle_type.trailer")) { + stop_obstacle_types_.push_back(ObjectClassification::TRAILER); + } + if (declare_parameter("common.stop_obstacle_type.motorcycle")) { + stop_obstacle_types_.push_back(ObjectClassification::MOTORCYCLE); + } + if (declare_parameter("common.stop_obstacle_type.bicycle")) { + stop_obstacle_types_.push_back(ObjectClassification::BICYCLE); + } + if (declare_parameter("common.stop_obstacle_type.pedestrian")) { + stop_obstacle_types_.push_back(ObjectClassification::PEDESTRIAN); + } } // wait for first self pose @@ -356,8 +411,7 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( parameters, "common.is_showing_debug_info", is_showing_debug_info_); planner_ptr_->setParams( is_showing_debug_info_, min_behavior_stop_margin_, nearest_dist_deviation_threshold_, - nearest_yaw_deviation_threshold_, obstacle_velocity_threshold_from_cruise_to_stop_, - obstacle_velocity_threshold_from_stop_to_cruise_); + nearest_yaw_deviation_threshold_); // obstacle_filtering tier4_autoware_utils::updateParam( @@ -375,6 +429,9 @@ rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( tier4_autoware_utils::updateParam( parameters, "obstacle_filtering.collision_time_margin", obstacle_filtering_param_.collision_time_margin); + tier4_autoware_utils::updateParam( + parameters, "obstacle_filtering.outside_rough_detection_area_expand_width", + obstacle_filtering_param_.outside_rough_detection_area_expand_width); tier4_autoware_utils::updateParam( parameters, "obstacle_filtering.ego_obstacle_overlap_time_threshold", obstacle_filtering_param_.ego_obstacle_overlap_time_threshold); @@ -429,9 +486,13 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms DebugData debug_data; const auto planner_data = createPlannerData(*msg, current_pose_ptr->pose, debug_data); - // generate Trajectory + // stop planning + const auto stop_traj = planner_ptr_->generateStopTrajectory(planner_data, debug_data); + + // cruise planning boost::optional vel_limit; - const auto output_traj = planner_ptr_->generateTrajectory(planner_data, vel_limit, debug_data); + const auto output_traj = + planner_ptr_->generateCruiseTrajectory(planner_data, stop_traj, vel_limit, debug_data); // publisher external velocity limit if required publishVelocityLimit(vel_limit); @@ -450,24 +511,39 @@ void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr ms calculation_time); } +bool ObstacleCruisePlannerNode::isCruiseObstacle(const uint8_t label) +{ + const auto & types = cruise_obstacle_types_; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +bool ObstacleCruisePlannerNode::isStopObstacle(const uint8_t label) +{ + const auto & types = stop_obstacle_types_; + return std::find(types.begin(), types.end(), label) != types.end(); +} + ObstacleCruisePlannerData ObstacleCruisePlannerNode::createPlannerData( const Trajectory & trajectory, const geometry_msgs::msg::Pose & current_pose, DebugData & debug_data) { 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 ObstacleCruisePlannerData planner_data; - planner_data.current_time = now(); + 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 = - filterObstacles(*in_objects_ptr_, trajectory, current_pose, current_vel, debug_data); + planner_data.target_obstacles = target_obstacles; // print calculation time const double calculation_time = stop_watch_.toc(__func__); @@ -519,9 +595,8 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( const auto object_id = toHexString(predicted_object.object_id).substr(0, 4); // filter object whose label is not cruised or stopped - const bool is_target_obstacle = - planner_ptr_->isStopObstacle(predicted_object.classification.front().label) || - planner_ptr_->isCruiseObstacle(predicted_object.classification.front().label); + const bool is_target_obstacle = isStopObstacle(predicted_object.classification.front().label) || + isCruiseObstacle(predicted_object.classification.front().label); if (!is_target_obstacle) { RCLCPP_INFO_EXPRESSION( get_logger(), is_showing_debug_info_, "Ignore obstacle (%s) since its label is not target.", @@ -603,6 +678,16 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( continue; } + if ( + std::fabs(dist_from_obstacle_to_traj) > + vehicle_info_.vehicle_width_m + + obstacle_filtering_param_.outside_rough_detection_area_expand_width) { + RCLCPP_INFO_EXPRESSION( + get_logger(), is_showing_debug_info_, + "Ignore outside obstacle (%s) since it is far from the trajectory.", object_id.c_str()); + continue; + } + const auto predicted_path_with_highest_confidence = getHighestConfidencePredictedPath(predicted_object); @@ -641,6 +726,43 @@ std::vector ObstacleCruisePlannerNode::filterObstacles( return target_obstacles; } +void ObstacleCruisePlannerNode::updateHasStopped(std::vector & target_obstacles) +{ + for (auto & obstacle : target_obstacles) { + const bool is_cruise_obstacle = isCruiseObstacle(obstacle.classification.label); + const bool is_stop_obstacle = isStopObstacle(obstacle.classification.label); + + if (is_stop_obstacle && !is_cruise_obstacle) { + obstacle.has_stopped = true; + continue; + } + + if (is_cruise_obstacle) { + const auto itr = std::find_if( + prev_target_obstacles_.begin(), prev_target_obstacles_.end(), + [&](const auto & prev_target_obstacle) { + return obstacle.uuid == prev_target_obstacle.uuid; + }); + const bool has_already_stopped = (itr != prev_target_obstacles_.end()) && itr->has_stopped; + if (has_already_stopped) { + if (std::abs(obstacle.velocity) < obstacle_velocity_threshold_from_stop_to_cruise_) { + obstacle.has_stopped = true; + continue; + } + } else { + if (std::abs(obstacle.velocity) < obstacle_velocity_threshold_from_cruise_to_stop_) { + obstacle.has_stopped = true; + continue; + } + } + } + + obstacle.has_stopped = false; + } + + prev_target_obstacles_ = target_obstacles; +} + geometry_msgs::msg::Point ObstacleCruisePlannerNode::calcNearestCollisionPoint( const size_t & first_within_idx, const std::vector & collision_points, const Trajectory & decimated_traj) 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 1346240e93720..a41b5665f7e03 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 @@ -31,7 +31,6 @@ constexpr double ZERO_VEL_THRESHOLD = 0.01; constexpr double CLOSE_S_DIST_THRESHOLD = 1e-3; -// TODO(shimizu) Is is ok to use planner_data.current_time instead of get_clock()->now()? namespace { inline void convertEulerAngleToMonotonic(std::vector & a) @@ -51,57 +50,6 @@ inline tf2::Vector3 getTransVector3( return tf2::Vector3(dx, dy, dz); } -// TODO(shimizu) consider dist/yaw threshold for nearest index -boost::optional calcForwardPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, - const geometry_msgs::msg::Point & point, const double target_length) -{ - if (traj.points.empty()) { - return {}; - } - - const size_t nearest_idx = tier4_autoware_utils::findNearestIndex(traj.points, point); - - size_t search_idx = nearest_idx; - double length_to_search_idx = 0.0; - for (; search_idx < traj.points.size(); ++search_idx) { - length_to_search_idx = - tier4_autoware_utils::calcSignedArcLength(traj.points, nearest_idx, search_idx); - if (length_to_search_idx > target_length) { - break; - } else if (search_idx == traj.points.size() - 1) { - return {}; - } - } - - if (search_idx == 0 && !traj.points.empty()) { - return traj.points.at(0).pose; - } - - const auto & pre_pose = traj.points.at(search_idx - 1).pose; - const auto & next_pose = traj.points.at(search_idx).pose; - - geometry_msgs::msg::Pose target_pose; - - // lerp position - const double seg_length = - tier4_autoware_utils::calcDistance2d(pre_pose.position, next_pose.position); - const double lerp_ratio = (length_to_search_idx - target_length) / seg_length; - target_pose.position.x = - pre_pose.position.x + (next_pose.position.x - pre_pose.position.x) * lerp_ratio; - target_pose.position.y = - pre_pose.position.y + (next_pose.position.y - pre_pose.position.y) * lerp_ratio; - target_pose.position.z = - pre_pose.position.z + (next_pose.position.z - pre_pose.position.z) * lerp_ratio; - - // lerp orientation - const double pre_yaw = tf2::getYaw(pre_pose.orientation); - const double next_yaw = tf2::getYaw(next_pose.orientation); - target_pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(pre_yaw + (next_yaw - pre_yaw) * lerp_ratio); - - return target_pose; -} } // namespace OptimizationBasedPlanner::OptimizationBasedPlanner( @@ -159,107 +107,84 @@ OptimizationBasedPlanner::OptimizationBasedPlanner( optimized_sv_pub_ = node.create_publisher("~/optimized_sv_trajectory", 1); optimized_st_graph_pub_ = node.create_publisher("~/optimized_st_graph", 1); boundary_pub_ = node.create_publisher("~/boundary", 1); - distance_to_closest_obj_pub_ = - node.create_publisher("~/distance_to_closest_obj", 1); debug_calculation_time_ = node.create_publisher("~/calculation_time", 1); debug_wall_marker_pub_ = node.create_publisher("~/debug/wall_marker", 1); } -Trajectory OptimizationBasedPlanner::generateTrajectory( - const ObstacleCruisePlannerData & planner_data, +Trajectory OptimizationBasedPlanner::generateCruiseTrajectory( + const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, [[maybe_unused]] boost::optional & vel_limit, [[maybe_unused]] DebugData & debug_data) { - // TODO(shimizu) refactor obstacle stop; - prev_target_obstacles_.clear(); - // Create Time Vector defined by resampling time interval const std::vector time_vec = createTimeVector(); if (time_vec.size() < 2) { RCLCPP_ERROR( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "Resolution size is not enough"); - prev_output_ = planner_data.traj; - return planner_data.traj; + prev_output_ = stop_traj; + return stop_traj; } // Get the nearest point on the trajectory const auto closest_idx = tier4_autoware_utils::findNearestIndex( - planner_data.traj.points, planner_data.current_pose, nearest_yaw_deviation_threshold_); + stop_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_ = planner_data.traj; - return planner_data.traj; + prev_output_ = stop_traj; + return stop_traj; } // Transform original trajectory to TrajectoryData - const auto base_traj_data = getTrajectoryData(planner_data.traj, planner_data.current_pose); + const auto base_traj_data = getTrajectoryData(stop_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_ = planner_data.traj; - return planner_data.traj; + prev_output_ = stop_traj; + return stop_traj; } - // Get Closest Stop Point for static obstacles - double closest_stop_dist = getClosestStopDistance(planner_data, base_traj_data); - // Compute maximum velocity double v_max = 0.0; - for (const auto & point : planner_data.traj.points) { + for (const auto & point : stop_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.current_vel, planner_data.traj, *closest_idx, prev_output_, closest_stop_dist); + std::tie(v0, a0) = calcInitialMotion(planner_data, stop_traj, *closest_idx, prev_output_); a0 = std::min(longitudinal_info_.max_accel, std::max(a0, longitudinal_info_.min_accel)); - // If closest distance is too close, return zero velocity - if (closest_stop_dist < 0.01) { - RCLCPP_DEBUG( - rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), - "Closest Stop Point is too close"); - - auto output_traj = planner_data.traj; - output_traj.points.at(*closest_idx).longitudinal_velocity_mps = v0; - for (size_t i = *closest_idx + 1; i < output_traj.points.size(); ++i) { - output_traj.points.at(i).longitudinal_velocity_mps = 0.0; - } - prev_output_ = output_traj; - return output_traj; - } - // Check trajectory size - if (planner_data.traj.points.size() - *closest_idx <= 2) { + if (stop_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_ = planner_data.traj; - return planner_data.traj; + prev_output_ = stop_traj; + return stop_traj; } // Check if reached goal - if (checkHasReachedGoal(planner_data.traj, *closest_idx, v0)) { - prev_output_ = planner_data.traj; - return planner_data.traj; + if (checkHasReachedGoal(planner_data, stop_traj)) { + prev_output_ = stop_traj; + return stop_traj; } // Resample base trajectory data by time - const auto resampled_traj_data = resampleTrajectoryData( - base_traj_data, resampling_s_interval_, max_trajectory_length_, closest_stop_dist); + const auto resampled_traj_data = + resampleTrajectoryData(base_traj_data, resampling_s_interval_, max_trajectory_length_); if (resampled_traj_data.traj.points.size() < 2) { RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "The number of points on the resampled trajectory data is too small"); - prev_output_ = planner_data.traj; - return planner_data.traj; + prev_output_ = stop_traj; + return stop_traj; } // Get S Boundaries from the obstacle @@ -268,8 +193,8 @@ Trajectory OptimizationBasedPlanner::generateTrajectory( RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "No Dangerous Objects around the ego vehicle"); - prev_output_ = planner_data.traj; - return planner_data.traj; + prev_output_ = stop_traj; + return stop_traj; } // Optimization @@ -307,8 +232,7 @@ Trajectory OptimizationBasedPlanner::generateTrajectory( // Publish Debug trajectories publishDebugTrajectory( - planner_data.current_time, planner_data.traj, *closest_idx, time_vec, *s_boundaries, - optimized_result); + planner_data.current_time, stop_traj, *closest_idx, time_vec, *s_boundaries, optimized_result); // Transformation from t to s const auto processed_result = processOptimizedResult(data.v0, optimized_result); @@ -316,15 +240,15 @@ Trajectory OptimizationBasedPlanner::generateTrajectory( RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "Processed Result is empty"); - prev_output_ = planner_data.traj; - return planner_data.traj; + prev_output_ = stop_traj; + return stop_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 = planner_data.traj; + auto output = stop_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; @@ -335,11 +259,12 @@ Trajectory OptimizationBasedPlanner::generateTrajectory( RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "Optimized Trajectory is too small"); - prev_output_ = planner_data.traj; - return planner_data.traj; + prev_output_ = stop_traj; + return stop_traj; } // Get Zero Velocity Position + double closest_stop_dist = std::numeric_limits::max(); for (size_t i = 0; i < opt_velocity.size(); ++i) { if (opt_velocity.at(i) < ZERO_VEL_THRESHOLD) { const double zero_vel_s = opt_position.at(i); @@ -347,6 +272,12 @@ Trajectory OptimizationBasedPlanner::generateTrajectory( break; } } + const auto traj_stop_dist = tier4_autoware_utils::calcDistanceToForwardStopPoint( + stop_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); + } size_t break_id = base_traj_data.s.size(); bool has_insert_stop_point = false; @@ -401,9 +332,9 @@ Trajectory OptimizationBasedPlanner::generateTrajectory( } Trajectory output; - output.header = planner_data.traj.header; + output.header = stop_traj.header; for (size_t i = 0; i < *closest_idx; ++i) { - auto point = planner_data.traj.points.at(i); + auto point = stop_traj.points.at(i); point.longitudinal_velocity_mps = data.v0; output.points.push_back(point); } @@ -452,91 +383,13 @@ std::vector OptimizationBasedPlanner::createTimeVector() return time_vec; } -double OptimizationBasedPlanner::getClosestStopDistance( - const ObstacleCruisePlannerData & planner_data, const TrajectoryData & ego_traj_data) -{ - auto modified_target_obstacles = planner_data.target_obstacles; - - const auto & current_time = planner_data.current_time; - double closest_stop_dist = ego_traj_data.s.back(); - const auto closest_stop_id = - tier4_autoware_utils::searchZeroVelocityIndex(ego_traj_data.traj.points); - closest_stop_dist = closest_stop_id - ? std::min(closest_stop_dist, ego_traj_data.s.at(*closest_stop_id)) - : closest_stop_dist; - - double closest_obj_distance = ego_traj_data.s.back(); - boost::optional closest_obj; - for (size_t o_idx = 0; o_idx < planner_data.target_obstacles.size(); ++o_idx) { - const auto & obj = planner_data.target_obstacles.at(o_idx); - const auto obj_base_time = obj.time_stamp; - - // Ignore obstacles that are not required to stop - if (!isStopRequired(obj, modified_target_obstacles.at(o_idx))) { - continue; - } - - // Get current pose from object's predicted path - const auto current_object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPaths( - obj.predicted_paths, obj_base_time, current_time); - if (!current_object_pose) { - continue; - } - - // Get current object.kinematics - ObjectData obj_data; - obj_data.pose = current_object_pose.get(); - obj_data.length = obj.shape.dimensions.x; - obj_data.width = obj.shape.dimensions.y; - obj_data.time = std::max((obj_base_time - current_time).seconds(), 0.0); - const auto dist_to_collision_point = getDistanceToCollisionPoint(ego_traj_data, obj_data); - - // Calculate Safety Distance - const auto & safe_distance_margin = longitudinal_info_.safe_distance_margin; - const auto & ego_vehicle_offset = vehicle_info_.max_longitudinal_offset_m; - const double object_offset = obj_data.length / 2.0; - const double safety_distance = ego_vehicle_offset + object_offset + safe_distance_margin; - - // If the object is on the current ego trajectory, - // we assume the object travels along ego trajectory - if (dist_to_collision_point) { - const double stop_dist = std::max(*dist_to_collision_point - safety_distance, 0.0); - closest_stop_dist = std::min(stop_dist, closest_stop_dist); - - // Update Distance to the closest object on the ego trajectory - const double current_obj_distance = std::max( - *dist_to_collision_point - safety_distance + safe_distance_margin, -safety_distance); - closest_obj_distance = std::min(closest_obj_distance, current_obj_distance); - closest_obj = obj; - } - } - - // TODO(shimizu) refactor obstacle stop; - prev_target_obstacles_ = modified_target_obstacles; - - // Publish distance from the ego vehicle to the object which is on the trajectory - if (closest_obj && closest_obj_distance < ego_traj_data.s.back()) { - Float32Stamped dist_to_obj; - dist_to_obj.stamp = planner_data.current_time; - dist_to_obj.data = closest_obj_distance; - distance_to_closest_obj_pub_->publish(dist_to_obj); - - RCLCPP_DEBUG( - rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), - "Closest Object Distance %f", closest_obj_distance); - RCLCPP_DEBUG( - rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), - "Closest Object Velocity %f", closest_obj.get().velocity); - } - - return closest_stop_dist; -} - // v0, a0 std::tuple OptimizationBasedPlanner::calcInitialMotion( - const double current_vel, const Trajectory & input_traj, const size_t input_closest, - const Trajectory & prev_traj, const double closest_stop_dist) + const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, + const size_t input_closest, const Trajectory & prev_traj) { + const auto & current_vel = planner_data.current_vel; + const auto & input_traj = stop_traj; const double vehicle_speed{std::abs(current_vel)}; const double target_vel{std::abs(input_traj.points.at(input_closest).longitudinal_velocity_mps)}; @@ -552,11 +405,11 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( TrajectoryPoint prev_output_closest_point; if (smoothed_trajectory_ptr_) { - prev_output_closest_point = calcInterpolatedTrajectoryPoint( - *smoothed_trajectory_ptr_, input_traj.points.at(input_closest).pose); + prev_output_closest_point = + calcInterpolatedTrajectoryPoint(*smoothed_trajectory_ptr_, planner_data.current_pose); } else { prev_output_closest_point = - calcInterpolatedTrajectoryPoint(prev_traj, input_traj.points.at(input_closest).pose); + calcInterpolatedTrajectoryPoint(prev_traj, planner_data.current_pose); } // when velocity tracking deviation is large @@ -578,25 +431,22 @@ std::tuple OptimizationBasedPlanner::calcInitialMotion( const double engage_vel_thr = engage_velocity_ * engage_exit_ratio_; if (vehicle_speed < engage_vel_thr) { if (target_vel >= engage_velocity_) { - const auto idx = tier4_autoware_utils::searchZeroVelocityIndex(input_traj.points); - const double zero_vel_dist = - idx ? tier4_autoware_utils::calcDistance2d( - input_traj.points.at(*idx), input_traj.points.at(input_closest)) - : 0.0; - const double stop_dist = std::min(zero_vel_dist, closest_stop_dist); - if (!idx || stop_dist > stop_dist_to_prohibit_engage_) { + const auto stop_dist = tier4_autoware_utils::calcDistanceToForwardStopPoint( + input_traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); + if ((stop_dist && *stop_dist > stop_dist_to_prohibit_engage_) || !stop_dist) { initial_vel = engage_velocity_; initial_acc = engage_acceleration_; RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "calcInitialMotion : vehicle speed is low (%.3f), and desired speed is high (%.3f). Use " "engage speed (%.3f) until vehicle speed reaches engage_vel_thr (%.3f). stop_dist = %.3f", - vehicle_speed, target_vel, engage_velocity_, engage_vel_thr, stop_dist); + vehicle_speed, target_vel, engage_velocity_, engage_vel_thr, stop_dist.get()); return std::make_tuple(initial_vel, initial_acc); - } else { + } else if (stop_dist) { RCLCPP_DEBUG( rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), - "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist); + "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist.get()); } } else if (target_vel > 0.0) { auto clock{rclcpp::Clock{RCL_ROS_TIME}}; @@ -634,39 +484,35 @@ TrajectoryPoint OptimizationBasedPlanner::calcInterpolatedTrajectoryPoint( const size_t segment_idx = tier4_autoware_utils::findNearestSegmentIndex(trajectory.points, target_pose.position); - auto v1 = getTransVector3( + const auto v1 = getTransVector3( trajectory.points.at(segment_idx).pose, trajectory.points.at(segment_idx + 1).pose); - auto v2 = getTransVector3(trajectory.points.at(segment_idx).pose, target_pose); - // calc internal proportion - const double prop{std::max(0.0, std::min(1.0, v1.dot(v2) / v1.length2()))}; + const auto v2 = getTransVector3(trajectory.points.at(segment_idx).pose, target_pose); - auto interpolate = [&prop](double x1, double x2) { return prop * x1 + (1.0 - prop) * x2; }; + // Calc interpolation ratio + const double prop{std::clamp(v1.dot(v2) / v1.length2(), 0.0, 1.0)}; { - const auto & seg_pt = trajectory.points.at(segment_idx); + const auto & curr_pt = trajectory.points.at(segment_idx); const auto & next_pt = trajectory.points.at(segment_idx + 1); - traj_p.longitudinal_velocity_mps = - interpolate(next_pt.longitudinal_velocity_mps, seg_pt.longitudinal_velocity_mps); - traj_p.acceleration_mps2 = interpolate(next_pt.acceleration_mps2, seg_pt.acceleration_mps2); - traj_p.pose.position.x = interpolate(next_pt.pose.position.x, seg_pt.pose.position.x); - traj_p.pose.position.y = interpolate(next_pt.pose.position.y, seg_pt.pose.position.y); - traj_p.pose.position.z = interpolate(next_pt.pose.position.z, seg_pt.pose.position.z); + 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 Trajectory & traj, const size_t closest_idx, const double v0) + const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj) { // If goal is close and current velocity is low, we don't optimize trajectory - const auto closest_stop_id = tier4_autoware_utils::searchZeroVelocityIndex(traj.points); - if (closest_stop_id) { - const double closest_stop_dist = - tier4_autoware_utils::calcSignedArcLength(traj.points, closest_idx, *closest_stop_id); - if (closest_stop_dist < 0.5 && v0 < 0.6) { - return true; - } + const auto closest_stop_dist = tier4_autoware_utils::calcDistanceToForwardStopPoint( + stop_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; } return false; @@ -676,17 +522,17 @@ OptimizationBasedPlanner::TrajectoryData OptimizationBasedPlanner::getTrajectory const Trajectory & traj, const geometry_msgs::msg::Pose & current_pose) { TrajectoryData base_traj; - const auto closest_segment_idx = + const auto nearest_segment_idx = tier4_autoware_utils::findNearestSegmentIndex(traj.points, current_pose.position); const auto interpolated_point = calcInterpolatedTrajectoryPoint(traj, current_pose); const auto dist = tier4_autoware_utils::calcDistance2d( - interpolated_point.pose.position, traj.points.at(closest_segment_idx).pose.position); + interpolated_point.pose.position, traj.points.at(nearest_segment_idx).pose.position); const auto current_point = - dist > CLOSE_S_DIST_THRESHOLD ? interpolated_point : traj.points.at(closest_segment_idx); + dist > CLOSE_S_DIST_THRESHOLD ? interpolated_point : traj.points.at(nearest_segment_idx); base_traj.traj.points.push_back(current_point); base_traj.s.push_back(0.0); - for (size_t id = closest_segment_idx + 1; id < traj.points.size(); ++id) { + for (size_t id = nearest_segment_idx + 1; id < traj.points.size(); ++id) { const auto prev_point = base_traj.traj.points.back(); const double ds = tier4_autoware_utils::calcDistance2d( prev_point.pose.position, traj.points.at(id).pose.position); @@ -704,7 +550,7 @@ OptimizationBasedPlanner::TrajectoryData OptimizationBasedPlanner::getTrajectory OptimizationBasedPlanner::TrajectoryData OptimizationBasedPlanner::resampleTrajectoryData( const TrajectoryData & base_traj_data, const double resampling_s_interval, - const double max_traj_length, const double stop_dist) + const double max_traj_length) { // Create Base Keys std::vector base_s(base_traj_data.s.size()); @@ -715,8 +561,7 @@ OptimizationBasedPlanner::TrajectoryData OptimizationBasedPlanner::resampleTraje // Obtain trajectory length until the velocity is zero or stop dist const auto closest_stop_id = tier4_autoware_utils::searchZeroVelocityIndex(base_traj_data.traj.points); - const double closest_stop_dist = - closest_stop_id ? std::min(stop_dist, base_traj_data.s.at(*closest_stop_id)) : stop_dist; + const double closest_stop_dist = closest_stop_id ? base_s.at(*closest_stop_id) : base_s.back(); const double traj_length = std::min(closest_stop_dist, std::min(base_s.back(), max_traj_length)); // Create Query Keys @@ -725,7 +570,7 @@ OptimizationBasedPlanner::TrajectoryData OptimizationBasedPlanner::resampleTraje resampled_s.push_back(s); } - if (traj_length - resampled_s.back() < CLOSE_S_DIST_THRESHOLD) { + if (!resampled_s.empty() && traj_length - resampled_s.back() < CLOSE_S_DIST_THRESHOLD) { resampled_s.back() = traj_length; } else { resampled_s.push_back(traj_length); @@ -820,7 +665,7 @@ boost::optional OptimizationBasedPlanner::getSBoundaries( const auto & obj = planner_data.target_obstacles.at(o_idx); const auto obj_base_time = planner_data.target_obstacles.at(o_idx).time_stamp; // Only see cruise obstacles - if (!isCruiseObstacle(obj.classification.label)) { + if (obj.has_stopped) { continue; } @@ -867,14 +712,13 @@ boost::optional OptimizationBasedPlanner::getSBoundaries( const auto current_object_pose = obstacle_cruise_utils::getCurrentObjectPoseFromPredictedPaths( obj.predicted_paths, obj.time_stamp, current_time); - const auto marker_pose = calcForwardPose( - ego_traj_data.traj, planner_data.current_pose.position, min_slow_down_point_length); + const auto marker_pose = + obstacle_cruise_utils::calcForwardPose(ego_traj_data.traj, 0, min_slow_down_point_length); if (marker_pose) { visualization_msgs::msg::MarkerArray wall_msg; - const double obj_vel = std::abs(obj.velocity); - if (obj_vel < obstacle_velocity_threshold_from_cruise_to_stop_) { + if (obj.has_stopped) { const auto markers = tier4_autoware_utils::createStopVirtualWallMarker( marker_pose.get(), "obstacle to follow", current_time, 0); tier4_autoware_utils::appendMarkerArray(markers, &wall_msg); @@ -962,8 +806,7 @@ boost::optional OptimizationBasedPlanner::getSBoundaries( const double v_obj = std::abs(object.velocity); double current_s_obj = std::max(dist_to_collision_point - safety_distance, 0.0); - const double current_v_obj = - v_obj < obstacle_velocity_threshold_from_cruise_to_stop_ ? 0.0 : v_obj; + const double current_v_obj = object.has_stopped ? 0.0 : v_obj; const double initial_s_upper_bound = current_s_obj + (current_v_obj * current_v_obj) / (2 * std::fabs(min_object_accel_for_rss)); s_boundaries.front().max_s = std::clamp(initial_s_upper_bound, 0.0, s_boundaries.front().max_s); @@ -989,8 +832,7 @@ boost::optional OptimizationBasedPlanner::getSBoundaries( const double & min_object_accel_for_rss = longitudinal_info_.min_object_accel_for_rss; const double abs_obj_vel = std::abs(object.velocity); - const double v_obj = - abs_obj_vel < obstacle_velocity_threshold_from_cruise_to_stop_ ? 0.0 : abs_obj_vel; + const double v_obj = object.has_stopped ? 0.0 : abs_obj_vel; SBoundaries s_boundaries(time_vec.size()); for (size_t i = 0; i < s_boundaries.size(); ++i) { 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 a684137c462e5..92d35568f52f1 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 @@ -21,15 +21,6 @@ namespace { -StopSpeedExceeded createStopSpeedExceededMsg( - const rclcpp::Time & current_time, const bool stop_flag) -{ - StopSpeedExceeded msg{}; - msg.stamp = current_time; - msg.stop_speed_exceeded = stop_flag; - return msg; -} - VelocityLimit createVelocityLimitMsg( const rclcpp::Time & current_time, const double vel, const double acc, const double max_jerk, const double min_jerk) @@ -59,95 +50,6 @@ Float32MultiArrayStamped convertDebugValuesToMsg( } return debug_msg; } - -template -size_t getIndexWithLongitudinalOffset( - const T & points, const double longitudinal_offset, boost::optional start_idx) -{ - if (points.empty()) { - throw std::logic_error("points is empty."); - } - - if (start_idx) { - if (/*start_idx.get() < 0 || */ points.size() <= start_idx.get()) { - throw std::out_of_range("start_idx is out of range."); - } - } else { - if (longitudinal_offset > 0) { - start_idx = 0; - } else { - start_idx = points.size() - 1; - } - } - - double sum_length = 0.0; - if (longitudinal_offset > 0) { - for (size_t i = start_idx.get(); i < points.size() - 1; ++i) { - const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); - sum_length += segment_length; - if (sum_length >= longitudinal_offset) { - const double front_length = segment_length; - const double back_length = sum_length - longitudinal_offset; - if (front_length < back_length) { - return i; - } else { - return i + 1; - } - } - } - return points.size() - 1; - } - - for (size_t i = start_idx.get(); i > 0; --i) { - const double segment_length = - tier4_autoware_utils::calcDistance2d(points.at(i), points.at(i + 1)); - sum_length += segment_length; - if (sum_length >= -longitudinal_offset) { - const double front_length = segment_length; - const double back_length = sum_length + longitudinal_offset; - if (front_length < back_length) { - return i; - } else { - return i + 1; - } - } - } - return 0; -} - -double calcMinimumDistanceToStop(const double initial_vel, const double min_acc) -{ - return -std::pow(initial_vel, 2) / 2.0 / min_acc; -} - -tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( - const rclcpp::Time & current_time, const geometry_msgs::msg::Pose & stop_pose, - const boost::optional & stop_obstacle_info) -{ - // create header - std_msgs::msg::Header header; - header.frame_id = "map"; - header.stamp = current_time; - - // create stop factor - tier4_planning_msgs::msg::StopFactor stop_factor; - stop_factor.stop_pose = stop_pose; - if (stop_obstacle_info) { - stop_factor.stop_factor_points.emplace_back(stop_obstacle_info->obstacle.collision_point); - } - - // create stop reason stamped - tier4_planning_msgs::msg::StopReason stop_reason_msg; - stop_reason_msg.reason = tier4_planning_msgs::msg::StopReason::OBSTACLE_STOP; - stop_reason_msg.stop_factors.emplace_back(stop_factor); - - // create stop reason array - tier4_planning_msgs::msg::StopReasonArray stop_reason_array; - stop_reason_array.header = header; - stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); - return stop_reason_array; -} } // namespace PIDBasedPlanner::PIDBasedPlanner( @@ -181,31 +83,23 @@ PIDBasedPlanner::PIDBasedPlanner( lpf_cruise_ptr_ = std::make_shared(lpf_cruise_gain); // publisher - stop_reasons_pub_ = - node.create_publisher("~/output/stop_reasons", 1); - stop_speed_exceeded_pub_ = - node.create_publisher("~/output/stop_speed_exceeded", 1); debug_values_pub_ = node.create_publisher("~/debug/values", 1); } -Trajectory PIDBasedPlanner::generateTrajectory( - const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, - DebugData & debug_data) +Trajectory PIDBasedPlanner::generateCruiseTrajectory( + const ObstacleCruisePlannerData & planner_data, const Trajectory & stop_traj, + boost::optional & vel_limit, DebugData & debug_data) { stop_watch_.tic(__func__); debug_values_.resetValues(); - // calc obstacles to cruise and stop - boost::optional stop_obstacle_info; + // calc obstacles to cruise boost::optional cruise_obstacle_info; - calcObstaclesToCruiseAndStop(planner_data, stop_obstacle_info, cruise_obstacle_info); + calcObstaclesToCruise(planner_data, cruise_obstacle_info); // plan cruise planCruise(planner_data, vel_limit, cruise_obstacle_info, debug_data); - // plan stop - const auto output_traj = planStop(planner_data, stop_obstacle_info, debug_data); - // publish debug values publishDebugValues(planner_data); @@ -214,12 +108,11 @@ Trajectory PIDBasedPlanner::generateTrajectory( rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, " %s := %f [ms]", __func__, calculation_time); - return output_traj; + return stop_traj; } -void PIDBasedPlanner::calcObstaclesToCruiseAndStop( +void PIDBasedPlanner::calcObstaclesToCruise( const ObstacleCruisePlannerData & planner_data, - boost::optional & stop_obstacle_info, boost::optional & cruise_obstacle_info) { debug_values_.setValues(DebugValues::TYPE::CURRENT_VELOCITY, planner_data.current_vel); @@ -227,33 +120,14 @@ void PIDBasedPlanner::calcObstaclesToCruiseAndStop( auto modified_target_obstacles = planner_data.target_obstacles; - // search highest probability obstacle for stop and cruise + // search highest probability obstacle for cruise for (size_t o_idx = 0; o_idx < planner_data.target_obstacles.size(); ++o_idx) { const auto & obstacle = planner_data.target_obstacles.at(o_idx); // NOTE: from ego's front to obstacle's back const double dist_to_obstacle = calcDistanceToObstacle(planner_data, obstacle); - const bool is_stop_required = isStopRequired(obstacle, modified_target_obstacles.at(o_idx)); - if (is_stop_required) { // stop - // calculate error distance (= distance to stop) - const double error_dist = dist_to_obstacle - longitudinal_info_.safe_distance_margin; - if (stop_obstacle_info) { - if (error_dist > stop_obstacle_info->dist_to_stop) { - continue; - } - } - stop_obstacle_info = StopObstacleInfo(obstacle, error_dist); - - // update debug values - debug_values_.setValues(DebugValues::TYPE::STOP_CURRENT_OBJECT_DISTANCE, dist_to_obstacle); - debug_values_.setValues(DebugValues::TYPE::STOP_CURRENT_OBJECT_VELOCITY, obstacle.velocity); - debug_values_.setValues( - DebugValues::TYPE::STOP_TARGET_OBJECT_DISTANCE, longitudinal_info_.safe_distance_margin); - debug_values_.setValues( - DebugValues::TYPE::STOP_TARGET_ACCELERATION, longitudinal_info_.limit_min_accel); - debug_values_.setValues(DebugValues::TYPE::STOP_ERROR_OBJECT_DISTANCE, error_dist); - } else { // cruise + if (!obstacle.has_stopped) { // cruise // calculate distance between ego and obstacle based on RSS const double rss_dist = calcRSSDistance( planner_data.current_vel, obstacle.velocity, longitudinal_info_.safe_distance_margin); @@ -276,9 +150,6 @@ void PIDBasedPlanner::calcObstaclesToCruiseAndStop( debug_values_.setValues(DebugValues::TYPE::CRUISE_ERROR_OBJECT_DISTANCE, error_dist); } } - - // TODO(shimizu) refactor obstacle stop; - prev_target_obstacles_ = modified_target_obstacles; } double PIDBasedPlanner::calcDistanceToObstacle( @@ -296,119 +167,6 @@ double PIDBasedPlanner::calcDistanceToObstacle( offset; } -Trajectory PIDBasedPlanner::planStop( - const ObstacleCruisePlannerData & planner_data, - const boost::optional & stop_obstacle_info, DebugData & debug_data) -{ - bool will_collide_with_obstacle = false; - - size_t zero_vel_idx = 0; - bool zero_vel_found = false; - if (stop_obstacle_info) { - RCLCPP_INFO_EXPRESSION( - rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), is_showing_debug_info_, - "stop planning"); - - auto local_stop_obstacle_info = stop_obstacle_info.get(); - - // check if the ego will collide with the obstacle with a limit acceleration - const double feasible_dist_to_stop = - calcMinimumDistanceToStop(planner_data.current_vel, longitudinal_info_.limit_min_accel); - if (local_stop_obstacle_info.dist_to_stop < feasible_dist_to_stop) { - will_collide_with_obstacle = true; - local_stop_obstacle_info.dist_to_stop = feasible_dist_to_stop; - } - - // set zero velocity index - const auto opt_zero_vel_idx = doStop( - planner_data, local_stop_obstacle_info, debug_data.obstacles_to_stop, - debug_data.stop_wall_marker); - if (opt_zero_vel_idx) { - zero_vel_idx = opt_zero_vel_idx.get(); - zero_vel_found = true; - } - } - - // generate output trajectory - auto output_traj = planner_data.traj; - if (zero_vel_found) { - // publish stop reason - const auto stop_pose = planner_data.traj.points.at(zero_vel_idx).pose; - const auto stop_reasons_msg = - makeStopReasonArray(planner_data.current_time, stop_pose, stop_obstacle_info); - stop_reasons_pub_->publish(stop_reasons_msg); - - // insert zero_velocity - for (size_t traj_idx = zero_vel_idx; traj_idx < output_traj.points.size(); ++traj_idx) { - output_traj.points.at(traj_idx).longitudinal_velocity_mps = 0.0; - } - } - - // publish stop_speed_exceeded if the ego will collide with the obstacle - const auto stop_speed_exceeded_msg = - createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle); - stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); - - return output_traj; -} - -boost::optional PIDBasedPlanner::doStop( - const ObstacleCruisePlannerData & planner_data, const StopObstacleInfo & stop_obstacle_info, - std::vector & debug_obstacles_to_stop, - visualization_msgs::msg::MarkerArray & debug_wall_marker) const -{ - // const size_t ego_idx = findExtendedNearestIndex(planner_data.traj, planner_data.current_pose); - - // TODO(murooka) use interpolation to calculate stop point and marker pose - const auto modified_stop_info = [&]() -> boost::optional> { - // const double dist_to_stop = stop_obstacle_info.dist_to_stop; - - const size_t collision_idx = tier4_autoware_utils::findNearestIndex( - planner_data.traj.points, stop_obstacle_info.obstacle.collision_point); - const size_t obstacle_zero_vel_idx = getIndexWithLongitudinalOffset( - planner_data.traj.points, - -longitudinal_info_.safe_distance_margin - vehicle_info_.max_longitudinal_offset_m, - collision_idx); - - // check if there is already stop line between obstacle and zero_vel_idx - const auto behavior_zero_vel_idx = - tier4_autoware_utils::searchZeroVelocityIndex(planner_data.traj.points); - if (behavior_zero_vel_idx) { - const double zero_vel_diff_length = tier4_autoware_utils::calcSignedArcLength( - planner_data.traj.points, obstacle_zero_vel_idx, behavior_zero_vel_idx.get()); - std::cerr << zero_vel_diff_length << std::endl; - if ( - 0 < zero_vel_diff_length && - zero_vel_diff_length < longitudinal_info_.safe_distance_margin) { - const size_t modified_obstacle_zero_vel_idx = getIndexWithLongitudinalOffset( - planner_data.traj.points, - -min_behavior_stop_margin_ - vehicle_info_.max_longitudinal_offset_m, collision_idx); - const size_t modified_wall_idx = getIndexWithLongitudinalOffset( - planner_data.traj.points, -min_behavior_stop_margin_, collision_idx); - return std::make_pair(modified_obstacle_zero_vel_idx, modified_wall_idx); - } - } - - const size_t wall_idx = getIndexWithLongitudinalOffset( - planner_data.traj.points, -longitudinal_info_.safe_distance_margin, collision_idx); - return std::make_pair(obstacle_zero_vel_idx, wall_idx); - }(); - if (!modified_stop_info) { - return {}; - } - const size_t modified_zero_vel_idx = modified_stop_info->first; - const size_t modified_wall_idx = modified_stop_info->second; - - // virtual wall marker for stop - const auto marker_pose = planner_data.traj.points.at(modified_wall_idx).pose; - const auto markers = tier4_autoware_utils::createStopVirtualWallMarker( - marker_pose, "obstacle stop", planner_data.current_time, 0); - tier4_autoware_utils::appendMarkerArray(markers, &debug_wall_marker); - - debug_obstacles_to_stop.push_back(stop_obstacle_info.obstacle); - return modified_zero_vel_idx; -} - void PIDBasedPlanner::planCruise( const ObstacleCruisePlannerData & planner_data, boost::optional & vel_limit, const boost::optional & cruise_obstacle_info, DebugData & debug_data) @@ -483,8 +241,8 @@ VelocityLimit PIDBasedPlanner::doCruise( const double dist_to_rss_wall = std::min( dist_to_cruise + vehicle_info_.max_longitudinal_offset_m, dist_to_obstacle + vehicle_info_.max_longitudinal_offset_m); - const size_t wall_idx = - getIndexWithLongitudinalOffset(planner_data.traj.points, dist_to_rss_wall, ego_idx); + const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( + planner_data.traj.points, dist_to_rss_wall, ego_idx); const auto markers = tier4_autoware_utils::createSlowDownVirtualWallMarker( planner_data.traj.points.at(wall_idx).pose, "obstacle cruise", planner_data.current_time, 0); diff --git a/planning/obstacle_cruise_planner/src/planner_interface.cpp b/planning/obstacle_cruise_planner/src/planner_interface.cpp new file mode 100644 index 0000000000000..1f15e7c1efa08 --- /dev/null +++ b/planning/obstacle_cruise_planner/src/planner_interface.cpp @@ -0,0 +1,183 @@ +// Copyright 2022 TIER IV, Inc. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "obstacle_cruise_planner/planner_interface.hpp" + +namespace +{ +StopSpeedExceeded createStopSpeedExceededMsg( + const rclcpp::Time & current_time, const bool stop_flag) +{ + StopSpeedExceeded msg{}; + msg.stamp = current_time; + msg.stop_speed_exceeded = stop_flag; + return msg; +} + +tier4_planning_msgs::msg::StopReasonArray makeStopReasonArray( + const rclcpp::Time & current_time, const geometry_msgs::msg::Pose & stop_pose, + const TargetObstacle & stop_obstacle) +{ + // create header + std_msgs::msg::Header header; + header.frame_id = "map"; + header.stamp = current_time; + + // create stop factor + tier4_planning_msgs::msg::StopFactor stop_factor; + stop_factor.stop_pose = stop_pose; + stop_factor.stop_factor_points.emplace_back(stop_obstacle.collision_point); + + // create stop reason stamped + tier4_planning_msgs::msg::StopReason stop_reason_msg; + stop_reason_msg.reason = tier4_planning_msgs::msg::StopReason::OBSTACLE_STOP; + stop_reason_msg.stop_factors.emplace_back(stop_factor); + + // create stop reason array + tier4_planning_msgs::msg::StopReasonArray stop_reason_array; + stop_reason_array.header = header; + stop_reason_array.stop_reasons.emplace_back(stop_reason_msg); + return stop_reason_array; +} + +double calcMinimumDistanceToStop(const double initial_vel, const double min_acc) +{ + return -std::pow(initial_vel, 2) / 2.0 / min_acc; +} + +boost::optional getClosestStopObstacle( + const Trajectory & traj, const std::vector & target_obstacles) +{ + if (target_obstacles.empty()) { + return boost::none; + } + + boost::optional closest_stop_obstacle = boost::none; + double dist_to_closest_stop_obstacle = std::numeric_limits::max(); + for (const auto & obstacle : target_obstacles) { + // Ignore obstacle that has not stopped + if (!obstacle.has_stopped) { + continue; + } + + const double dist_to_stop_obstacle = + tier4_autoware_utils::calcSignedArcLength(traj.points, 0, obstacle.collision_point); + if (dist_to_stop_obstacle < dist_to_closest_stop_obstacle) { + dist_to_closest_stop_obstacle = dist_to_stop_obstacle; + closest_stop_obstacle = obstacle; + } + } + return closest_stop_obstacle; +} +} // namespace + +Trajectory PlannerInterface::generateStopTrajectory( + const ObstacleCruisePlannerData & planner_data, DebugData & debug_data) +{ + if (planner_data.target_obstacles.empty()) { + return planner_data.traj; + } + + // Get Closest Stop Obstacle + const auto closest_stop_obstacle = + getClosestStopObstacle(planner_data.traj, planner_data.target_obstacles); + if (!closest_stop_obstacle) { + return planner_data.traj; + } + + // Get Closest Obstacle Stop Distance + const double closest_obstacle_dist = tier4_autoware_utils::calcSignedArcLength( + planner_data.traj.points, 0, closest_stop_obstacle->collision_point); + + const auto negative_dist_to_ego = tier4_autoware_utils::calcSignedArcLength( + planner_data.traj.points, planner_data.current_pose, 0, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); + if (!negative_dist_to_ego) { + return planner_data.traj; + } + const double dist_to_ego = -negative_dist_to_ego.get(); + + // If behavior stop point is ahead of the closest_obstacle_stop point within a certain margin + // we set closest_obstacle_stop_distance to closest_behavior_stop_distance + const double margin_from_obstacle = [&]() { + const auto closest_behavior_stop_dist_from_ego = + tier4_autoware_utils::calcDistanceToForwardStopPoint( + planner_data.traj.points, planner_data.current_pose, nearest_dist_deviation_threshold_, + nearest_yaw_deviation_threshold_); + + if (closest_behavior_stop_dist_from_ego) { + const double closest_obstacle_stop_dist_from_ego = closest_obstacle_dist - dist_to_ego - + longitudinal_info_.safe_distance_margin - + vehicle_info_.max_longitudinal_offset_m; + + const double stop_dist_diff = + *closest_behavior_stop_dist_from_ego - closest_obstacle_stop_dist_from_ego; + if (0.0 < stop_dist_diff && stop_dist_diff < longitudinal_info_.safe_distance_margin) { + // Use shorter margin (min_behavior_stop_margin) for obstacle stop + return min_behavior_stop_margin_; + } + } + return longitudinal_info_.safe_distance_margin; + }(); + + // Calculate feasible stop margin (Check the feasibility) + const double feasible_stop_dist = + calcMinimumDistanceToStop(planner_data.current_vel, longitudinal_info_.limit_min_accel) + + dist_to_ego; + const double closest_obstacle_stop_dist = + closest_obstacle_dist - margin_from_obstacle - vehicle_info_.max_longitudinal_offset_m; + + bool will_collide_with_obstacle = false; + double feasible_margin_from_obstacle = margin_from_obstacle; + if (closest_obstacle_stop_dist < feasible_stop_dist) { + feasible_margin_from_obstacle = + margin_from_obstacle - (feasible_stop_dist - closest_obstacle_stop_dist); + will_collide_with_obstacle = true; + } + + const size_t collision_idx = tier4_autoware_utils::findNearestIndex( + planner_data.traj.points, closest_stop_obstacle->collision_point); + const size_t zero_vel_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( + planner_data.traj.points, + -vehicle_info_.max_longitudinal_offset_m - feasible_margin_from_obstacle, collision_idx); + const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( + planner_data.traj.points, -feasible_margin_from_obstacle, collision_idx); + + // TODO(shimizu) insert stop point with interpolation + // Generate Output Trajectory + auto output_traj = planner_data.traj; + for (size_t o_idx = zero_vel_idx; o_idx < output_traj.points.size(); ++o_idx) { + output_traj.points.at(o_idx).longitudinal_velocity_mps = 0.0; + } + + // virtual wall marker for stop obstacle + const auto marker_pose = planner_data.traj.points.at(wall_idx).pose; + const auto markers = tier4_autoware_utils::createStopVirtualWallMarker( + marker_pose, "obstacle stop", planner_data.current_time, 0); + tier4_autoware_utils::appendMarkerArray(markers, &debug_data.stop_wall_marker); + debug_data.obstacles_to_stop.push_back(*closest_stop_obstacle); + + // Publish Stop Reason + const auto stop_pose = output_traj.points.at(zero_vel_idx).pose; + const auto stop_reasons_msg = + makeStopReasonArray(planner_data.current_time, stop_pose, *closest_stop_obstacle); + stop_reasons_pub_->publish(stop_reasons_msg); + + // Publish if ego vehicle collides with the obstacle with a limit acceleration + const auto stop_speed_exceeded_msg = + createStopSpeedExceededMsg(planner_data.current_time, will_collide_with_obstacle); + stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); + + return output_traj; +} diff --git a/planning/obstacle_cruise_planner/src/utils.cpp b/planning/obstacle_cruise_planner/src/utils.cpp index 8e571077d2ec9..a7dff0388bb7b 100644 --- a/planning/obstacle_cruise_planner/src/utils.cpp +++ b/planning/obstacle_cruise_planner/src/utils.cpp @@ -14,8 +14,6 @@ #include "obstacle_cruise_planner/utils.hpp" -#include "tier4_autoware_utils/tier4_autoware_utils.hpp" - namespace obstacle_cruise_utils { bool isVehicle(const uint8_t label) @@ -41,18 +39,18 @@ visualization_msgs::msg::Marker getObjectMarker( } boost::optional calcForwardPose( - const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t nearest_idx, + const autoware_auto_planning_msgs::msg::Trajectory & traj, const size_t start_idx, const double target_length) { if (traj.points.empty()) { return {}; } - size_t search_idx = nearest_idx; + size_t search_idx = start_idx; double length_to_search_idx = 0.0; for (; search_idx < traj.points.size(); ++search_idx) { length_to_search_idx = - tier4_autoware_utils::calcSignedArcLength(traj.points, nearest_idx, search_idx); + tier4_autoware_utils::calcSignedArcLength(traj.points, start_idx, search_idx); if (length_to_search_idx > target_length) { break; } else if (search_idx == traj.points.size() - 1) { @@ -73,40 +71,8 @@ boost::optional calcForwardPose( const double seg_length = tier4_autoware_utils::calcDistance2d(pre_pose.position, next_pose.position); const double lerp_ratio = (length_to_search_idx - target_length) / seg_length; - target_pose.position.x = - pre_pose.position.x + (next_pose.position.x - pre_pose.position.x) * lerp_ratio; - target_pose.position.y = - pre_pose.position.y + (next_pose.position.y - pre_pose.position.y) * lerp_ratio; - target_pose.position.z = - pre_pose.position.z + (next_pose.position.z - pre_pose.position.z) * lerp_ratio; - - // lerp orientation - const double pre_yaw = tf2::getYaw(pre_pose.orientation); - const double next_yaw = tf2::getYaw(next_pose.orientation); - target_pose.orientation = - tier4_autoware_utils::createQuaternionFromYaw(pre_yaw + (next_yaw - pre_yaw) * lerp_ratio); - - return target_pose; -} -geometry_msgs::msg::Pose lerpByPose( - const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const double t) -{ - tf2::Transform tf_transform1, tf_transform2; - tf2::fromMsg(p1, tf_transform1); - tf2::fromMsg(p2, tf_transform2); - const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t); - const auto & tf_quaternion = - tf2::slerp(tf_transform1.getRotation(), tf_transform2.getRotation(), t); - - geometry_msgs::msg::Pose pose; - { - pose.position.x = tf_point.x(); - pose.position.y = tf_point.y(); - pose.position.z = tf_point.z(); - } - pose.orientation = tf2::toMsg(tf_quaternion); - return pose; + return tier4_autoware_utils::calcInterpolatedPose(pre_pose, next_pose, lerp_ratio); } boost::optional lerpByTimeStamp( @@ -125,7 +91,7 @@ boost::optional lerpByTimeStamp( if (rel_time <= rclcpp::Duration(path.time_step) * static_cast(i)) { const auto offset = rel_time - rclcpp::Duration(path.time_step) * static_cast(i - 1); const auto ratio = offset.seconds() / rclcpp::Duration(path.time_step).seconds(); - return lerpByPose(prev_pt, pt, ratio); + return tier4_autoware_utils::calcInterpolatedPose(prev_pt, pt, ratio); } } @@ -186,4 +152,67 @@ geometry_msgs::msg::Pose getCurrentObjectPose( return interpolated_pose.get(); } + +autoware_auto_planning_msgs::msg::Trajectory insertStopPoint( + const autoware_auto_planning_msgs::msg::Trajectory & trajectory, + const double distance_to_stop_point) +{ + if (trajectory.points.size() < 2) { + return trajectory; + } + + const double traj_length = tier4_autoware_utils::calcArcLength(trajectory.points); + if (traj_length < distance_to_stop_point) { + return trajectory; + } + + autoware_auto_planning_msgs::msg::Trajectory output; + output.header = trajectory.header; + + double accumulated_length = 0; + size_t insert_idx = trajectory.points.size(); + for (size_t i = 0; i < trajectory.points.size() - 1; ++i) { + const auto curr_traj_point = trajectory.points.at(i); + const auto next_traj_point = trajectory.points.at(i + 1); + const auto curr_pose = curr_traj_point.pose; + const auto next_pose = next_traj_point.pose; + const double segment_length = tier4_autoware_utils::calcDistance2d(curr_pose, next_pose); + accumulated_length += segment_length; + + if (accumulated_length > distance_to_stop_point) { + const double ratio = 1 - (accumulated_length - distance_to_stop_point) / segment_length; + + autoware_auto_planning_msgs::msg::TrajectoryPoint stop_point; + stop_point.pose = tier4_autoware_utils::calcInterpolatedPose(curr_pose, next_pose, ratio); + stop_point.lateral_velocity_mps = 0.0; + const double front_dist = tier4_autoware_utils::calcDistance2d(curr_pose, stop_point.pose); + const double back_dist = tier4_autoware_utils::calcDistance2d(stop_point.pose, next_pose); + if (front_dist < 1e-3) { + auto traj_point = trajectory.points.at(i); + traj_point.longitudinal_velocity_mps = 0.0; + output.points.push_back(traj_point); + } else if (back_dist < 1e-3) { + output.points.push_back(curr_traj_point); + } else { + output.points.push_back(curr_traj_point); + output.points.push_back(stop_point); + } + insert_idx = i + 1; + break; + } + + output.points.push_back(curr_traj_point); + } + + for (size_t i = insert_idx; i < trajectory.points.size() - 1; ++i) { + auto traj_point = trajectory.points.at(i); + traj_point.longitudinal_velocity_mps = 0.0; + output.points.push_back(traj_point); + } + + // Terminal Velocity Should be zero + output.points.back().longitudinal_velocity_mps = 0.0; + + return output; +} } // namespace obstacle_cruise_utils