From 6d1ef8b0d9d0f670bd3bc642b7af06d68aef4f63 Mon Sep 17 00:00:00 2001 From: Takayuki Murooka Date: Tue, 28 Jan 2025 02:47:33 +0900 Subject: [PATCH] revert obstacle_cruise_planner Signed-off-by: Takayuki Murooka --- .../CMakeLists.txt | 8 +- .../README.md | 65 - .../config/obstacle_cruise_planner.param.yaml | 221 ++ .../common_structs.hpp | 343 +++ .../autoware/obstacle_cruise_planner/node.hpp | 249 +- .../optimization_based_planner.hpp | 122 + .../optimization_based_planner/s_boundary.hpp | 33 + .../velocity_optimizer.hpp | 75 + .../cruise_planning_debug_info.hpp | 97 + .../pid_based_planner/pid_based_planner.hpp | 142 ++ .../pid_based_planner/pid_controller.hpp | 62 + .../planner_interface.hpp | 448 ++++ .../obstacle_cruise_planner/polygon_utils.hpp | 60 + .../stop_planning_debug_info.hpp | 88 + .../obstacle_cruise_planner/type_alias.hpp | 65 + .../obstacle_cruise_planner/utils.hpp | 99 + .../package.xml | 3 +- .../src/node.cpp | 2210 ++++++++++++++++- .../optimization_based_planner.cpp | 754 ++++++ .../velocity_optimizer.cpp | 284 +++ .../pid_based_planner/pid_based_planner.cpp | 724 ++++++ .../src/planner_interface.cpp | 865 +++++++ .../src/polygon_utils.cpp | 201 ++ .../src/utils.cpp | 121 + .../test_obstacle_cruise_planner_utils.cpp | 32 +- 25 files changed, 7227 insertions(+), 144 deletions(-) create mode 100644 planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp create mode 100644 planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp create mode 100644 planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp create mode 100644 planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp create mode 100644 planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp create mode 100644 planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp create mode 100644 planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp create mode 100644 planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp create mode 100644 planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp create mode 100644 planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp create mode 100644 planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp create mode 100644 planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp create mode 100644 planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp create mode 100644 planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp create mode 100644 planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp create mode 100644 planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp create mode 100644 planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp create mode 100644 planning/autoware_obstacle_cruise_planner/src/utils.cpp diff --git a/planning/autoware_obstacle_cruise_planner/CMakeLists.txt b/planning/autoware_obstacle_cruise_planner/CMakeLists.txt index 38815d4615650..7548536be8c47 100644 --- a/planning/autoware_obstacle_cruise_planner/CMakeLists.txt +++ b/planning/autoware_obstacle_cruise_planner/CMakeLists.txt @@ -7,7 +7,13 @@ autoware_package() find_package(Eigen3 REQUIRED) ament_auto_add_library(autoware_obstacle_cruise_planner_core SHARED - DIRECTORY src + src/node.cpp + src/utils.cpp + src/polygon_utils.cpp + 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(autoware_obstacle_cruise_planner_core diff --git a/planning/autoware_obstacle_cruise_planner/README.md b/planning/autoware_obstacle_cruise_planner/README.md index 518788aab064b..273363489d56f 100644 --- a/planning/autoware_obstacle_cruise_planner/README.md +++ b/planning/autoware_obstacle_cruise_planner/README.md @@ -68,71 +68,6 @@ struct Obstacle }; ``` -```plantuml -@startuml -skinparam monochrome true - -title Overall flowchart -start - -partition filter_stop_obstacle_for_predicted_object { -:is_stop_obstacle_type; - -partition filter_inside_stop_obstacle_for_predicted_object { -:filter by lateral distance; - -:filter by label; - -:is_stop_obstacle_velocity; - -:check if the obstacle really collides with the trajectory; - -:is_crossing_transient_obstacle; -} - -partition filter_outside_stop_obstacle_for_predicted_object { -} - -:checkConsistency; -note right -consistent -end note -} - -partition filter_stop_obstacle_for_point_cloud { -} - -:concat stop obstacles; -note right -concat stop obstacles by predicted objects and point cloud -end note - -partition generate_stop_trajectory { -:calculate dist to collide; - -:calc_desired_stop_margin; -note right -- whether the ego is cloes to the goal -- whether the ego is on the curve -- whether there is stop point by behavior's stop point -end note - -:calc_candidate_zero_vel_dist; - -:hold_previous_stop_if_necessary; - -:insert_stop_point; - -:set_stop_planning_debug_info; -} - -:publishDebugInfo; - -stop - -@enduml -``` - ### Behavior determination against obstacles Obstacles for cruising, stopping and slowing down are selected in this order based on their pose and velocity. diff --git a/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml b/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml index 8ae491339263c..75adf228cc7ca 100644 --- a/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml +++ b/planning/autoware_obstacle_cruise_planner/config/obstacle_cruise_planner.param.yaml @@ -1,10 +1,86 @@ /**: ros__parameters: common: + planning_algorithm: "pid_base" # currently supported algorithm is "pid_base" + enable_debug_info: false + enable_calculation_time_info: false + + enable_slow_down_planning: true + + # longitudinal info + idling_time: 2.0 # 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 : 5.0 # This is also used as a stop margin [m] + terminal_safe_distance_margin : 3.0 # Stop margin at the goal. This value cannot exceed safe distance margin. [m] + hold_stop_velocity_threshold: 0.01 # The maximum ego velocity to hold stopping [m/s] + hold_stop_distance_threshold: 0.3 # The ego keeps stopping if the distance to stop changes within the threshold [m] + slow_down_min_acc: -1.0 # slow down min deceleration [m/ss] + slow_down_min_jerk: -1.0 # slow down min jerk [m/sss] nearest_dist_deviation_threshold: 3.0 # [m] for finding nearest index nearest_yaw_deviation_threshold: 1.57 # [rad] for finding nearest index + min_behavior_stop_margin: 3.0 # [m] + stop_on_curve: + enable_approaching: false # false + additional_safe_distance_margin: 3.0 # [m] + min_safe_distance_margin: 3.0 # [m] + suppress_sudden_obstacle_stop: false + + stop_obstacle_type: + pointcloud: false + inside: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + + outside: + unknown: false + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + + cruise_obstacle_type: + inside: + unknown: true + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: false + + outside: + unknown: false + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: false + pedestrian: false + + slow_down_obstacle_type: + unknown: false + car: true + truck: true + bus: true + trailer: true + motorcycle: true + bicycle: true + pedestrian: true + pointcloud: false behavior_determination: pointcloud_search_radius: 5.0 @@ -20,9 +96,154 @@ prediction_resampling_time_interval: 0.1 prediction_resampling_time_horizon: 10.0 + stop_obstacle_hold_time_threshold : 1.0 # maximum time for holding closest stop obstacle + + # hysteresis for cruise and stop + 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] + + # if crossing vehicle is determined as target obstacles or not + crossing_obstacle: + obstacle_velocity_threshold : 3.0 # velocity threshold for crossing obstacle for cruise or stop [m/s] + obstacle_traj_angle_threshold : 1.22 # [rad] = 70 [deg], yaw threshold of crossing obstacle against the nearest trajectory point for cruise or stop + + stop: + max_lat_margin: 0.3 # lateral margin between the obstacles except for unknown and ego's footprint + max_lat_margin_against_unknown: 0.3 # lateral margin between the unknown obstacles and ego's footprint + min_velocity_to_reach_collision_point: 2.0 # minimum velocity margin to calculate time to reach collision [m/s] + outside_obstacle: + max_lateral_time_margin: 4.0 # time threshold of lateral margin between the obstacles and ego's footprint [s] + num_of_predicted_paths: 3 # number of the highest confidence predicted path to check collision between obstacle and ego + pedestrian_deceleration_rate: 0.5 # planner perceives pedestrians will stop with this rate to avoid unnecessary stops [m/ss] + bicycle_deceleration_rate: 0.5 # planner perceives bicycles will stop with this rate to avoid unnecessary stops [m/ss] + crossing_obstacle: + collision_time_margin : 4.0 # time threshold of collision between obstacle adn ego for cruise or stop [s] + + cruise: + max_lat_margin: 1.0 # lateral margin between obstacle and trajectory band with ego's width + outside_obstacle: + obstacle_velocity_threshold : 3.5 # minimum velocity threshold of obstacles outside the trajectory to cruise or stop [m/s] + ego_obstacle_overlap_time_threshold : 2.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 + max_lateral_time_margin: 5.0 # time threshold of lateral margin between obstacle and trajectory band with ego's width [s] + num_of_predicted_paths: 3 # number of the highest confidence predicted path to check collision between obstacle and ego + yield: + enable_yield: true + lat_distance_threshold: 5.0 # lateral margin between obstacle in neighbor lanes and trajectory band with ego's width for yielding + max_lat_dist_between_obstacles: 2.5 # lateral margin between moving obstacle in neighbor lanes and stopped obstacle in front of it + max_obstacles_collision_time: 10.0 # how far the blocking obstacle + stopped_obstacle_velocity_threshold: 0.5 + slow_down: + max_lat_margin: 1.1 # lateral margin between obstacle and trajectory band with ego's width + lat_hysteresis_margin: 0.2 + + successive_num_to_entry_slow_down_condition: 5 + successive_num_to_exit_slow_down_condition: 5 + # consider the current ego pose (it is not the nearest pose on the reference trajectory) # Both the lateral error and the yaw error are assumed to decrease to zero by the time duration "time_to_convergence" # The both errors decrease with constant rates against the time. consider_current_pose: enable_to_consider_current_pose: true time_to_convergence: 1.5 #[s] + + cruise: + pid_based_planner: + use_velocity_limit_based_planner: true + error_function_type: quadratic # choose from linear, quadratic + + velocity_limit_based_planner: + # PID gains to keep safe distance with the front vehicle + kp: 10.0 + ki: 0.0 + kd: 2.0 + + 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: 12.0 # target acceleration is calculated by (target_velocity - current_velocity) * vel_to_acc_weight [-] + + enable_jerk_limit_to_output_acc: false + + disable_target_acceleration: true + + velocity_insertion_based_planner: + kp_acc: 6.0 + ki_acc: 0.0 + kd_acc: 2.0 + + kp_jerk: 20.0 + ki_jerk: 0.0 + kd_jerk: 0.0 + + output_acc_ratio_during_accel: 0.6 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + output_jerk_ratio_during_accel: 1.0 # target acceleration is multiplied with this value while ego accelerates to catch up the front vehicle [-] + + enable_jerk_limit_to_output_acc: true + + min_accel_during_cruise: -2.0 # minimum acceleration during cruise to slow down [m/ss] + min_cruise_target_vel: 0.0 # minimum target velocity during slow down [m/s] + time_to_evaluate_rss: 0.0 + + lpf_normalized_error_cruise_dist_gain: 0.2 + + optimization_based_planner: + dense_resampling_time_interval: 0.2 + sparse_resampling_time_interval: 2.0 + dense_time_horizon: 5.0 + max_time_horizon: 25.0 + velocity_margin: 0.2 #[m/s] + + # Parameters for safe distance + t_dangerous: 0.5 + + # For initial Motion + 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. + stop_dist_to_prohibit_engage: 0.5 # if the stop point is in this distance, the speed is set to 0 not to move the vehicle [m] + + # Weights for optimization + max_s_weight: 100.0 + max_v_weight: 1.0 + over_s_safety_weight: 1000000.0 + over_s_ideal_weight: 50.0 + over_v_weight: 500000.0 + over_a_weight: 5000.0 + over_j_weight: 10000.0 + + slow_down: + # parameters to calculate slow down velocity by linear interpolation + labels: + - "default" + default: + moving: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 2.0 + max_ego_velocity: 8.0 + static: + min_lat_margin: 0.2 + max_lat_margin: 1.0 + min_ego_velocity: 4.0 + max_ego_velocity: 8.0 + + moving_object_speed_threshold: 0.5 # [m/s] how fast the object needs to move to be considered as "moving" + moving_object_hysteresis_range: 0.1 # [m/s] hysteresis range used to prevent chattering when obstacle moves close to moving_object_speed_threshold + + time_margin_on_target_velocity: 1.5 # [s] + + lpf_gain_slow_down_vel: 0.99 # low-pass filter gain for slow down velocity + lpf_gain_lat_dist: 0.999 # low-pass filter gain for lateral distance from obstacle to ego's path + lpf_gain_dist_to_slow_down: 0.7 # low-pass filter gain for distance to slow down start + stop: + type_specified_params: + labels: # For the listed types, the node try to read the following type specified values + - "default" + - "unknown" + # default: For the default type, which denotes the other types listed above, the following param is defined implicitly, and the other type-specified parameters are not defined. + # limit_min_acc: common_param.yaml/limit.min_acc + unknown: + limit_min_acc: -1.2 # overwrite the deceleration limit, in usually, common_param.yaml/limit.min_acc is referred. + sudden_object_acc_threshold: -1.1 # If a stop can be achieved by a deceleration smaller than this value, it is not considered as “sudden stop". + sudden_object_dist_threshold: 1000.0 # If a stop distance is longer than this value, it is not considered as “sudden stop". + abandon_to_stop: false # If true, the planner gives up to stop when it cannot avoid to run over while maintaining the deceleration limit. diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp new file mode 100644 index 0000000000000..8abff415e47df --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/common_structs.hpp @@ -0,0 +1,343 @@ +// 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. + +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ + +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/ros/uuid_helper.hpp" + +#include + +#include +#include + +#include +#include +#include + +using Metric = tier4_metric_msgs::msg::Metric; +using MetricArray = tier4_metric_msgs::msg::MetricArray; + +struct PlannerData +{ + rclcpp::Time current_time; + std::vector traj_points; + geometry_msgs::msg::Pose ego_pose; + double ego_vel; + double ego_acc; + bool is_driving_forward; +}; + +struct PoseWithStamp +{ + rclcpp::Time stamp; + geometry_msgs::msg::Pose pose; +}; + +struct PointWithStamp +{ + rclcpp::Time stamp; + geometry_msgs::msg::Point point; +}; + +struct Obstacle +{ + Obstacle( + const rclcpp::Time & arg_stamp, const PredictedObject & object, + const geometry_msgs::msg::Pose & arg_pose, const double ego_to_obstacle_distance, + const double lat_dist_from_obstacle_to_traj, const double longitudinal_velocity, + const double approach_velocity) + : stamp(arg_stamp), + ego_to_obstacle_distance(ego_to_obstacle_distance), + lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), + longitudinal_velocity(longitudinal_velocity), + approach_velocity(approach_velocity), + pose(arg_pose), + orientation_reliable(true), + twist(object.kinematics.initial_twist_with_covariance.twist), + twist_reliable(true), + classification(object.classification.at(0)), + uuid(autoware::universe_utils::toHexString(object.object_id)), + shape(object.shape) + { + predicted_paths.clear(); + for (const auto & path : object.kinematics.predicted_paths) { + predicted_paths.push_back(path); + } + } + + Obstacle( + const rclcpp::Time & arg_stamp, + const std::optional & stop_collision_point, + const std::optional & slow_down_front_collision_point, + const std::optional & slow_down_back_collision_point, + const double ego_to_obstacle_distance, const double lat_dist_from_obstacle_to_traj) + : stamp(arg_stamp), + ego_to_obstacle_distance(ego_to_obstacle_distance), + lat_dist_from_obstacle_to_traj(lat_dist_from_obstacle_to_traj), + stop_collision_point(stop_collision_point), + slow_down_front_collision_point(slow_down_front_collision_point), + slow_down_back_collision_point(slow_down_back_collision_point) + { + } + + rclcpp::Time stamp; // This is not the current stamp, but when the object was observed. + double ego_to_obstacle_distance; + double lat_dist_from_obstacle_to_traj; + double longitudinal_velocity; + double approach_velocity; + + // for PredictedObject + geometry_msgs::msg::Pose pose; // interpolated with the current stamp + bool orientation_reliable; + Twist twist; + bool twist_reliable; + ObjectClassification classification; + std::string uuid; + Shape shape; + std::vector predicted_paths; + + // for PointCloud + std::optional stop_collision_point; + std::optional slow_down_front_collision_point; + std::optional slow_down_back_collision_point; +}; + +struct TargetObstacleInterface +{ + TargetObstacleInterface( + const std::string & arg_uuid, const rclcpp::Time & arg_stamp, + const geometry_msgs::msg::Pose & arg_pose, const double arg_velocity, + const double arg_lat_velocity) + : uuid(arg_uuid), + stamp(arg_stamp), + pose(arg_pose), + velocity(arg_velocity), + lat_velocity(arg_lat_velocity) + { + } + std::string uuid; + rclcpp::Time stamp; + geometry_msgs::msg::Pose pose; // interpolated with the current stamp + double velocity; // longitudinal velocity against ego's trajectory + double lat_velocity; // lateral velocity against ego's trajectory +}; + +struct StopObstacle : public TargetObstacleInterface +{ + StopObstacle( + const std::string & arg_uuid, const rclcpp::Time & arg_stamp, + const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose, + const Shape & arg_shape, const double arg_lon_velocity, const double arg_lat_velocity, + const geometry_msgs::msg::Point arg_collision_point, + const double arg_dist_to_collide_on_decimated_traj) + : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), + shape(arg_shape), + collision_point(arg_collision_point), + dist_to_collide_on_decimated_traj(arg_dist_to_collide_on_decimated_traj), + classification(object_classification) + { + } + Shape shape; + geometry_msgs::msg::Point + collision_point; // TODO(yuki_takagi): this member variable still used in + // calculateMarginFromObstacleOnCurve() and should be removed as it can be + // replaced by ”dist_to_collide_on_decimated_traj” + double dist_to_collide_on_decimated_traj; + ObjectClassification classification; +}; + +struct CruiseObstacle : public TargetObstacleInterface +{ + CruiseObstacle( + const std::string & arg_uuid, const rclcpp::Time & arg_stamp, + const geometry_msgs::msg::Pose & arg_pose, const double arg_lon_velocity, + const double arg_lat_velocity, const std::vector & arg_collision_points, + bool arg_is_yield_obstacle = false) + : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), + collision_points(arg_collision_points), + is_yield_obstacle(arg_is_yield_obstacle) + { + } + std::vector collision_points; // time-series collision points + bool is_yield_obstacle; +}; + +struct SlowDownObstacle : public TargetObstacleInterface +{ + SlowDownObstacle( + const std::string & arg_uuid, const rclcpp::Time & arg_stamp, + const ObjectClassification & object_classification, const geometry_msgs::msg::Pose & arg_pose, + const double arg_lon_velocity, const double arg_lat_velocity, const double arg_precise_lat_dist, + const geometry_msgs::msg::Point & arg_front_collision_point, + const geometry_msgs::msg::Point & arg_back_collision_point) + : TargetObstacleInterface(arg_uuid, arg_stamp, arg_pose, arg_lon_velocity, arg_lat_velocity), + precise_lat_dist(arg_precise_lat_dist), + front_collision_point(arg_front_collision_point), + back_collision_point(arg_back_collision_point), + classification(object_classification) + { + } + double precise_lat_dist; // for efficient calculation + geometry_msgs::msg::Point front_collision_point; + geometry_msgs::msg::Point back_collision_point; + ObjectClassification classification; +}; + +struct LongitudinalInfo +{ + explicit LongitudinalInfo(rclcpp::Node & node) + { + max_accel = node.declare_parameter("normal.max_acc"); + min_accel = node.declare_parameter("normal.min_acc"); + max_jerk = node.declare_parameter("normal.max_jerk"); + min_jerk = node.declare_parameter("normal.min_jerk"); + limit_max_accel = node.declare_parameter("limit.max_acc"); + limit_min_accel = node.declare_parameter("limit.min_acc"); + limit_max_jerk = node.declare_parameter("limit.max_jerk"); + limit_min_jerk = node.declare_parameter("limit.min_jerk"); + slow_down_min_accel = node.declare_parameter("common.slow_down_min_acc"); + slow_down_min_jerk = node.declare_parameter("common.slow_down_min_jerk"); + + idling_time = node.declare_parameter("common.idling_time"); + min_ego_accel_for_rss = node.declare_parameter("common.min_ego_accel_for_rss"); + min_object_accel_for_rss = node.declare_parameter("common.min_object_accel_for_rss"); + + safe_distance_margin = node.declare_parameter("common.safe_distance_margin"); + terminal_safe_distance_margin = + node.declare_parameter("common.terminal_safe_distance_margin"); + + hold_stop_velocity_threshold = + node.declare_parameter("common.hold_stop_velocity_threshold"); + hold_stop_distance_threshold = + node.declare_parameter("common.hold_stop_distance_threshold"); + } + + void onParam(const std::vector & parameters) + { + autoware::universe_utils::updateParam(parameters, "normal.max_accel", max_accel); + autoware::universe_utils::updateParam(parameters, "normal.min_accel", min_accel); + autoware::universe_utils::updateParam(parameters, "normal.max_jerk", max_jerk); + autoware::universe_utils::updateParam(parameters, "normal.min_jerk", min_jerk); + autoware::universe_utils::updateParam(parameters, "limit.max_accel", limit_max_accel); + autoware::universe_utils::updateParam(parameters, "limit.min_accel", limit_min_accel); + autoware::universe_utils::updateParam(parameters, "limit.max_jerk", limit_max_jerk); + autoware::universe_utils::updateParam(parameters, "limit.min_jerk", limit_min_jerk); + autoware::universe_utils::updateParam( + parameters, "common.slow_down_min_accel", slow_down_min_accel); + autoware::universe_utils::updateParam( + parameters, "common.slow_down_min_jerk", slow_down_min_jerk); + + autoware::universe_utils::updateParam(parameters, "common.idling_time", idling_time); + autoware::universe_utils::updateParam( + parameters, "common.min_ego_accel_for_rss", min_ego_accel_for_rss); + autoware::universe_utils::updateParam( + parameters, "common.min_object_accel_for_rss", min_object_accel_for_rss); + + autoware::universe_utils::updateParam( + parameters, "common.safe_distance_margin", safe_distance_margin); + autoware::universe_utils::updateParam( + parameters, "common.terminal_safe_distance_margin", terminal_safe_distance_margin); + + autoware::universe_utils::updateParam( + parameters, "common.hold_stop_velocity_threshold", hold_stop_velocity_threshold); + autoware::universe_utils::updateParam( + parameters, "common.hold_stop_distance_threshold", hold_stop_distance_threshold); + } + + // common parameter + double max_accel; + double min_accel; + double max_jerk; + double min_jerk; + double slow_down_min_jerk; + double slow_down_min_accel; + double limit_max_accel; + double limit_min_accel; + double limit_max_jerk; + double limit_min_jerk; + + // rss parameter + double idling_time; + double min_ego_accel_for_rss; + double min_object_accel_for_rss; + + // distance margin + double safe_distance_margin; + double terminal_safe_distance_margin; + + // hold stop + double hold_stop_velocity_threshold; + double hold_stop_distance_threshold; +}; + +struct DebugData +{ + DebugData() = default; + std::vector intentionally_ignored_obstacles; + std::vector obstacles_to_stop; + std::vector obstacles_to_cruise; + std::vector obstacles_to_slow_down; + MarkerArray slow_down_debug_wall_marker; + MarkerArray stop_wall_marker; + MarkerArray cruise_wall_marker; + MarkerArray slow_down_wall_marker; + std::vector detection_polygons; + std::optional> stop_metrics{std::nullopt}; + std::optional> slow_down_metrics{std::nullopt}; + std::optional> cruise_metrics{std::nullopt}; +}; + +struct EgoNearestParam +{ + EgoNearestParam() = default; + explicit EgoNearestParam(rclcpp::Node & node) + { + dist_threshold = node.declare_parameter("ego_nearest_dist_threshold"); + yaw_threshold = node.declare_parameter("ego_nearest_yaw_threshold"); + } + + TrajectoryPoint calcInterpolatedPoint( + const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const + { + return autoware::motion_utils::calcInterpolatedPoint( + autoware::motion_utils::convertToTrajectory(traj_points), pose, dist_threshold, + yaw_threshold); + } + + size_t findIndex( + const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const + { + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_points, pose, dist_threshold, yaw_threshold); + } + + size_t findSegmentIndex( + const std::vector & traj_points, const geometry_msgs::msg::Pose & pose) const + { + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj_points, pose, dist_threshold, yaw_threshold); + } + + double dist_threshold; + double yaw_threshold; +}; + +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__COMMON_STRUCTS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp index ad0d0240d3c9b..6a106776e48b5 100644 --- a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/node.hpp @@ -16,15 +16,15 @@ #define AUTOWARE__OBSTACLE_CRUISE_PLANNER__NODE_HPP_ #include "autoware/obstacle_cruise_planner/common_structs.hpp" -#include "autoware/obstacle_cruise_planner/cruise/obstacle_cruise_module.hpp" -#include "autoware/obstacle_cruise_planner/slow_down/obstacle_slow_down_module.hpp" -#include "autoware/obstacle_cruise_planner/stop/obstacle_stop_module.hpp" -#include "autoware/obstacle_cruise_planner/stop/type_alias.hpp" +#include "autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" +#include "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" +#include "autoware/obstacle_cruise_planner/type_alias.hpp" #include "autoware/signal_processing/lowpass_filter_1d.hpp" #include "autoware/universe_utils/ros/logger_level_configure.hpp" #include "autoware/universe_utils/ros/polling_subscriber.hpp" #include "autoware/universe_utils/system/stop_watch.hpp" +#include #include #include @@ -56,26 +56,120 @@ class ObstacleCruisePlannerNode : public rclcpp::Node void onSmoothedTrajectory(const Trajectory::ConstSharedPtr msg); // main functions - std::vector convertToPredictedObjectBasedObstacles( + std::vector createOneStepPolygons( + const std::vector & traj_points, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin = 0.0) const; + std::vector convertToObstacles( const Odometry & odometry, const PredictedObjects & objects, - const std::vector & traj_points, const PlannerData & planner_data) const; + const std::vector & traj_points) const; + std::vector convertToObstacles( + const Odometry & odometry, const PointCloud2 & pointcloud, + const std::vector & traj_points, + const std_msgs::msg::Header & traj_header) const; + std::tuple, std::vector, std::vector> + determineEgoBehaviorAgainstPredictedObjectObstacles( + const Odometry & odometry, const PredictedObjects & objects, + const std::vector & traj_points, const std::vector & obstacles); + std::tuple, std::vector, std::vector> + determineEgoBehaviorAgainstPointCloudObstacles( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & obstacles); std::vector decimateTrajectoryPoints( + const Odometry & odometry, const std::vector & traj_points) const; + std::optional createStopObstacleForPredictedObject( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const double precise_lateral_dist) const; + std::optional> + createCollisionPointForOutsideStopObstacle( const Odometry & odometry, const std::vector & traj_points, - const PlannerData & planner_data) const; + const std::vector & traj_polys, const Obstacle & obstacle, + const PredictedPath & resampled_predicted_path, double max_lat_margin_for_stop) const; + std::optional createStopObstacleForPointCloud( + const std::vector & traj_points, const Obstacle & obstacle, + const double precise_lateral_dist) const; + bool isInsideStopObstacle(const uint8_t label) const; + bool isOutsideStopObstacle(const uint8_t label) const; + bool isStopObstacle(const uint8_t label) const; + bool isInsideCruiseObstacle(const uint8_t label) const; + bool isOutsideCruiseObstacle(const uint8_t label) const; + bool isCruiseObstacle(const uint8_t label) const; + bool isSlowDownObstacle(const uint8_t label) const; + std::optional createYieldCruiseObstacle( + const Obstacle & obstacle, const std::vector & traj_points); + std::optional> findYieldCruiseObstacles( + const std::vector & obstacles, const std::vector & traj_points); + std::optional createCruiseObstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const double precise_lat_dist); + std::optional> createCollisionPointsForInsideCruiseObstacle( + const std::vector & traj_points, const std::vector & traj_polys, + const Obstacle & obstacle) const; + std::optional> createCollisionPointsForOutsideCruiseObstacle( + const std::vector & traj_points, const std::vector & traj_polys, + const Obstacle & obstacle) const; + bool isObstacleCrossing( + const std::vector & traj_points, const Obstacle & obstacle) const; + double calcCollisionTimeMargin( + const Odometry & odometry, const std::vector & collision_points, + const std::vector & traj_points, const bool is_driving_forward) const; + std::optional createSlowDownObstacleForPredictedObject( + const Odometry & odometry, const std::vector & traj_points, + const Obstacle & obstacle, const double precise_lat_dist); + std::optional createSlowDownObstacleForPointCloud( + const Obstacle & obstacle, const double precise_lat_dist); PlannerData createPlannerData( const Odometry & odometry, const AccelWithCovarianceStamped & acc, - const std::vector & traj_points); + const std::vector & traj_points) const; + void checkConsistency( + const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, + std::vector & stop_obstacles); + void publishVelocityLimit( + const std::optional & vel_limit, const std::string & module_name); void publishDebugMarker() const; + void publishDebugInfo() const; + void publishCalculationTime(const double calculation_time) const; + bool isFrontCollideObstacle( + const std::vector & traj_points, const Obstacle & obstacle, + const size_t first_collision_idx) const; + double calcTimeToReachCollisionPoint( + const Odometry & odometry, const geometry_msgs::msg::Point & collision_point, + const std::vector & traj_points, const double abs_ego_offset) const; bool enable_debug_info_; + bool enable_calculation_time_info_; + bool use_pointcloud_for_stop_; + bool use_pointcloud_for_slow_down_; + double min_behavior_stop_margin_; + bool enable_approaching_on_curve_; + double additional_safe_distance_margin_on_curve_; + double min_safe_distance_margin_on_curve_; + bool suppress_sudden_obstacle_stop_; + + std::vector inside_stop_obstacle_types_; + std::vector outside_stop_obstacle_types_; + std::vector inside_cruise_obstacle_types_; + std::vector outside_cruise_obstacle_types_; + std::vector slow_down_obstacle_types_; // parameter callback result OnSetParametersCallbackHandle::SharedPtr set_param_res_; // publisher rclcpp::Publisher::SharedPtr trajectory_pub_; + rclcpp::Publisher::SharedPtr vel_limit_pub_; + rclcpp::Publisher::SharedPtr clear_vel_limit_pub_; rclcpp::Publisher::SharedPtr debug_marker_pub_; + rclcpp::Publisher::SharedPtr debug_cruise_wall_marker_pub_; + rclcpp::Publisher::SharedPtr debug_stop_wall_marker_pub_; + rclcpp::Publisher::SharedPtr debug_slow_down_wall_marker_pub_; + rclcpp::Publisher::SharedPtr debug_stop_planning_info_pub_; + rclcpp::Publisher::SharedPtr debug_cruise_planning_info_pub_; + rclcpp::Publisher::SharedPtr debug_slow_down_planning_info_pub_; + rclcpp::Publisher::SharedPtr debug_calculation_time_pub_; // subscriber rclcpp::Subscription::SharedPtr traj_sub_; @@ -87,6 +181,8 @@ class ObstacleCruisePlannerNode : public rclcpp::Node this, "~/input/pointcloud"}; autoware::universe_utils::InterProcessPollingSubscriber acc_sub_{ this, "~/input/acceleration"}; + autoware::objects_of_interest_marker_interface::ObjectsOfInterestMarkerInterface + objects_of_interest_marker_interface_{this, "obstacle_cruise_planner"}; std::unique_ptr tf_buffer_; std::shared_ptr tf_listener_{nullptr}; @@ -94,33 +190,148 @@ class ObstacleCruisePlannerNode : public rclcpp::Node // Vehicle Parameters VehicleInfo vehicle_info_; - struct DebugData - { - DebugData() = default; - std::vector detection_polygons; - }; + // planning algorithm + enum class PlanningAlgorithm { OPTIMIZATION_BASE, PID_BASE, INVALID }; + PlanningAlgorithm getPlanningAlgorithmType(const std::string & param) const; + PlanningAlgorithm planning_algorithm_; + // stop watch + mutable autoware::universe_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; mutable std::shared_ptr debug_data_ptr_{nullptr}; // planner - std::unique_ptr planner_ptr_{nullptr}; + std::unique_ptr planner_ptr_{nullptr}; + + // previous PredictedObject-based obstacles + std::vector prev_stop_object_obstacles_; + std::vector prev_cruise_object_obstacles_; + std::vector prev_slow_down_object_obstacles_; + + // PointCloud-based stop obstacle history + std::vector stop_pc_obstacle_history_; // behavior determination parameter - CommonBehaviorDeterminationParam common_behavior_determination_param_; + struct BehaviorDeterminationParam + { + BehaviorDeterminationParam() = default; + explicit BehaviorDeterminationParam(rclcpp::Node & node); + void onParam(const std::vector & parameters); + + double decimate_trajectory_step_length; + double pointcloud_search_radius; + double pointcloud_voxel_grid_x; + double pointcloud_voxel_grid_y; + double pointcloud_voxel_grid_z; + double pointcloud_cluster_tolerance; + int pointcloud_min_cluster_size; + int pointcloud_max_cluster_size; + // hysteresis for stop and cruise + double obstacle_velocity_threshold_from_cruise_to_stop; + double obstacle_velocity_threshold_from_stop_to_cruise; + // inside + double crossing_obstacle_velocity_threshold; + double collision_time_margin; + // outside + double outside_obstacle_min_velocity_threshold; + double ego_obstacle_overlap_time_threshold; + double max_prediction_time_for_collision_check; + double crossing_obstacle_traj_angle_threshold; + int num_of_predicted_paths_for_outside_cruise_obstacle; + int num_of_predicted_paths_for_outside_stop_obstacle; + double pedestrian_deceleration_rate; + double bicycle_deceleration_rate; + // obstacle hold + double stop_obstacle_hold_time_threshold; + // reach collision point + double min_velocity_to_reach_collision_point; + // prediction resampling + double prediction_resampling_time_interval; + double prediction_resampling_time_horizon; + // max lateral time margin + double max_lat_time_margin_for_stop; + double max_lat_time_margin_for_cruise; + // max lateral margin + double max_lat_margin_for_stop; + double max_lat_margin_for_stop_against_unknown; + double max_lat_margin_for_cruise; + double max_lat_margin_for_slow_down; + double lat_hysteresis_margin_for_slow_down; + int successive_num_to_entry_slow_down_condition; + int successive_num_to_exit_slow_down_condition; + // consideration for the current ego pose + double time_to_convergence{1.5}; + bool enable_to_consider_current_pose{false}; + // yield related parameters + bool enable_yield{false}; + double yield_lat_distance_threshold; + double max_lat_dist_between_obstacles; + double stopped_obstacle_velocity_threshold; + double max_obstacles_collision_time; + }; + BehaviorDeterminationParam behavior_determination_param_; std::unordered_map need_to_clear_vel_limit_{ {"cruise", false}, {"slow_down", false}}; + struct SlowDownConditionCounter + { + void resetCurrentUuids() { current_uuids_.clear(); } + void addCurrentUuid(const std::string & uuid) { current_uuids_.push_back(uuid); } + void removeCounterUnlessUpdated() + { + std::vector obsolete_uuids; + for (const auto & key_and_value : counter_) { + if ( + std::find(current_uuids_.begin(), current_uuids_.end(), key_and_value.first) == + current_uuids_.end()) { + obsolete_uuids.push_back(key_and_value.first); + } + } + + for (const auto & obsolete_uuid : obsolete_uuids) { + counter_.erase(obsolete_uuid); + } + } + + int increaseCounter(const std::string & uuid) + { + if (counter_.count(uuid) != 0) { + counter_.at(uuid) = std::max(1, counter_.at(uuid) + 1); + } else { + counter_.emplace(uuid, 1); + } + return counter_.at(uuid); + } + int decreaseCounter(const std::string & uuid) + { + if (counter_.count(uuid) != 0) { + counter_.at(uuid) = std::min(-1, counter_.at(uuid) - 1); + } else { + counter_.emplace(uuid, -1); + } + return counter_.at(uuid); + } + void reset(const std::string & uuid) { counter_.emplace(uuid, 0); } + + // NOTE: positive is for meeting entering condition, and negative is for exiting. + std::unordered_map counter_; + std::vector current_uuids_; + }; + SlowDownConditionCounter slow_down_condition_counter_; + + EgoNearestParam ego_nearest_param_; + bool is_driving_forward_{true}; + bool enable_slow_down_planning_{false}; bool use_pointcloud_{false}; + std::vector prev_closest_stop_object_obstacles_{}; + std::unique_ptr logger_configure_; std::unique_ptr published_time_publisher_; - - std::unique_ptr obstacle_stop_module_; - std::unique_ptr obstacle_slow_down_module_; - std::unique_ptr obstacle_cruise_module_; }; } // namespace autoware::motion_planning diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp new file mode 100644 index 0000000000000..e173a19ea19de --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp @@ -0,0 +1,122 @@ +// 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. + +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT + +#include "autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" +#include "autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" +#include "autoware/obstacle_cruise_planner/planner_interface.hpp" +#include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include +#include +#include + +#include +#include +#include +#include + +class OptimizationBasedPlanner : public PlannerInterface +{ +public: + OptimizationBasedPlanner( + rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr); + + std::vector generateCruiseTrajectory( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const std::vector & obstacles, + std::optional & vel_limit) override; + +private: + // Member Functions + std::vector createTimeVector(); + std::tuple calcInitialMotion( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const std::vector & prev_traj_points); + + bool checkHasReachedGoal( + const PlannerData & planner_data, const std::vector & stop_traj_points); + + std::optional getSBoundaries( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const std::vector & obstacles, const std::vector & time_vec); + + std::optional getSBoundaries( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const CruiseObstacle & object, const std::vector & time_vec, const double traj_length); + + std::optional getSBoundariesForOnTrajectoryObject( + const PlannerData & planner_data, const std::vector & time_vec, + const double safety_distance, const CruiseObstacle & object, const double traj_length); + + std::optional getSBoundariesForOffTrajectoryObject( + const PlannerData & planner_data, const std::vector & time_vec, + const double safety_distance, const CruiseObstacle & object, const double traj_length); + + bool checkOnTrajectory( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const PointWithStamp & point); + + std::optional calcTrajectoryLengthFromCurrentPose( + const std::vector & traj_points, const geometry_msgs::msg::Pose & ego_pose); + + geometry_msgs::msg::Pose transformBaseLink2Center( + const geometry_msgs::msg::Pose & pose_base_link); + + std::optional processOptimizedResult( + const double v0, const VelocityOptimizer::OptimizationResult & opt_result, const double offset); + + void publishDebugTrajectory( + const PlannerData & planner_data, const double offset, const std::vector & time_vec, + const SBoundaries & s_boundaries, const VelocityOptimizer::OptimizationResult & opt_result); + + std::vector prev_output_; + + // Velocity Optimizer + std::shared_ptr velocity_optimizer_ptr_; + + // Publisher + rclcpp::Publisher::SharedPtr boundary_pub_; + rclcpp::Publisher::SharedPtr optimized_sv_pub_; + rclcpp::Publisher::SharedPtr optimized_st_graph_pub_; + rclcpp::Publisher::SharedPtr debug_wall_marker_pub_; + + // Subscriber + rclcpp::Subscription::SharedPtr smoothed_traj_sub_; + + Trajectory::ConstSharedPtr smoothed_trajectory_ptr_{nullptr}; + + // Resampling Parameter + double dense_resampling_time_interval_; + double sparse_resampling_time_interval_; + double dense_time_horizon_; + double max_time_horizon_; + + double t_dangerous_; + double velocity_margin_; + + double replan_vel_deviation_; + double engage_velocity_; + double engage_acceleration_; + double engage_exit_ratio_; + double stop_dist_to_prohibit_engage_; +}; +// clang-format off +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__OPTIMIZATION_BASED_PLANNER_HPP_ // NOLINT +// clang-format on diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp new file mode 100644 index 0000000000000..4224f35fbc6e1 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp @@ -0,0 +1,33 @@ +// 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. +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ + +#include +#include + +class SBoundary +{ +public: + SBoundary(const double _max_s, const double _min_s) : max_s(_max_s), min_s(_min_s) {} + SBoundary() : max_s(std::numeric_limits::max()), min_s(0.0) {} + + double max_s = std::numeric_limits::max(); + double min_s = 0.0; + bool is_object = false; +}; + +using SBoundaries = std::vector; + +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__S_BOUNDARY_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp new file mode 100644 index 0000000000000..ca17b428e369a --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp @@ -0,0 +1,75 @@ +// 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. +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ + +#include "autoware/obstacle_cruise_planner/optimization_based_planner/s_boundary.hpp" +#include "autoware/osqp_interface/osqp_interface.hpp" + +#include + +class VelocityOptimizer +{ +public: + struct OptimizationData + { + std::vector time_vec; + double s0; + double v0; + double a0; + double v_max; + double a_max; + double a_min; + double limit_a_max; + double limit_a_min; + double limit_j_max; + double limit_j_min; + double j_max; + double j_min; + double t_dangerous; + double idling_time; + SBoundaries s_boundary; + }; + + struct OptimizationResult + { + std::vector t; + std::vector s; + std::vector v; + std::vector a; + std::vector j; + }; + + VelocityOptimizer( + const double max_s_weight, const double max_v_weight, const double over_s_safety_weight, + const double over_s_ideal_weight, const double over_v_weight, const double over_a_weight, + const double over_j_weight); + + OptimizationResult optimize(const OptimizationData & data); + +private: + // Parameter + double max_s_weight_; + double max_v_weight_; + double over_s_safety_weight_; + double over_s_ideal_weight_; + double over_v_weight_; + double over_a_weight_; + double over_j_weight_; + + // QPSolver + autoware::osqp_interface::OSQPInterface qp_solver_; +}; + +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__OPTIMIZATION_BASED_PLANNER__VELOCITY_OPTIMIZER_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp new file mode 100644 index 0000000000000..f2269aeaa2967 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp @@ -0,0 +1,97 @@ +// 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. + +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ + +#include + +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include + +using tier4_debug_msgs::msg::Float32MultiArrayStamped; + +class CruisePlanningDebugInfo +{ +public: + enum class TYPE { + // ego + EGO_VELOCITY = 0, // index: 0 + EGO_ACCELERATION, + EGO_JERK, // no data + + // cruise planning + CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, // index: 3 + CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED, + CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, + CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED, + CRUISE_TARGET_OBSTACLE_DISTANCE, + CRUISE_ERROR_DISTANCE_RAW, + CRUISE_ERROR_DISTANCE_FILTERED, + + CRUISE_RELATIVE_EGO_VELOCITY, // index: 10 + CRUISE_TIME_TO_COLLISION, + + CRUISE_TARGET_VELOCITY, // index: 12 + CRUISE_TARGET_ACCELERATION, + CRUISE_TARGET_JERK_RATIO, + + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + int getIndex(const TYPE type) const { return static_cast(type); } + + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> get() const { return info_; } + + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void set(const TYPE type, const double val) { info_.at(static_cast(type)) = val; } + + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void set(const int type, const double val) { info_.at(type) = val; } + + void reset() { info_.fill(0.0); } + + Float32MultiArrayStamped convertToMessage(const rclcpp::Time & current_time) const + { + Float32MultiArrayStamped msg{}; + msg.stamp = current_time; + for (const auto & v : get()) { + msg.data.push_back(v); + } + return msg; + } + +private: + std::array(TYPE::SIZE)> info_; +}; + +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__CRUISE_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp new file mode 100644 index 0000000000000..d063d08d82d92 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp @@ -0,0 +1,142 @@ +// 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. + +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ + +#include "autoware/obstacle_cruise_planner/pid_based_planner/cruise_planning_debug_info.hpp" +#include "autoware/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp" +#include "autoware/obstacle_cruise_planner/planner_interface.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" + +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include +#include + +using autoware::signal_processing::LowpassFilter1d; + +class PIDBasedPlanner : public PlannerInterface +{ +public: + struct CruiseObstacleInfo + { + CruiseObstacleInfo( + const CruiseObstacle & obstacle_arg, const double error_cruise_dist_arg, + const double dist_to_obstacle_arg, const double target_dist_to_obstacle_arg) + : obstacle(obstacle_arg), + error_cruise_dist(error_cruise_dist_arg), + dist_to_obstacle(dist_to_obstacle_arg), + target_dist_to_obstacle(target_dist_to_obstacle_arg) + { + } + + CruiseObstacle obstacle; + double error_cruise_dist; + double dist_to_obstacle; + double target_dist_to_obstacle; + }; + + PIDBasedPlanner( + rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr); + + std::vector generateCruiseTrajectory( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const std::vector & obstacles, + std::optional & vel_limit) override; + + void updateCruiseParam(const std::vector & parameters) override; + +private: + Float32MultiArrayStamped getCruisePlanningDebugMessage( + const rclcpp::Time & current_time) const override + { + return cruise_planning_debug_info_.convertToMessage(current_time); + } + + std::optional calcObstacleToCruise( + const PlannerData & planner_data, const std::vector & obstacles); + + std::vector planCruise( + const PlannerData & planner_data, const std::vector & stop_traj_points, + std::optional & vel_limit, + const std::optional & cruise_obstacle_info); + + // velocity limit based planner + VelocityLimit doCruiseWithVelocityLimit( + const PlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info); + + // velocity insertion based planner + std::vector doCruiseWithTrajectory( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const CruiseObstacleInfo & cruise_obstacle_info); + std::vector getAccelerationLimitedTrajectory( + const std::vector traj, const geometry_msgs::msg::Pose & start_pose, + const double v0, const double a0, const double target_acc, + const double target_jerk_ratio) const; + + // ROS parameters + double min_accel_during_cruise_; + double min_cruise_target_vel_; + + CruisePlanningDebugInfo cruise_planning_debug_info_; + + struct VelocityLimitBasedPlannerParam + { + std::unique_ptr pid_vel_controller; + double output_ratio_during_accel; + double vel_to_acc_weight; + bool enable_jerk_limit_to_output_acc{false}; + bool disable_target_acceleration{false}; + }; + VelocityLimitBasedPlannerParam velocity_limit_based_planner_param_; + + struct VelocityInsertionBasedPlannerParam + { + std::unique_ptr pid_acc_controller; + std::unique_ptr pid_jerk_controller; + double output_acc_ratio_during_accel; + double output_jerk_ratio_during_accel; + bool enable_jerk_limit_to_output_acc{false}; + }; + VelocityInsertionBasedPlannerParam velocity_insertion_based_planner_param_; + + std::optional prev_target_acc_; + + // lpf for nodes's input + std::shared_ptr lpf_dist_to_obstacle_ptr_; + std::shared_ptr lpf_error_cruise_dist_ptr_; + std::shared_ptr lpf_obstacle_vel_ptr_; + + // lpf for planner's input + std::shared_ptr lpf_normalized_error_cruise_dist_ptr_; + + // lpf for output + std::shared_ptr lpf_output_vel_ptr_; + std::shared_ptr lpf_output_acc_ptr_; + std::shared_ptr lpf_output_jerk_ptr_; + + std::vector prev_traj_points_; + + bool use_velocity_limit_based_planner_{true}; + + double time_to_evaluate_rss_; + + std::function error_func_; +}; + +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_BASED_PLANNER_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp new file mode 100644 index 0000000000000..c5d93b30fa6e0 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/pid_based_planner/pid_controller.hpp @@ -0,0 +1,62 @@ +// 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. + +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_CONTROLLER_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_CONTROLLER_HPP_ + +#include + +class PIDController +{ +public: + PIDController(const double kp, const double ki, const double kd) + : kp_(kp), ki_(ki), kd_(kd), error_sum_(0.0) + { + } + + double calc(const double error) + { + error_sum_ += error; + + // TODO(murooka) use time for d gain calculation + const double output = + kp_ * error + ki_ * error_sum_ + (prev_error_ ? kd_ * (error - *prev_error_) : 0.0); + prev_error_ = error; + return output; + } + + double getKp() const { return kp_; } + double getKi() const { return ki_; } + double getKd() const { return kd_; } + + void updateParam(const double kp, const double ki, const double kd) + { + kp_ = kp; + ki_ = ki; + kd_ = kd; + + error_sum_ = 0.0; + prev_error_ = {}; + } + +private: + double kp_; + double ki_; + double kd_; + + double error_sum_; + std::optional prev_error_; +}; + +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__PID_BASED_PLANNER__PID_CONTROLLER_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp new file mode 100644 index 0000000000000..f5e183afd12bd --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/planner_interface.hpp @@ -0,0 +1,448 @@ +// 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. + +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/obstacle_cruise_planner/common_structs.hpp" +#include "autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp" +#include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/planning_factor_interface/planning_factor_interface.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" +#include "autoware/universe_utils/system/stop_watch.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using Metric = tier4_metric_msgs::msg::Metric; +using MetricArray = tier4_metric_msgs::msg::MetricArray; + +class PlannerInterface +{ +public: + virtual ~PlannerInterface() = default; + PlannerInterface( + rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) + : planning_factor_interface_{std::make_unique< + autoware::planning_factor_interface::PlanningFactorInterface>( + &node, "obstacle_cruise_planner")}, + longitudinal_info_(longitudinal_info), + vehicle_info_(vehicle_info), + ego_nearest_param_(ego_nearest_param), + debug_data_ptr_(debug_data_ptr), + slow_down_param_(SlowDownParam(node)), + stop_param_(StopParam(node, longitudinal_info)) + { + stop_speed_exceeded_pub_ = + node.create_publisher("~/output/stop_speed_exceeded", 1); + metrics_pub_ = node.create_publisher("~/metrics", 10); + + moving_object_speed_threshold = + node.declare_parameter("slow_down.moving_object_speed_threshold"); + moving_object_hysteresis_range = + node.declare_parameter("slow_down.moving_object_hysteresis_range"); + } + + void setParam( + const bool enable_debug_info, const bool enable_calculation_time_info, + const bool use_pointcloud, const double min_behavior_stop_margin, + const double enable_approaching_on_curve, const double additional_safe_distance_margin_on_curve, + const double min_safe_distance_margin_on_curve, const bool suppress_sudden_obstacle_stop) + { + enable_debug_info_ = enable_debug_info; + enable_calculation_time_info_ = enable_calculation_time_info; + use_pointcloud_ = use_pointcloud; + min_behavior_stop_margin_ = min_behavior_stop_margin; + enable_approaching_on_curve_ = enable_approaching_on_curve; + additional_safe_distance_margin_on_curve_ = additional_safe_distance_margin_on_curve; + min_safe_distance_margin_on_curve_ = min_safe_distance_margin_on_curve; + suppress_sudden_obstacle_stop_ = suppress_sudden_obstacle_stop; + } + + std::vector generateStopTrajectory( + const PlannerData & planner_data, const std::vector & stop_obstacles); + + virtual std::vector generateCruiseTrajectory( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const std::vector & cruise_obstacles, + std::optional & vel_limit) = 0; + + std::vector generateSlowDownTrajectory( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const std::vector & slow_down_obstacles, + std::optional & vel_limit); + + std::vector makeMetrics( + const std::string & module_name, const std::string & reason, + const std::optional & planner_data = std::nullopt, + const std::optional & stop_pose = std::nullopt, + const std::optional & stop_obstacle = std::nullopt); + void publishMetrics(const rclcpp::Time & current_time); + void publishPlanningFactors() { planning_factor_interface_->publish(); } + void clearMetrics(); + + void onParam(const std::vector & parameters) + { + updateCommonParam(parameters); + updateCruiseParam(parameters); + slow_down_param_.onParam(parameters); + stop_param_.onParam(parameters, longitudinal_info_); + } + + Float32MultiArrayStamped getStopPlanningDebugMessage(const rclcpp::Time & current_time) const + { + return stop_planning_debug_info_.convertToMessage(current_time); + } + virtual Float32MultiArrayStamped getCruisePlanningDebugMessage( + [[maybe_unused]] const rclcpp::Time & current_time) const + { + return Float32MultiArrayStamped{}; + } + Float32MultiArrayStamped getSlowDownPlanningDebugMessage(const rclcpp::Time & current_time) + { + slow_down_debug_multi_array_.stamp = current_time; + return slow_down_debug_multi_array_; + } + double getSafeDistanceMargin() const { return longitudinal_info_.safe_distance_margin; } + +protected: + std::unique_ptr + planning_factor_interface_; + + // Parameters + bool enable_debug_info_{false}; + bool enable_calculation_time_info_{false}; + bool use_pointcloud_{false}; + LongitudinalInfo longitudinal_info_; + double min_behavior_stop_margin_; + bool enable_approaching_on_curve_; + double additional_safe_distance_margin_on_curve_; + double min_safe_distance_margin_on_curve_; + bool suppress_sudden_obstacle_stop_; + + // stop watch + autoware::universe_utils::StopWatch< + std::chrono::milliseconds, std::chrono::microseconds, std::chrono::steady_clock> + stop_watch_; + + // Publishers + rclcpp::Publisher::SharedPtr stop_speed_exceeded_pub_; + rclcpp::Publisher::SharedPtr metrics_pub_; + + // Vehicle Parameters + autoware::vehicle_info_utils::VehicleInfo vehicle_info_; + + EgoNearestParam ego_nearest_param_; + + mutable std::shared_ptr debug_data_ptr_; + + // debug info + StopPlanningDebugInfo stop_planning_debug_info_; + Float32MultiArrayStamped slow_down_debug_multi_array_; + + double calcDistanceToCollisionPoint( + const PlannerData & planner_data, const geometry_msgs::msg::Point & collision_point); + + double calcRSSDistance( + const double ego_vel, const double obstacle_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_ego_accel_for_rss) - + std::pow(obstacle_vel, 2) * 0.5 / std::abs(i.min_object_accel_for_rss) + margin; + return rss_dist_with_margin; + } + + void updateCommonParam(const std::vector & parameters) + { + longitudinal_info_.onParam(parameters); + } + + virtual void updateCruiseParam([[maybe_unused]] const std::vector & parameters) + { + } + + size_t findEgoIndex( + const std::vector & traj_points, + const geometry_msgs::msg::Pose & ego_pose) const + { + const auto & p = ego_nearest_param_; + return autoware::motion_utils::findFirstNearestIndexWithSoftConstraints( + traj_points, ego_pose, p.dist_threshold, p.yaw_threshold); + } + + size_t findEgoSegmentIndex( + const std::vector & traj_points, + const geometry_msgs::msg::Pose & ego_pose) const + { + const auto & p = ego_nearest_param_; + return autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + traj_points, ego_pose, p.dist_threshold, p.yaw_threshold); + } + +private: + struct SlowDownOutput + { + SlowDownOutput() = default; + SlowDownOutput( + const std::string & arg_uuid, const std::vector & traj_points, + const std::optional & start_idx, const std::optional & end_idx, + const double arg_target_vel, const double arg_feasible_target_vel, + const double arg_precise_lat_dist, const bool is_moving) + : uuid(arg_uuid), + target_vel(arg_target_vel), + feasible_target_vel(arg_feasible_target_vel), + precise_lat_dist(arg_precise_lat_dist), + is_moving(is_moving) + { + if (start_idx) { + start_point = traj_points.at(*start_idx).pose; + } + if (end_idx) { + end_point = traj_points.at(*end_idx).pose; + } + } + + std::string uuid; + double target_vel; + double feasible_target_vel; + double precise_lat_dist; + std::optional start_point{std::nullopt}; + std::optional end_point{std::nullopt}; + bool is_moving; + }; + double calculateMarginFromObstacleOnCurve( + const PlannerData & planner_data, const StopObstacle & stop_obstacle) const; + double calculateSlowDownVelocity( + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const bool is_obstacle_moving) const; + std::optional> calculateDistanceToSlowDownWithConstraints( + const PlannerData & planner_data, const std::vector & traj_points, + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const double dist_to_ego, const bool is_obstacle_moving) const; + + struct SlowDownInfo + { + // NOTE: Acceleration limit is applied to lon_dist not to occur sudden brake. + const double lon_dist; // from ego pose to slow down point + const double vel; // slow down velocity + }; + + struct SlowDownParam + { + std::vector obstacle_labels{"default"}; + std::vector obstacle_moving_classification{"static", "moving"}; + std::unordered_map types_map; + struct ObstacleSpecificParams + { + double max_lat_margin; + double min_lat_margin; + double max_ego_velocity; + double min_ego_velocity; + }; + explicit SlowDownParam(rclcpp::Node & node) + { + obstacle_labels = + node.declare_parameter>("slow_down.labels", obstacle_labels); + // obstacle label dependant parameters + for (const auto & label : obstacle_labels) { + for (const auto & movement_postfix : obstacle_moving_classification) { + ObstacleSpecificParams params; + params.max_lat_margin = node.declare_parameter( + "slow_down." + label + "." + movement_postfix + ".max_lat_margin"); + params.min_lat_margin = node.declare_parameter( + "slow_down." + label + "." + movement_postfix + ".min_lat_margin"); + params.max_ego_velocity = node.declare_parameter( + "slow_down." + label + "." + movement_postfix + ".max_ego_velocity"); + params.min_ego_velocity = node.declare_parameter( + "slow_down." + label + "." + movement_postfix + ".min_ego_velocity"); + obstacle_to_param_struct_map.emplace( + std::make_pair(label + "." + movement_postfix, params)); + } + } + + // common parameters + time_margin_on_target_velocity = + node.declare_parameter("slow_down.time_margin_on_target_velocity"); + lpf_gain_slow_down_vel = node.declare_parameter("slow_down.lpf_gain_slow_down_vel"); + lpf_gain_lat_dist = node.declare_parameter("slow_down.lpf_gain_lat_dist"); + lpf_gain_dist_to_slow_down = + node.declare_parameter("slow_down.lpf_gain_dist_to_slow_down"); + + types_map = {{ObjectClassification::UNKNOWN, "unknown"}, + {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, + {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, + {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, + {ObjectClassification::PEDESTRIAN, "pedestrian"}}; + } + + ObstacleSpecificParams getObstacleParamByLabel( + const ObjectClassification & label_id, const bool is_obstacle_moving) const + { + const std::string label = + (types_map.count(label_id.label) > 0) ? types_map.at(label_id.label) : "default"; + const std::string movement_postfix = (is_obstacle_moving) ? "moving" : "static"; + return (obstacle_to_param_struct_map.count(label + "." + movement_postfix) > 0) + ? obstacle_to_param_struct_map.at(label + "." + movement_postfix) + : obstacle_to_param_struct_map.at("default." + movement_postfix); + } + + void onParam(const std::vector & parameters) + { + // obstacle type dependant parameters + for (const auto & label : obstacle_labels) { + for (const auto & movement_postfix : obstacle_moving_classification) { + if (obstacle_to_param_struct_map.count(label + "." + movement_postfix) < 1) continue; + auto & param_by_obstacle_label = + obstacle_to_param_struct_map.at(label + "." + movement_postfix); + autoware::universe_utils::updateParam( + parameters, "slow_down." + label + "." + movement_postfix + ".max_lat_margin", + param_by_obstacle_label.max_lat_margin); + autoware::universe_utils::updateParam( + parameters, "slow_down." + label + "." + movement_postfix + ".min_lat_margin", + param_by_obstacle_label.min_lat_margin); + autoware::universe_utils::updateParam( + parameters, "slow_down." + label + "." + movement_postfix + ".max_ego_velocity", + param_by_obstacle_label.max_ego_velocity); + autoware::universe_utils::updateParam( + parameters, "slow_down." + label + "." + movement_postfix + ".min_ego_velocity", + param_by_obstacle_label.min_ego_velocity); + } + } + + // common parameters + autoware::universe_utils::updateParam( + parameters, "slow_down.time_margin_on_target_velocity", time_margin_on_target_velocity); + autoware::universe_utils::updateParam( + parameters, "slow_down.lpf_gain_slow_down_vel", lpf_gain_slow_down_vel); + autoware::universe_utils::updateParam( + parameters, "slow_down.lpf_gain_lat_dist", lpf_gain_lat_dist); + autoware::universe_utils::updateParam( + parameters, "slow_down.lpf_gain_dist_to_slow_down", lpf_gain_dist_to_slow_down); + } + + std::unordered_map obstacle_to_param_struct_map; + + double time_margin_on_target_velocity; + double lpf_gain_slow_down_vel; + double lpf_gain_lat_dist; + double lpf_gain_dist_to_slow_down; + }; + SlowDownParam slow_down_param_; + struct StopParam + { + struct ObstacleSpecificParams + { + double limit_min_acc; + double sudden_object_acc_threshold; + double sudden_object_dist_threshold; + bool abandon_to_stop; + }; + const std::unordered_map types_maps = { + {ObjectClassification::UNKNOWN, "unknown"}, {ObjectClassification::CAR, "car"}, + {ObjectClassification::TRUCK, "truck"}, {ObjectClassification::BUS, "bus"}, + {ObjectClassification::TRAILER, "trailer"}, {ObjectClassification::MOTORCYCLE, "motorcycle"}, + {ObjectClassification::BICYCLE, "bicycle"}, {ObjectClassification::PEDESTRIAN, "pedestrian"}}; + std::unordered_map type_specified_param_list; + explicit StopParam(rclcpp::Node & node, const LongitudinalInfo & longitudinal_info) + { + const std::string param_prefix = "stop.type_specified_params."; + std::vector obstacle_labels{"default"}; + obstacle_labels = + node.declare_parameter>(param_prefix + "labels", obstacle_labels); + + for (const auto & type_str : obstacle_labels) { + if (type_str != "default") { + ObstacleSpecificParams param{ + node.declare_parameter(param_prefix + type_str + ".limit_min_acc"), + node.declare_parameter( + param_prefix + type_str + ".sudden_object_acc_threshold"), + node.declare_parameter( + param_prefix + type_str + ".sudden_object_dist_threshold"), + node.declare_parameter(param_prefix + type_str + ".abandon_to_stop")}; + + param.sudden_object_acc_threshold = + std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel); + param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold); + + type_specified_param_list.emplace(type_str, param); + } + } + } + void onParam( + const std::vector & parameters, const LongitudinalInfo & longitudinal_info) + { + const std::string param_prefix = "stop.type_specified_params."; + for (auto & [type_str, param] : type_specified_param_list) { + if (type_str == "default") { + continue; + } + autoware::universe_utils::updateParam( + parameters, param_prefix + type_str + ".limit_min_acc", param.limit_min_acc); + autoware::universe_utils::updateParam( + parameters, param_prefix + type_str + ".sudden_object_acc_threshold", + param.sudden_object_acc_threshold); + autoware::universe_utils::updateParam( + parameters, param_prefix + type_str + ".sudden_object_dist_threshold", + param.sudden_object_dist_threshold); + autoware::universe_utils::updateParam( + parameters, param_prefix + type_str + ".abandon_to_stop", param.abandon_to_stop); + + param.sudden_object_acc_threshold = + std::min(param.sudden_object_acc_threshold, longitudinal_info.limit_min_accel); + param.limit_min_acc = std::min(param.limit_min_acc, param.sudden_object_acc_threshold); + } + } + std::string getParamType(const ObjectClassification label) + { + const auto type_str = types_maps.at(label.label); + if (type_specified_param_list.count(type_str) == 0) { + return "default"; + } + return type_str; + } + ObstacleSpecificParams getParam(const ObjectClassification label) + { + return type_specified_param_list.at(getParamType(label)); + } + }; + StopParam stop_param_; + double moving_object_speed_threshold; + double moving_object_hysteresis_range; + std::vector prev_slow_down_output_; + // previous trajectory and distance to stop + // NOTE: Previous trajectory is memorized to deal with nearest index search for overlapping or + // crossing lanes. + std::optional, double>> prev_stop_distance_info_{ + std::nullopt}; +}; + +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__PLANNER_INTERFACE_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp new file mode 100644 index 0000000000000..f84aa26480765 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/polygon_utils.hpp @@ -0,0 +1,60 @@ +// 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. + +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ + +#include "autoware/obstacle_cruise_planner/common_structs.hpp" +#include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/universe_utils/geometry/boost_geometry.hpp" +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include + +#include +#include +#include +#include + +namespace polygon_utils +{ +namespace bg = boost::geometry; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; + +Polygon2d createOneStepPolygon( + const std::vector & last_poses, + const std::vector & current_poses, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, const double lat_margin); + +std::optional> getCollisionPoint( + const std::vector & traj_points, const std::vector & traj_polygons, + const Obstacle & obstacle, const bool is_driving_forward, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + +std::optional> getCollisionPoint( + const std::vector & traj_points, const size_t collision_idx, + const std::vector & collision_points, const bool is_driving_forward, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info); + +std::vector getCollisionPoints( + const std::vector & traj_points, const std::vector & traj_polygons, + const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape, + const rclcpp::Time & current_time, const bool is_driving_forward, + std::vector & collision_index, const double max_dist = std::numeric_limits::max(), + const double max_prediction_time_for_collision_check = std::numeric_limits::max()); + +} // namespace polygon_utils + +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__POLYGON_UTILS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp new file mode 100644 index 0000000000000..ef4f5d0eba808 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/stop_planning_debug_info.hpp @@ -0,0 +1,88 @@ +// 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. + +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ + +#include + +#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp" + +#include + +using tier4_debug_msgs::msg::Float32MultiArrayStamped; + +class StopPlanningDebugInfo +{ +public: + enum class TYPE { + // ego + EGO_VELOCITY = 0, // index: 0 + EGO_ACCELERATION, + EGO_JERK, // no data + + // stop planning + STOP_CURRENT_OBSTACLE_DISTANCE, // index: 3 + STOP_CURRENT_OBSTACLE_VELOCITY, + STOP_TARGET_OBSTACLE_DISTANCE, + STOP_TARGET_VELOCITY, + STOP_TARGET_ACCELERATION, + + SIZE + }; + + /** + * @brief get the index corresponding to the given value TYPE + * @param [in] type the TYPE enum for which to get the index + * @return index of the type + */ + int getIndex(const TYPE type) const { return static_cast(type); } + + /** + * @brief get all the debug values as an std::array + * @return array of all debug values + */ + std::array(TYPE::SIZE)> get() const { return info_; } + + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void set(const TYPE type, const double val) { info_.at(static_cast(type)) = val; } + + /** + * @brief set the given type to the given value + * @param [in] type index of the type + * @param [in] value value to set + */ + void set(const int type, const double val) { info_.at(type) = val; } + + void reset() { info_.fill(0.0); } + + Float32MultiArrayStamped convertToMessage(const rclcpp::Time & current_time) const + { + Float32MultiArrayStamped msg{}; + msg.stamp = current_time; + for (const auto & v : get()) { + msg.data.push_back(v); + } + return msg; + } + +private: + std::array(TYPE::SIZE)> info_; +}; + +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__STOP_PLANNING_DEBUG_INFO_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp new file mode 100644 index 0000000000000..31344903b6809 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/type_alias.hpp @@ -0,0 +1,65 @@ +// 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. + +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ + +#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp" + +#include "autoware_perception_msgs/msg/predicted_object.hpp" +#include "autoware_perception_msgs/msg/predicted_objects.hpp" +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "geometry_msgs/msg/accel_stamped.hpp" +#include "geometry_msgs/msg/accel_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" +#include "sensor_msgs/msg/point_cloud2.hpp" +#include "tier4_debug_msgs/msg/float32_stamped.hpp" +#include "tier4_debug_msgs/msg/float64_stamped.hpp" +#include "tier4_planning_msgs/msg/stop_speed_exceeded.hpp" +#include "tier4_planning_msgs/msg/velocity_limit.hpp" +#include "tier4_planning_msgs/msg/velocity_limit_clear_command.hpp" +#include "visualization_msgs/msg/marker_array.hpp" + +#include +#include + +using autoware::vehicle_info_utils::VehicleInfo; +using autoware_perception_msgs::msg::ObjectClassification; +using autoware_perception_msgs::msg::PredictedObject; +using autoware_perception_msgs::msg::PredictedObjects; +using autoware_perception_msgs::msg::PredictedPath; +using autoware_perception_msgs::msg::Shape; +using autoware_planning_msgs::msg::Trajectory; +using autoware_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::AccelStamped; +using geometry_msgs::msg::AccelWithCovarianceStamped; +using geometry_msgs::msg::Twist; +using nav_msgs::msg::Odometry; +using sensor_msgs::msg::PointCloud2; +using tier4_debug_msgs::msg::Float32Stamped; +using tier4_debug_msgs::msg::Float64Stamped; +using tier4_planning_msgs::msg::StopSpeedExceeded; +using tier4_planning_msgs::msg::VelocityLimit; +using tier4_planning_msgs::msg::VelocityLimitClearCommand; +using visualization_msgs::msg::Marker; +using visualization_msgs::msg::MarkerArray; +namespace bg = boost::geometry; +using autoware::universe_utils::Point2d; +using autoware::universe_utils::Polygon2d; + +using PointCloud = pcl::PointCloud; + +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__TYPE_ALIAS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp new file mode 100644 index 0000000000000..ebabd96a54608 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/include/autoware/obstacle_cruise_planner/utils.hpp @@ -0,0 +1,99 @@ +// 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. + +#ifndef AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ +#define AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ + +#include "autoware/obstacle_cruise_planner/type_alias.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "common_structs.hpp" + +#include + +#include +#include +#include +#include + +namespace obstacle_cruise_utils +{ +Marker getObjectMarker( + const geometry_msgs::msg::Pose & obj_pose, size_t idx, const std::string & ns, const double r, + const double g, const double b); + +PoseWithStamp getCurrentObjectPose( + const PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time, const bool use_prediction); + +std::vector getClosestStopObstacles(const std::vector & stop_obstacles); + +template +size_t getIndexWithLongitudinalOffset( + const T & points, const double longitudinal_offset, std::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) { + 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; i < points.size() - 1; ++i) { + const double segment_length = + autoware::universe_utils::calcDistance2d(points.at(i), points.at(i + 1)); + sum_length += segment_length; + if (sum_length >= longitudinal_offset) { + const double back_length = sum_length - longitudinal_offset; + const double front_length = segment_length - back_length; + if (front_length < back_length) { + return i; + } else { + return i + 1; + } + } + } + return points.size() - 1; + } + + for (size_t i = *start_idx; 0 < i; --i) { + const double segment_length = + autoware::universe_utils::calcDistance2d(points.at(i - 1), points.at(i)); + sum_length += segment_length; + if (sum_length >= -longitudinal_offset) { + const double back_length = sum_length + longitudinal_offset; + const double front_length = segment_length - back_length; + if (front_length < back_length) { + return i; + } else { + return i - 1; + } + } + } + return 0; +} + +} // namespace obstacle_cruise_utils + +#endif // AUTOWARE__OBSTACLE_CRUISE_PLANNER__UTILS_HPP_ diff --git a/planning/autoware_obstacle_cruise_planner/package.xml b/planning/autoware_obstacle_cruise_planner/package.xml index 2062d0a49f666..71c3b1fb1e0d5 100644 --- a/planning/autoware_obstacle_cruise_planner/package.xml +++ b/planning/autoware_obstacle_cruise_planner/package.xml @@ -18,8 +18,6 @@ ament_cmake_auto autoware_cmake - autoware_adapi_v1_msgs - autoware_internal_debug_msgs autoware_interpolation autoware_lanelet2_extension autoware_motion_utils @@ -40,6 +38,7 @@ std_msgs tf2 tf2_ros + tier4_debug_msgs tier4_metric_msgs tier4_planning_msgs visualization_msgs diff --git a/planning/autoware_obstacle_cruise_planner/src/node.cpp b/planning/autoware_obstacle_cruise_planner/src/node.cpp index 20fd9a30c5314..4242f0d559946 100644 --- a/planning/autoware_obstacle_cruise_planner/src/node.cpp +++ b/planning/autoware_obstacle_cruise_planner/src/node.cpp @@ -16,12 +16,11 @@ #include "autoware/motion_utils/resample/resample.hpp" #include "autoware/motion_utils/trajectory/conversion.hpp" -#include "autoware/obstacle_cruise_planner/cruise/obstacle_cruise_initializer.hpp" +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" #include "autoware/obstacle_cruise_planner/polygon_utils.hpp" #include "autoware/obstacle_cruise_planner/utils.hpp" #include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" #include "autoware/universe_utils/ros/marker_helper.hpp" -#include "autoware/universe_utils/ros/parameter.hpp" #include "autoware/universe_utils/ros/update_param.hpp" #include @@ -41,29 +40,804 @@ #include #include +namespace +{ +VelocityLimitClearCommand createVelocityLimitClearCommandMessage( + const rclcpp::Time & current_time, const std::string & module_name) +{ + VelocityLimitClearCommand msg; + msg.stamp = current_time; + msg.sender = "obstacle_cruise_planner." + module_name; + msg.command = true; + return msg; +} + +template +std::optional getObstacleFromUuid( + const std::vector & obstacles, const std::string & target_uuid) +{ + const auto itr = std::find_if(obstacles.begin(), obstacles.end(), [&](const auto & obstacle) { + return obstacle.uuid == target_uuid; + }); + + if (itr == obstacles.end()) { + return std::nullopt; + } + return *itr; +} + +std::optional calcDistanceToFrontVehicle( + const std::vector & traj_points, const size_t ego_idx, + const geometry_msgs::msg::Point & obstacle_pos) +{ + const size_t obstacle_idx = autoware::motion_utils::findNearestIndex(traj_points, obstacle_pos); + const auto ego_to_obstacle_distance = + autoware::motion_utils::calcSignedArcLength(traj_points, ego_idx, obstacle_idx); + if (ego_to_obstacle_distance < 0.0) return std::nullopt; + return ego_to_obstacle_distance; +} + +std::vector resampleHighestConfidencePredictedPaths( + const std::vector & predicted_paths, const double time_interval, + const double time_horizon, const size_t num_paths) +{ + std::vector sorted_paths = predicted_paths; + + // Sort paths by descending confidence + std::sort( + sorted_paths.begin(), sorted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence > b.confidence; }); + + std::vector selected_paths; + size_t path_count = 0; + + // Select paths that meet the confidence thresholds + for (const auto & path : sorted_paths) { + if (path_count < num_paths) { + selected_paths.push_back(path); + ++path_count; + } + } + + // Resample each selected path + std::vector resampled_paths; + for (const auto & path : selected_paths) { + if (path.path.size() < 2) { + continue; + } + resampled_paths.push_back( + autoware::object_recognition_utils::resamplePredictedPath(path, time_interval, time_horizon)); + } + + return resampled_paths; +} + +double calcDiffAngleAgainstTrajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & target_pose) +{ + const size_t nearest_idx = + autoware::motion_utils::findNearestIndex(traj_points, target_pose.position); + const double traj_yaw = tf2::getYaw(traj_points.at(nearest_idx).pose.orientation); + + const double target_yaw = tf2::getYaw(target_pose.orientation); + + const double diff_yaw = autoware::universe_utils::normalizeRadian(target_yaw - traj_yaw); + return diff_yaw; +} + +/** + * @brief Calculates the obstacle's longitudinal and approach velocities relative to the trajectory. + * + * This function calculates the obstacle's velocity components relative to the trajectory. + * It returns the longitudinal and approach components of the obstacle's velocity + * with respect to the trajectory. Negative approach velocity indicates that the + * obstacle is getting far away from the trajectory. + * + * @param traj_points The trajectory points. + * @param obstacle_pose The current pose of the obstacle. + * @param obstacle_twist The twist (velocity) of the obstacle. + * @return A pair containing the longitudinal and approach velocity components. + */ +std::pair calculateObstacleVelocitiesRelativeToTrajectory( + const std::vector & traj_points, const geometry_msgs::msg::Pose & obstacle_pose, + const geometry_msgs::msg::Twist & obstacle_twist) +{ + const size_t object_idx = + autoware::motion_utils::findNearestIndex(traj_points, obstacle_pose.position); + + const auto & nearest_point = traj_points.at(object_idx); + + const double traj_yaw = tf2::getYaw(nearest_point.pose.orientation); + const double obstacle_yaw = tf2::getYaw(obstacle_pose.orientation); + const Eigen::Rotation2Dd R_ego_to_obstacle( + autoware::universe_utils::normalizeRadian(obstacle_yaw - traj_yaw)); + + // Calculate the trajectory direction and the vector from the trajectory to the obstacle + const Eigen::Vector2d traj_direction(std::cos(traj_yaw), std::sin(traj_yaw)); + const Eigen::Vector2d traj_to_obstacle( + obstacle_pose.position.x - nearest_point.pose.position.x, + obstacle_pose.position.y - nearest_point.pose.position.y); + + // Determine if the obstacle is to the left or right of the trajectory using the cross product + const double cross_product = + traj_direction.x() * traj_to_obstacle.y() - traj_direction.y() * traj_to_obstacle.x(); + const int sign = (cross_product > 0) ? -1 : 1; + + const Eigen::Vector2d obstacle_velocity(obstacle_twist.linear.x, obstacle_twist.linear.y); + const Eigen::Vector2d projected_velocity = R_ego_to_obstacle * obstacle_velocity; + + return std::make_pair(projected_velocity[0], sign * projected_velocity[1]); +} + +double calcObstacleMaxLength(const Shape & shape) +{ + if (shape.type == Shape::BOUNDING_BOX) { + return std::hypot(shape.dimensions.x / 2.0, shape.dimensions.y / 2.0); + } else if (shape.type == Shape::CYLINDER) { + return shape.dimensions.x / 2.0; + } else if (shape.type == Shape::POLYGON) { + double max_length_to_point = 0.0; + for (const auto rel_point : shape.footprint.points) { + const double length_to_point = std::hypot(rel_point.x, rel_point.y); + if (max_length_to_point < length_to_point) { + max_length_to_point = length_to_point; + } + } + return max_length_to_point; + } + + throw std::logic_error("The shape type is not supported in obstacle_cruise_planner."); +} + +TrajectoryPoint getExtendTrajectoryPoint( + const double extend_distance, const TrajectoryPoint & goal_point, const bool is_driving_forward) +{ + TrajectoryPoint extend_trajectory_point; + extend_trajectory_point.pose = autoware::universe_utils::calcOffsetPose( + goal_point.pose, extend_distance * (is_driving_forward ? 1.0 : -1.0), 0.0, 0.0); + extend_trajectory_point.longitudinal_velocity_mps = goal_point.longitudinal_velocity_mps; + extend_trajectory_point.lateral_velocity_mps = goal_point.lateral_velocity_mps; + extend_trajectory_point.acceleration_mps2 = goal_point.acceleration_mps2; + return extend_trajectory_point; +} + +std::vector extendTrajectoryPoints( + const std::vector & input_points, const double extend_distance, + const double step_length) +{ + auto output_points = input_points; + const auto is_driving_forward_opt = + autoware::motion_utils::isDrivingForwardWithTwist(input_points); + const bool is_driving_forward = is_driving_forward_opt ? *is_driving_forward_opt : true; + + if (extend_distance < std::numeric_limits::epsilon()) { + return output_points; + } + + const auto goal_point = input_points.back(); + + double extend_sum = 0.0; + while (extend_sum <= (extend_distance - step_length)) { + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_sum, goal_point, is_driving_forward); + output_points.push_back(extend_trajectory_point); + extend_sum += step_length; + } + const auto extend_trajectory_point = + getExtendTrajectoryPoint(extend_distance, goal_point, is_driving_forward); + output_points.push_back(extend_trajectory_point); + + return output_points; +} + +std::vector getTargetObjectType(rclcpp::Node & node, const std::string & param_prefix) +{ + std::unordered_map types_map{ + {"unknown", ObjectClassification::UNKNOWN}, {"car", ObjectClassification::CAR}, + {"truck", ObjectClassification::TRUCK}, {"bus", ObjectClassification::BUS}, + {"trailer", ObjectClassification::TRAILER}, {"motorcycle", ObjectClassification::MOTORCYCLE}, + {"bicycle", ObjectClassification::BICYCLE}, {"pedestrian", ObjectClassification::PEDESTRIAN}}; + + std::vector types; + for (const auto & type : types_map) { + if (node.declare_parameter(param_prefix + type.first)) { + types.push_back(type.second); + } + } + return types; +} + +std::vector resampleTrajectoryPoints( + const std::vector & traj_points, const double interval) +{ + const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware::motion_utils::resampleTrajectory(traj, interval); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +geometry_msgs::msg::Point toGeomPoint(const pcl::PointXYZ & point) +{ + geometry_msgs::msg::Point geom_point; + geom_point.x = point.x; + geom_point.y = point.y; + geom_point.z = point.z; + return geom_point; +} + +geometry_msgs::msg::Point toGeomPoint(const autoware::universe_utils::Point2d & point) +{ + geometry_msgs::msg::Point geom_point; + geom_point.x = point.x(); + geom_point.y = point.y(); + return geom_point; +} + +bool isLowerConsideringHysteresis( + const double current_val, const bool was_low, const double high_val, const double low_val) +{ + if (was_low) { + if (high_val < current_val) { + return false; + } + return true; + } + if (current_val < low_val) { + return true; + } + return false; +} + +template +void concatenate(std::vector & first, const std::vector & last) +{ + first.insert(first.end(), last.begin(), last.end()); +} +} // namespace + namespace autoware::motion_planning { -using autoware::universe_utils::getOrDeclareParameter; +ObstacleCruisePlannerNode::BehaviorDeterminationParam::BehaviorDeterminationParam( + rclcpp::Node & node) +{ // behavior determination + decimate_trajectory_step_length = + node.declare_parameter("behavior_determination.decimate_trajectory_step_length"); + pointcloud_search_radius = + node.declare_parameter("behavior_determination.pointcloud_search_radius"); + pointcloud_voxel_grid_x = + node.declare_parameter("behavior_determination.pointcloud_voxel_grid_x"); + pointcloud_voxel_grid_y = + node.declare_parameter("behavior_determination.pointcloud_voxel_grid_y"); + pointcloud_voxel_grid_z = + node.declare_parameter("behavior_determination.pointcloud_voxel_grid_z"); + pointcloud_cluster_tolerance = + node.declare_parameter("behavior_determination.pointcloud_cluster_tolerance"); + pointcloud_min_cluster_size = + node.declare_parameter("behavior_determination.pointcloud_min_cluster_size"); + pointcloud_max_cluster_size = + node.declare_parameter("behavior_determination.pointcloud_max_cluster_size"); + obstacle_velocity_threshold_from_cruise_to_stop = node.declare_parameter( + "behavior_determination.obstacle_velocity_threshold_from_cruise_to_stop"); + obstacle_velocity_threshold_from_stop_to_cruise = node.declare_parameter( + "behavior_determination.obstacle_velocity_threshold_from_stop_to_cruise"); + crossing_obstacle_velocity_threshold = node.declare_parameter( + "behavior_determination.crossing_obstacle.obstacle_velocity_threshold"); + crossing_obstacle_traj_angle_threshold = node.declare_parameter( + "behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold"); + collision_time_margin = node.declare_parameter( + "behavior_determination.stop.crossing_obstacle.collision_time_margin"); + outside_obstacle_min_velocity_threshold = node.declare_parameter( + "behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold"); + ego_obstacle_overlap_time_threshold = node.declare_parameter( + "behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold"); + max_prediction_time_for_collision_check = node.declare_parameter( + "behavior_determination.cruise.outside_obstacle.max_prediction_time_for_collision_check"); + stop_obstacle_hold_time_threshold = + node.declare_parameter("behavior_determination.stop_obstacle_hold_time_threshold"); + prediction_resampling_time_interval = + node.declare_parameter("behavior_determination.prediction_resampling_time_interval"); + prediction_resampling_time_horizon = + node.declare_parameter("behavior_determination.prediction_resampling_time_horizon"); + + max_lat_margin_for_stop = + node.declare_parameter("behavior_determination.stop.max_lat_margin"); + max_lat_margin_for_stop_against_unknown = + node.declare_parameter("behavior_determination.stop.max_lat_margin_against_unknown"); + max_lat_margin_for_cruise = + node.declare_parameter("behavior_determination.cruise.max_lat_margin"); + enable_yield = node.declare_parameter("behavior_determination.cruise.yield.enable_yield"); + yield_lat_distance_threshold = + node.declare_parameter("behavior_determination.cruise.yield.lat_distance_threshold"); + max_lat_dist_between_obstacles = node.declare_parameter( + "behavior_determination.cruise.yield.max_lat_dist_between_obstacles"); + stopped_obstacle_velocity_threshold = node.declare_parameter( + "behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold"); + max_obstacles_collision_time = node.declare_parameter( + "behavior_determination.cruise.yield.max_obstacles_collision_time"); + max_lat_margin_for_slow_down = + node.declare_parameter("behavior_determination.slow_down.max_lat_margin"); + lat_hysteresis_margin_for_slow_down = + node.declare_parameter("behavior_determination.slow_down.lat_hysteresis_margin"); + successive_num_to_entry_slow_down_condition = node.declare_parameter( + "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition"); + successive_num_to_exit_slow_down_condition = node.declare_parameter( + "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition"); + enable_to_consider_current_pose = node.declare_parameter( + "behavior_determination.consider_current_pose.enable_to_consider_current_pose"); + time_to_convergence = node.declare_parameter( + "behavior_determination.consider_current_pose.time_to_convergence"); + min_velocity_to_reach_collision_point = node.declare_parameter( + "behavior_determination.stop.min_velocity_to_reach_collision_point"); + max_lat_time_margin_for_stop = node.declare_parameter( + "behavior_determination.stop.outside_obstacle.max_lateral_time_margin"); + max_lat_time_margin_for_cruise = node.declare_parameter( + "behavior_determination.cruise.outside_obstacle.max_lateral_time_margin"); + num_of_predicted_paths_for_outside_cruise_obstacle = node.declare_parameter( + "behavior_determination.cruise.outside_obstacle.num_of_predicted_paths"); + num_of_predicted_paths_for_outside_stop_obstacle = node.declare_parameter( + "behavior_determination.stop.outside_obstacle.num_of_predicted_paths"); + pedestrian_deceleration_rate = node.declare_parameter( + "behavior_determination.stop.outside_obstacle.pedestrian_deceleration_rate"); + bicycle_deceleration_rate = node.declare_parameter( + "behavior_determination.stop.outside_obstacle.bicycle_deceleration_rate"); +} + +void ObstacleCruisePlannerNode::BehaviorDeterminationParam::onParam( + const std::vector & parameters) +{ + // behavior determination + autoware::universe_utils::updateParam( + parameters, "behavior_determination.decimate_trajectory_step_length", + decimate_trajectory_step_length); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_search_radius", pointcloud_search_radius); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_voxel_grid_x", pointcloud_voxel_grid_x); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_voxel_grid_y", pointcloud_voxel_grid_y); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_voxel_grid_z", pointcloud_voxel_grid_z); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_cluster_tolerance", + pointcloud_cluster_tolerance); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_min_cluster_size", pointcloud_min_cluster_size); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.pointcloud_max_cluster_size", pointcloud_max_cluster_size); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.crossing_obstacle.obstacle_velocity_threshold", + crossing_obstacle_velocity_threshold); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.crossing_obstacle.obstacle_traj_angle_threshold", + crossing_obstacle_traj_angle_threshold); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.crossing_obstacle.collision_time_margin", + collision_time_margin); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.cruise.outside_obstacle.obstacle_velocity_threshold", + outside_obstacle_min_velocity_threshold); + autoware::universe_utils::updateParam( + parameters, + "behavior_determination.cruise.outside_obstacle.ego_obstacle_overlap_time_threshold", + ego_obstacle_overlap_time_threshold); + autoware::universe_utils::updateParam( + parameters, + "behavior_determination.cruise.outside_obstacle.max_prediction_time_for_collision_check", + max_prediction_time_for_collision_check); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop_obstacle_hold_time_threshold", + stop_obstacle_hold_time_threshold); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.prediction_resampling_time_interval", + prediction_resampling_time_interval); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.prediction_resampling_time_horizon", + prediction_resampling_time_horizon); + + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.max_lat_margin", max_lat_margin_for_stop); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.max_lat_margin_against_unknown", + max_lat_margin_for_stop_against_unknown); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.cruise.max_lat_margin", max_lat_margin_for_cruise); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.cruise.yield.enable_yield", enable_yield); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.cruise.yield.lat_distance_threshold", + yield_lat_distance_threshold); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.cruise.yield.max_lat_dist_between_obstacles", + max_lat_dist_between_obstacles); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.cruise.yield.stopped_obstacle_velocity_threshold", + stopped_obstacle_velocity_threshold); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.cruise.yield.max_obstacles_collision_time", + max_obstacles_collision_time); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.slow_down.max_lat_margin", max_lat_margin_for_slow_down); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.slow_down.lat_hysteresis_margin", + lat_hysteresis_margin_for_slow_down); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.slow_down.successive_num_to_entry_slow_down_condition", + successive_num_to_entry_slow_down_condition); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.slow_down.successive_num_to_exit_slow_down_condition", + successive_num_to_exit_slow_down_condition); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.consider_current_pose.enable_to_consider_current_pose", + enable_to_consider_current_pose); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.consider_current_pose.time_to_convergence", + time_to_convergence); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.min_velocity_to_reach_collision_point", + min_velocity_to_reach_collision_point); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.outside_obstacle.max_lateral_time_margin", + max_lat_time_margin_for_stop); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.cruise.outside_obstacle.max_lateral_time_margin", + max_lat_time_margin_for_cruise); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.cruise.outside_obstacle.num_of_predicted_paths", + num_of_predicted_paths_for_outside_cruise_obstacle); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.outside_obstacle.num_of_predicted_paths", + num_of_predicted_paths_for_outside_stop_obstacle); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.outside_obstacle.pedestrian_deceleration_rate", + pedestrian_deceleration_rate); + autoware::universe_utils::updateParam( + parameters, "behavior_determination.stop.outside_obstacle.bicycle_deceleration_rate", + bicycle_deceleration_rate); +} + +ObstacleCruisePlannerNode::ObstacleCruisePlannerNode(const rclcpp::NodeOptions & node_options) +: Node("obstacle_cruise_planner", node_options), + vehicle_info_(autoware::vehicle_info_utils::VehicleInfoUtils(*this).getVehicleInfo()), + debug_data_ptr_(std::make_shared()) +{ + using std::placeholders::_1; + + // subscriber + traj_sub_ = create_subscription( + "~/input/trajectory", rclcpp::QoS{1}, + std::bind(&ObstacleCruisePlannerNode::onTrajectory, this, _1)); + + // publisher + trajectory_pub_ = create_publisher("~/output/trajectory", 1); + vel_limit_pub_ = + create_publisher("~/output/velocity_limit", rclcpp::QoS{1}.transient_local()); + clear_vel_limit_pub_ = create_publisher( + "~/output/clear_velocity_limit", rclcpp::QoS{1}.transient_local()); + + // debug publisher + debug_calculation_time_pub_ = create_publisher("~/debug/processing_time_ms", 1); + debug_cruise_wall_marker_pub_ = create_publisher("~/virtual_wall/cruise", 1); + debug_stop_wall_marker_pub_ = create_publisher("~/virtual_wall/stop", 1); + debug_slow_down_wall_marker_pub_ = create_publisher("~/virtual_wall/slow_down", 1); + debug_marker_pub_ = create_publisher("~/debug/marker", 1); + debug_stop_planning_info_pub_ = + create_publisher("~/debug/stop_planning_info", 1); + debug_cruise_planning_info_pub_ = + create_publisher("~/debug/cruise_planning_info", 1); + debug_slow_down_planning_info_pub_ = + create_publisher("~/debug/slow_down_planning_info", 1); + + // tf listener + tf_buffer_ = std::make_unique(get_clock()); + tf_listener_ = std::make_shared(*tf_buffer_); + + const auto longitudinal_info = LongitudinalInfo(*this); + + ego_nearest_param_ = EgoNearestParam(*this); + + enable_debug_info_ = declare_parameter("common.enable_debug_info"); + enable_calculation_time_info_ = declare_parameter("common.enable_calculation_time_info"); + enable_slow_down_planning_ = declare_parameter("common.enable_slow_down_planning"); + + use_pointcloud_for_stop_ = declare_parameter("common.stop_obstacle_type.pointcloud"); + use_pointcloud_for_slow_down_ = + declare_parameter("common.slow_down_obstacle_type.pointcloud"); + use_pointcloud_ = use_pointcloud_for_stop_ || use_pointcloud_for_slow_down_; + + behavior_determination_param_ = BehaviorDeterminationParam(*this); + + { // planning algorithm + const std::string planning_algorithm_param = + declare_parameter("common.planning_algorithm"); + planning_algorithm_ = getPlanningAlgorithmType(planning_algorithm_param); + + if (planning_algorithm_ == PlanningAlgorithm::OPTIMIZATION_BASE) { + planner_ptr_ = std::make_unique( + *this, longitudinal_info, vehicle_info_, ego_nearest_param_, debug_data_ptr_); + } else if (planning_algorithm_ == PlanningAlgorithm::PID_BASE) { + planner_ptr_ = std::make_unique( + *this, longitudinal_info, vehicle_info_, ego_nearest_param_, debug_data_ptr_); + } else { + throw std::logic_error("Designated algorithm is not supported."); + } + + min_behavior_stop_margin_ = declare_parameter("common.min_behavior_stop_margin"); + additional_safe_distance_margin_on_curve_ = + declare_parameter("common.stop_on_curve.additional_safe_distance_margin"); + enable_approaching_on_curve_ = + declare_parameter("common.stop_on_curve.enable_approaching"); + min_safe_distance_margin_on_curve_ = + declare_parameter("common.stop_on_curve.min_safe_distance_margin"); + suppress_sudden_obstacle_stop_ = + declare_parameter("common.suppress_sudden_obstacle_stop"); + planner_ptr_->setParam( + enable_debug_info_, enable_calculation_time_info_, use_pointcloud_, min_behavior_stop_margin_, + enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); + } + + { // stop/cruise/slow down obstacle type + inside_stop_obstacle_types_ = getTargetObjectType(*this, "common.stop_obstacle_type.inside."); + outside_stop_obstacle_types_ = getTargetObjectType(*this, "common.stop_obstacle_type.outside."); + inside_cruise_obstacle_types_ = + getTargetObjectType(*this, "common.cruise_obstacle_type.inside."); + outside_cruise_obstacle_types_ = + getTargetObjectType(*this, "common.cruise_obstacle_type.outside."); + slow_down_obstacle_types_ = getTargetObjectType(*this, "common.slow_down_obstacle_type."); + } + + // set parameter callback + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&ObstacleCruisePlannerNode::onParam, this, std::placeholders::_1)); + + logger_configure_ = std::make_unique(this); + published_time_publisher_ = + std::make_unique(this); +} + +ObstacleCruisePlannerNode::PlanningAlgorithm ObstacleCruisePlannerNode::getPlanningAlgorithmType( + const std::string & param) const +{ + if (param == "pid_base") { + return PlanningAlgorithm::PID_BASE; + } else if (param == "optimization_base") { + return PlanningAlgorithm::OPTIMIZATION_BASE; + } + return PlanningAlgorithm::INVALID; +} + +rcl_interfaces::msg::SetParametersResult ObstacleCruisePlannerNode::onParam( + const std::vector & parameters) +{ + planner_ptr_->onParam(parameters); + + autoware::universe_utils::updateParam( + parameters, "common.enable_debug_info", enable_debug_info_); + autoware::universe_utils::updateParam( + parameters, "common.enable_calculation_time_info", enable_calculation_time_info_); + + autoware::universe_utils::updateParam( + parameters, "common.stop_on_curve.enable_approaching", enable_approaching_on_curve_); + autoware::universe_utils::updateParam( + parameters, "common.stop_on_curve.additional_safe_distance_margin", + additional_safe_distance_margin_on_curve_); + autoware::universe_utils::updateParam( + parameters, "common.stop_on_curve.min_safe_distance_margin", + min_safe_distance_margin_on_curve_); + + planner_ptr_->setParam( + enable_debug_info_, enable_calculation_time_info_, use_pointcloud_, min_behavior_stop_margin_, + enable_approaching_on_curve_, additional_safe_distance_margin_on_curve_, + min_safe_distance_margin_on_curve_, suppress_sudden_obstacle_stop_); + + autoware::universe_utils::updateParam( + parameters, "common.enable_slow_down_planning", enable_slow_down_planning_); + + behavior_determination_param_.onParam(parameters); + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + return result; +} + +void ObstacleCruisePlannerNode::onTrajectory(const Trajectory::ConstSharedPtr msg) +{ + const auto ego_odom_ptr = ego_odom_sub_.takeData(); + const auto objects_ptr = objects_sub_.takeData(); + const auto pointcloud_ptr = use_pointcloud_ ? pointcloud_sub_.takeData() : nullptr; + const auto acc_ptr = acc_sub_.takeData(); + const bool can_detect_obstacles = objects_ptr || pointcloud_ptr; + if (!ego_odom_ptr || !can_detect_obstacles || !acc_ptr) { + return; + } + + const auto & ego_odom = *ego_odom_ptr; + const auto & acc = *acc_ptr; + + const auto traj_points = autoware::motion_utils::convertToTrajectoryPointArray(*msg); + // check if subscribed variables are ready + if (traj_points.empty()) { + return; + } + + stop_watch_.tic(__func__); + *debug_data_ptr_ = DebugData(); + + const auto is_driving_forward = autoware::motion_utils::isDrivingForwardWithTwist(traj_points); + is_driving_forward_ = is_driving_forward ? is_driving_forward.value() : is_driving_forward_; + + const auto & [stop_obstacles, cruise_obstacles, slow_down_obstacles] = [&]() { + std::vector stop_obstacles; + std::vector cruise_obstacles; + std::vector slow_down_obstacles; + if (objects_ptr) { + // 1. Convert predicted objects to obstacles which are + // (1) with a proper label + // (2) in front of ego + // (3) not too far from trajectory + const auto target_obstacles = convertToObstacles(ego_odom, *objects_ptr, traj_points); + + // 2. Determine ego's behavior against each obstacle from stop, cruise and slow down. + const auto & [stop_object_obstacles, cruise_object_obstacles, slow_down_object_obstacles] = + determineEgoBehaviorAgainstPredictedObjectObstacles( + ego_odom, *objects_ptr, traj_points, target_obstacles); + + concatenate(stop_obstacles, stop_object_obstacles); + concatenate(cruise_obstacles, cruise_object_obstacles); + concatenate(slow_down_obstacles, slow_down_object_obstacles); + } + if (pointcloud_ptr) { + const auto target_obstacles = + convertToObstacles(ego_odom, *pointcloud_ptr, traj_points, msg->header); + + const auto & [stop_pc_obstacles, cruise_pc_obstacles, slow_down_pc_obstacles] = + determineEgoBehaviorAgainstPointCloudObstacles(ego_odom, traj_points, target_obstacles); + + concatenate(stop_obstacles, stop_pc_obstacles); + concatenate(cruise_obstacles, cruise_pc_obstacles); + concatenate(slow_down_obstacles, slow_down_pc_obstacles); + } + return std::make_tuple(stop_obstacles, cruise_obstacles, slow_down_obstacles); + }(); + + // 3. Create data for planning + const auto planner_data = createPlannerData(ego_odom, acc, traj_points); + + // 4. Stop planning + const auto stop_traj_points = planner_ptr_->generateStopTrajectory(planner_data, stop_obstacles); + + // 5. Cruise planning + std::optional cruise_vel_limit; + const auto cruise_traj_points = planner_ptr_->generateCruiseTrajectory( + planner_data, stop_traj_points, cruise_obstacles, cruise_vel_limit); + publishVelocityLimit(cruise_vel_limit, "cruise"); + + // 6. Slow down planning + std::optional slow_down_vel_limit; + const auto slow_down_traj_points = planner_ptr_->generateSlowDownTrajectory( + planner_data, cruise_traj_points, slow_down_obstacles, slow_down_vel_limit); + publishVelocityLimit(slow_down_vel_limit, "slow_down"); + + // 7. Publish trajectory + const auto output_traj = + autoware::motion_utils::convertToTrajectory(slow_down_traj_points, msg->header); + trajectory_pub_->publish(output_traj); + + // 8. Publish debug data + published_time_publisher_->publish_if_subscribed(trajectory_pub_, output_traj.header.stamp); + planner_ptr_->publishMetrics(now()); + planner_ptr_->publishPlanningFactors(); + publishDebugMarker(); + publishDebugInfo(); + objects_of_interest_marker_interface_.publishMarkerArray(); + + // 9. Publish and print calculation time + const double calculation_time = stop_watch_.toc(__func__); + publishCalculationTime(calculation_time); + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_calculation_time_info_, "%s := %f [ms]", __func__, calculation_time); +} + +std::vector ObstacleCruisePlannerNode::createOneStepPolygons( + const std::vector & traj_points, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const geometry_msgs::msg::Pose & current_ego_pose, const double lat_margin) const +{ + const auto & p = behavior_determination_param_; + + const double front_length = vehicle_info.max_longitudinal_offset_m; + const double rear_length = vehicle_info.rear_overhang_m; + const double vehicle_width = vehicle_info.vehicle_width_m; + + const size_t nearest_idx = + autoware::motion_utils::findNearestSegmentIndex(traj_points, current_ego_pose.position); + const auto nearest_pose = traj_points.at(nearest_idx).pose; + const auto current_ego_pose_error = + autoware::universe_utils::inverseTransformPose(current_ego_pose, nearest_pose); + const double current_ego_lat_error = current_ego_pose_error.position.y; + const double current_ego_yaw_error = tf2::getYaw(current_ego_pose_error.orientation); + double time_elapsed{0.0}; + + std::vector output_polygons; + Polygon2d tmp_polys{}; + for (size_t i = 0; i < traj_points.size(); ++i) { + std::vector current_poses = {traj_points.at(i).pose}; + + // estimate the future ego pose with assuming that the pose error against the reference path + // will decrease to zero by the time_to_convergence + if (p.enable_to_consider_current_pose && time_elapsed < p.time_to_convergence) { + const double rem_ratio = (p.time_to_convergence - time_elapsed) / p.time_to_convergence; + geometry_msgs::msg::Pose indexed_pose_err; + indexed_pose_err.set__orientation( + autoware::universe_utils::createQuaternionFromYaw(current_ego_yaw_error * rem_ratio)); + indexed_pose_err.set__position( + autoware::universe_utils::createPoint(0.0, current_ego_lat_error * rem_ratio, 0.0)); + current_poses.push_back( + autoware::universe_utils::transformPose(indexed_pose_err, traj_points.at(i).pose)); + if (traj_points.at(i).longitudinal_velocity_mps != 0.0) { + time_elapsed += + p.decimate_trajectory_step_length / std::abs(traj_points.at(i).longitudinal_velocity_mps); + } else { + time_elapsed = std::numeric_limits::max(); + } + } + + Polygon2d idx_poly{}; + for (const auto & pose : current_poses) { + if (i == 0 && traj_points.at(i).longitudinal_velocity_mps > 1e-3) { + boost::geometry::append( + idx_poly, + autoware::universe_utils::toFootprint(pose, front_length, rear_length, vehicle_width) + .outer()); + boost::geometry::append( + idx_poly, autoware::universe_utils::fromMsg( + autoware::universe_utils::calcOffsetPose( + pose, front_length, vehicle_width * 0.5 + lat_margin, 0.0) + .position) + .to_2d()); + boost::geometry::append( + idx_poly, autoware::universe_utils::fromMsg( + autoware::universe_utils::calcOffsetPose( + pose, front_length, -vehicle_width * 0.5 - lat_margin, 0.0) + .position) + .to_2d()); + } else { + boost::geometry::append( + idx_poly, autoware::universe_utils::toFootprint( + pose, front_length, rear_length, vehicle_width + lat_margin * 2.0) + .outer()); + } + } + + boost::geometry::append(tmp_polys, idx_poly.outer()); + Polygon2d hull_polygon; + boost::geometry::convex_hull(tmp_polys, hull_polygon); + boost::geometry::correct(hull_polygon); + + output_polygons.push_back(hull_polygon); + tmp_polys = std::move(idx_poly); + } + return output_polygons; +} -std::vector -ObstacleCruisePlannerNode::convertToPredictedObjectBasedObstacles( +std::vector ObstacleCruisePlannerNode::convertToObstacles( const Odometry & odometry, const PredictedObjects & objects, - const std::vector & traj_points, const PlannerData & planner_data) const + const std::vector & traj_points) const { + stop_watch_.tic(__func__); + const auto obj_stamp = rclcpp::Time(objects.header.stamp); - // const auto & p = common_behavior_determination_param_; + const auto & p = behavior_determination_param_; constexpr double epsilon = 1e-6; - const double max_lat_margin = 3.0; - const double max_lat_time_margin = 3.0; - // const double max_lat_margin = std::max( - // {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown, - // p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); - // const double max_lat_time_margin = - // std::max({p.max_lat_time_margin_for_stop, p.max_lat_time_margin_for_cruise}); - const size_t ego_idx = planner_data.findIndex(traj_points, odometry.pose.pose); - - std::vector target_obstacles; + const double max_lat_margin = std::max( + {p.max_lat_margin_for_stop, p.max_lat_margin_for_stop_against_unknown, + p.max_lat_margin_for_cruise, p.max_lat_margin_for_slow_down}); + const double max_lat_time_margin = + std::max({p.max_lat_time_margin_for_stop, p.max_lat_time_margin_for_cruise}); + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); + + std::vector target_obstacles; for (const auto & predicted_object : objects.objects) { const auto & object_id = autoware::universe_utils::toHexString(predicted_object.object_id).substr(0, 4); @@ -73,18 +847,38 @@ ObstacleCruisePlannerNode::convertToPredictedObjectBasedObstacles( const auto & current_obstacle_pose = obstacle_cruise_utils::getCurrentObjectPose(predicted_object, obj_stamp, now(), true); + // 1. Check if the obstacle's label is target + const uint8_t label = predicted_object.classification.front().label; + const bool is_target_obstacle = + isStopObstacle(label) || isCruiseObstacle(label) || isSlowDownObstacle(label); + if (!is_target_obstacle) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, "Ignore obstacle (%s) since its label is not target.", + object_id.c_str()); + continue; + } + const auto projected_vel = calculateObstacleVelocitiesRelativeToTrajectory( traj_points, current_obstacle_pose.pose, predicted_object.kinematics.initial_twist_with_covariance.twist); + // 2. Check if the obstacle is in front of the ego. + const auto ego_to_obstacle_distance = + calcDistanceToFrontVehicle(traj_points, ego_idx, current_obstacle_pose.pose.position); + if (!ego_to_obstacle_distance) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, "Ignore obstacle (%s) since it is not front obstacle.", + object_id.c_str()); + continue; + } + // 3. Check if rough lateral distance and time to reach trajectory are smaller than the // threshold const double lat_dist_from_obstacle_to_traj = autoware::motion_utils::calcLateralOffset(traj_points, current_obstacle_pose.pose.position); const double min_lat_dist_to_traj_poly = [&]() { - const double obstacle_max_length = - obstacle_cruise_utils::calcObstacleMaxLength(predicted_object.shape); + const double obstacle_max_length = calcObstacleMaxLength(predicted_object.shape); return std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m - obstacle_max_length; }(); @@ -106,35 +900,1371 @@ ObstacleCruisePlannerNode::convertToPredictedObjectBasedObstacles( continue; } } + + const auto target_obstacle = Obstacle( + obj_stamp, predicted_object, current_obstacle_pose.pose, *ego_to_obstacle_distance, + lat_dist_from_obstacle_to_traj, projected_vel.first, projected_vel.second); + target_obstacles.push_back(target_obstacle); } + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), enable_debug_info_, " %s := %f [ms]", __func__, + calculation_time); + return target_obstacles; } -void ObstacleCruisePlannerNode::publishDebugMarker() const +std::vector ObstacleCruisePlannerNode::convertToObstacles( + const Odometry & odometry, const PointCloud2 & pointcloud, + const std::vector & traj_points, const std_msgs::msg::Header & traj_header) const +{ + stop_watch_.tic(__func__); + + const auto & p = behavior_determination_param_; + + std::vector target_obstacles; + + std::optional transform_stamped{}; + try { + transform_stamped = tf_buffer_->lookupTransform( + traj_header.frame_id, pointcloud.header.frame_id, pointcloud.header.stamp, + rclcpp::Duration::from_seconds(0.5)); + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR_STREAM( + get_logger(), "Failed to look up transform from " << traj_header.frame_id << " to " + << pointcloud.header.frame_id); + transform_stamped = std::nullopt; + } + + if (!pointcloud.data.empty() && transform_stamped) { + // 1. transform pointcloud + PointCloud::Ptr pointcloud_ptr(new PointCloud); + pcl::fromROSMsg(pointcloud, *pointcloud_ptr); + const Eigen::Matrix4f transform = + tf2::transformToEigen(transform_stamped.value().transform).matrix().cast(); + pcl::transformPointCloud(*pointcloud_ptr, *pointcloud_ptr, transform); + + // 2. downsample & cluster pointcloud + PointCloud::Ptr filtered_points_ptr(new PointCloud); + pcl::VoxelGrid filter; + filter.setInputCloud(pointcloud_ptr); + filter.setLeafSize( + p.pointcloud_voxel_grid_x, p.pointcloud_voxel_grid_y, p.pointcloud_voxel_grid_z); + filter.filter(*filtered_points_ptr); + + pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); + tree->setInputCloud(filtered_points_ptr); + std::vector clusters; + pcl::EuclideanClusterExtraction ec; + ec.setClusterTolerance(p.pointcloud_cluster_tolerance); + ec.setMinClusterSize(p.pointcloud_min_cluster_size); + ec.setMaxClusterSize(p.pointcloud_max_cluster_size); + ec.setSearchMethod(tree); + ec.setInputCloud(filtered_points_ptr); + ec.extract(clusters); + + const auto max_lat_margin = + std::max(p.max_lat_margin_for_stop_against_unknown, p.max_lat_margin_for_slow_down); + const size_t ego_idx = ego_nearest_param_.findIndex(traj_points, odometry.pose.pose); + + // 3. convert clusters to obstacles + for (const auto & cluster_indices : clusters) { + double ego_to_stop_collision_distance = std::numeric_limits::max(); + double ego_to_slow_down_front_collision_distance = std::numeric_limits::max(); + double ego_to_slow_down_back_collision_distance = std::numeric_limits::min(); + double lat_dist_from_obstacle_to_traj = std::numeric_limits::max(); + double ego_to_obstacle_distance = std::numeric_limits::max(); + std::optional stop_collision_point = std::nullopt; + std::optional slow_down_front_collision_point = std::nullopt; + std::optional slow_down_back_collision_point = std::nullopt; + + for (const auto & index : cluster_indices.indices) { + const auto obstacle_point = toGeomPoint(filtered_points_ptr->points[index]); + const auto current_lat_dist_from_obstacle_to_traj = + autoware::motion_utils::calcLateralOffset(traj_points, obstacle_point); + const auto min_lat_dist_to_traj_poly = + std::abs(current_lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; + + if (min_lat_dist_to_traj_poly < max_lat_margin) { + const auto current_ego_to_obstacle_distance = + calcDistanceToFrontVehicle(traj_points, ego_idx, obstacle_point); + if (current_ego_to_obstacle_distance) { + ego_to_obstacle_distance = + std::min(ego_to_obstacle_distance, *current_ego_to_obstacle_distance); + } else { + continue; + } + + lat_dist_from_obstacle_to_traj = + std::min(lat_dist_from_obstacle_to_traj, current_lat_dist_from_obstacle_to_traj); + + if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) { + if (*current_ego_to_obstacle_distance < ego_to_stop_collision_distance) { + stop_collision_point = obstacle_point; + ego_to_stop_collision_distance = *current_ego_to_obstacle_distance; + } + } else if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_slow_down) { + if (*current_ego_to_obstacle_distance < ego_to_slow_down_front_collision_distance) { + slow_down_front_collision_point = obstacle_point; + ego_to_slow_down_front_collision_distance = *current_ego_to_obstacle_distance; + } else if ( + *current_ego_to_obstacle_distance > ego_to_slow_down_back_collision_distance) { + slow_down_back_collision_point = obstacle_point; + ego_to_slow_down_back_collision_distance = *current_ego_to_obstacle_distance; + } + } + } + } + + if (stop_collision_point || slow_down_front_collision_point) { + target_obstacles.emplace_back( + pointcloud.header.stamp, stop_collision_point, slow_down_front_collision_point, + slow_down_back_collision_point, ego_to_obstacle_distance, lat_dist_from_obstacle_to_traj); + } + } + } + + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), enable_debug_info_, " %s := %f [ms]", __func__, + calculation_time); + + return target_obstacles; +} + +bool ObstacleCruisePlannerNode::isInsideStopObstacle(const uint8_t label) const +{ + const auto & types = inside_stop_obstacle_types_; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +bool ObstacleCruisePlannerNode::isOutsideStopObstacle(const uint8_t label) const +{ + const auto & types = outside_stop_obstacle_types_; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +bool ObstacleCruisePlannerNode::isStopObstacle(const uint8_t label) const +{ + return isInsideStopObstacle(label) || isOutsideStopObstacle(label); +} + +bool ObstacleCruisePlannerNode::isInsideCruiseObstacle(const uint8_t label) const +{ + const auto & types = inside_cruise_obstacle_types_; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +bool ObstacleCruisePlannerNode::isOutsideCruiseObstacle(const uint8_t label) const +{ + const auto & types = outside_cruise_obstacle_types_; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +bool ObstacleCruisePlannerNode::isCruiseObstacle(const uint8_t label) const +{ + return isInsideCruiseObstacle(label) || isOutsideCruiseObstacle(label); +} + +bool ObstacleCruisePlannerNode::isSlowDownObstacle(const uint8_t label) const +{ + const auto & types = slow_down_obstacle_types_; + return std::find(types.begin(), types.end(), label) != types.end(); +} + +bool ObstacleCruisePlannerNode::isFrontCollideObstacle( + const std::vector & traj_points, const Obstacle & obstacle, + const size_t first_collision_idx) const { - // { // footprint polygons - // auto marker = autoware::universe_utils::createDefaultMarker( - // "map", now(), "detection_polygons", 0, Marker::LINE_LIST, - // autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), - // autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); - // - // for (const auto & detection_polygon : debug_data_ptr_->detection_polygons) { - // for (size_t dp_idx = 0; dp_idx < detection_polygon.outer().size(); ++dp_idx) { - // const auto & current_point = detection_polygon.outer().at(dp_idx); - // const auto & next_point = - // detection_polygon.outer().at((dp_idx + 1) % detection_polygon.outer().size()); - // - // marker.points.push_back( - // autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); - // marker.points.push_back( - // autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); - // } - // } - // debug_marker.markers.push_back(marker); - // } + const auto obstacle_idx = + autoware::motion_utils::findNearestIndex(traj_points, obstacle.pose.position); + + const double obstacle_to_col_points_distance = + autoware::motion_utils::calcSignedArcLength(traj_points, obstacle_idx, first_collision_idx); + const double obstacle_max_length = calcObstacleMaxLength(obstacle.shape); + + // If the obstacle is far in front of the collision point, the obstacle is behind the ego. + return obstacle_to_col_points_distance > -obstacle_max_length; +} + +std::tuple, std::vector, std::vector> +ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPredictedObjectObstacles( + const Odometry & odometry, const PredictedObjects & objects, + const std::vector & traj_points, const std::vector & obstacles) +{ + stop_watch_.tic(__func__); + + // calculated decimated trajectory points and trajectory polygon + const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points); + const auto decimated_traj_polys = + createOneStepPolygons(decimated_traj_points, vehicle_info_, odometry.pose.pose); + debug_data_ptr_->detection_polygons = decimated_traj_polys; + + // determine ego's behavior from stop, cruise and slow down + std::vector stop_obstacles; + std::vector cruise_obstacles; + std::vector slow_down_obstacles; + slow_down_condition_counter_.resetCurrentUuids(); + for (const auto & obstacle : obstacles) { + const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); + + // Calculate distance between trajectory and obstacle first + double precise_lat_dist = std::numeric_limits::max(); + for (const auto & traj_poly : decimated_traj_polys) { + const double current_precise_lat_dist = bg::distance(traj_poly, obstacle_poly); + precise_lat_dist = std::min(precise_lat_dist, current_precise_lat_dist); + } + + // Filter obstacles for cruise, stop and slow down + const auto cruise_obstacle = createCruiseObstacle( + odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); + if (cruise_obstacle) { + cruise_obstacles.push_back(*cruise_obstacle); + continue; + } + const auto stop_obstacle = createStopObstacleForPredictedObject( + odometry, decimated_traj_points, decimated_traj_polys, obstacle, precise_lat_dist); + if (stop_obstacle) { + stop_obstacles.push_back(*stop_obstacle); + continue; + } + const auto slow_down_obstacle = createSlowDownObstacleForPredictedObject( + odometry, decimated_traj_points, obstacle, precise_lat_dist); + if (slow_down_obstacle) { + slow_down_obstacles.push_back(*slow_down_obstacle); + continue; + } + } + const auto & p = behavior_determination_param_; + if (p.enable_yield) { + const auto yield_obstacles = findYieldCruiseObstacles(obstacles, decimated_traj_points); + if (yield_obstacles) { + for (const auto & y : yield_obstacles.value()) { + // Check if there is no member with the same UUID in cruise_obstacles + auto it = std::find_if( + cruise_obstacles.begin(), cruise_obstacles.end(), + [&y](const auto & c) { return y.uuid == c.uuid; }); + + // If no matching UUID found, insert yield obstacle into cruise_obstacles + if (it == cruise_obstacles.end()) { + cruise_obstacles.push_back(y); + } + } + } + } + slow_down_condition_counter_.removeCounterUnlessUpdated(); + + // Check target obstacles' consistency + checkConsistency(objects.header.stamp, objects, stop_obstacles); + + // update previous obstacles + prev_stop_object_obstacles_ = stop_obstacles; + prev_cruise_object_obstacles_ = cruise_obstacles; + prev_slow_down_object_obstacles_ = slow_down_obstacles; + + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]", + __func__, calculation_time); + + return {stop_obstacles, cruise_obstacles, slow_down_obstacles}; } +std::tuple, std::vector, std::vector> +ObstacleCruisePlannerNode::determineEgoBehaviorAgainstPointCloudObstacles( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & obstacles) +{ + stop_watch_.tic(__func__); + + const auto & p = behavior_determination_param_; + + // calculated decimated trajectory points and trajectory polygon + const auto decimated_traj_points = decimateTrajectoryPoints(odometry, traj_points); + const auto decimated_traj_polys = + createOneStepPolygons(decimated_traj_points, vehicle_info_, odometry.pose.pose); + debug_data_ptr_->detection_polygons = decimated_traj_polys; + + // determine ego's behavior from stop and slow down + std::vector stop_obstacles; + std::vector slow_down_obstacles; + for (const auto & obstacle : obstacles) { + const auto & precise_lat_dist = obstacle.lat_dist_from_obstacle_to_traj; + + // Filter obstacles for stop and slow down + const auto stop_obstacle = + createStopObstacleForPointCloud(decimated_traj_points, obstacle, precise_lat_dist); + if (stop_obstacle) { + stop_obstacles.push_back(*stop_obstacle); + continue; + } + const auto slow_down_obstacle = createSlowDownObstacleForPointCloud(obstacle, precise_lat_dist); + if (slow_down_obstacle) { + slow_down_obstacles.push_back(*slow_down_obstacle); + continue; + } + } + + std::vector past_stop_obstacles; + for (auto itr = stop_pc_obstacle_history_.begin(); itr != stop_pc_obstacle_history_.end();) { + const double elapsed_time = (rclcpp::Time(odometry.header.stamp) - itr->stamp).seconds(); + if (elapsed_time >= p.stop_obstacle_hold_time_threshold) { + itr = stop_pc_obstacle_history_.erase(itr); + continue; + } + + const auto lat_dist_from_obstacle_to_traj = + autoware::motion_utils::calcLateralOffset(traj_points, itr->collision_point); + const auto min_lat_dist_to_traj_poly = + std::abs(lat_dist_from_obstacle_to_traj) - vehicle_info_.vehicle_width_m; + + if (min_lat_dist_to_traj_poly < p.max_lat_margin_for_stop_against_unknown) { + auto stop_obstacle = *itr; + stop_obstacle.dist_to_collide_on_decimated_traj = autoware::motion_utils::calcSignedArcLength( + decimated_traj_points, 0, stop_obstacle.collision_point); + past_stop_obstacles.push_back(stop_obstacle); + } + + ++itr; + } + + concatenate(stop_pc_obstacle_history_, stop_obstacles); + concatenate(stop_obstacles, past_stop_obstacles); + + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]", + __func__, calculation_time); + + return {stop_obstacles, {}, slow_down_obstacles}; +} + +std::vector ObstacleCruisePlannerNode::decimateTrajectoryPoints( + const Odometry & odometry, const std::vector & traj_points) const +{ + const auto & p = behavior_determination_param_; + + // trim trajectory + const size_t ego_seg_idx = ego_nearest_param_.findSegmentIndex(traj_points, odometry.pose.pose); + const size_t traj_start_point_idx = ego_seg_idx; + const auto trimmed_traj_points = + std::vector(traj_points.begin() + traj_start_point_idx, traj_points.end()); + + // decimate trajectory + const auto decimated_traj_points = + resampleTrajectoryPoints(trimmed_traj_points, p.decimate_trajectory_step_length); + + // extend trajectory + const auto extended_traj_points = extendTrajectoryPoints( + decimated_traj_points, planner_ptr_->getSafeDistanceMargin(), + p.decimate_trajectory_step_length); + if (extended_traj_points.size() < 2) { + return traj_points; + } + return extended_traj_points; +} + +std::optional ObstacleCruisePlannerNode::createCruiseObstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const double precise_lat_dist) +{ + const auto & object_id = obstacle.uuid.substr(0, 4); + const auto & p = behavior_determination_param_; + + // NOTE: When driving backward, Stop will be planned instead of cruise. + // When the obstacle is crossing the ego's trajectory, cruise can be ignored. + if (!isCruiseObstacle(obstacle.classification.label) || !is_driving_forward_) { + return std::nullopt; + } + + if (obstacle.longitudinal_velocity < 0.0) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore obstacle (%s) since it's driving in opposite direction.", object_id.c_str()); + return std::nullopt; + } + + if (p.max_lat_margin_for_cruise < precise_lat_dist) { + const auto time_to_traj = precise_lat_dist / std::max(1e-6, obstacle.approach_velocity); + if (time_to_traj > p.max_lat_time_margin_for_cruise) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore obstacle (%s) since it's far from trajectory.", object_id.c_str()); + return std::nullopt; + } + } + + if (isObstacleCrossing(traj_points, obstacle)) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore obstacle (%s) since it's crossing the ego's trajectory..", + object_id.c_str()); + return std::nullopt; + } + + const auto collision_points = [&]() -> std::optional> { + constexpr double epsilon = 1e-6; + if (precise_lat_dist < epsilon) { + // obstacle is inside the trajectory + return createCollisionPointsForInsideCruiseObstacle(traj_points, traj_polys, obstacle); + } + // obstacle is outside the trajectory + // If the ego is stopping, do not plan cruise for outside obstacles. Stop will be planned. + if (odometry.twist.twist.linear.x < 0.1) { + return std::nullopt; + } + return createCollisionPointsForOutsideCruiseObstacle(traj_points, traj_polys, obstacle); + }(); + if (!collision_points) { + return std::nullopt; + } + + return CruiseObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.pose, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + *collision_points}; +} + +std::optional ObstacleCruisePlannerNode::createYieldCruiseObstacle( + const Obstacle & obstacle, const std::vector & traj_points) +{ + if (traj_points.empty()) return std::nullopt; + // check label + const auto & object_id = obstacle.uuid.substr(0, 4); + const auto & p = behavior_determination_param_; + + if (!isOutsideCruiseObstacle(obstacle.classification.label)) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore yield obstacle (%s) since its type is not designated.", object_id.c_str()); + return std::nullopt; + } + + if ( + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) < + p.outside_obstacle_min_velocity_threshold) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore yield obstacle (%s) since the obstacle velocity is low.", object_id.c_str()); + return std::nullopt; + } + + if (isObstacleCrossing(traj_points, obstacle)) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore yield obstacle (%s) since it's crossing the ego's trajectory..", + object_id.c_str()); + return std::nullopt; + } + + const auto collision_points = [&]() -> std::optional> { + const auto obstacle_idx = autoware::motion_utils::findNearestIndex(traj_points, obstacle.pose); + if (!obstacle_idx) return std::nullopt; + const auto collision_traj_point = traj_points.at(obstacle_idx.value()); + const auto object_time = now() + traj_points.at(obstacle_idx.value()).time_from_start; + + PointWithStamp collision_traj_point_with_stamp; + collision_traj_point_with_stamp.stamp = object_time; + collision_traj_point_with_stamp.point.x = collision_traj_point.pose.position.x; + collision_traj_point_with_stamp.point.y = collision_traj_point.pose.position.y; + collision_traj_point_with_stamp.point.z = collision_traj_point.pose.position.z; + std::vector collision_points_vector{collision_traj_point_with_stamp}; + return collision_points_vector; + }(); + + if (!collision_points) return std::nullopt; + // check if obstacle is driving on the opposite direction + if (obstacle.longitudinal_velocity < 0.0) return std::nullopt; + return CruiseObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.pose, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + collision_points.value(), + true}; +} + +std::optional> ObstacleCruisePlannerNode::findYieldCruiseObstacles( + const std::vector & obstacles, const std::vector & traj_points) +{ + if (obstacles.empty() || traj_points.empty()) return std::nullopt; + const auto & p = behavior_determination_param_; + + std::vector stopped_obstacles; + std::vector moving_obstacles; + + std::for_each( + obstacles.begin(), obstacles.end(), + [&stopped_obstacles, &moving_obstacles, &p](const auto & o) { + const bool is_moving = + std::hypot(o.twist.linear.x, o.twist.linear.y) > p.stopped_obstacle_velocity_threshold; + if (is_moving) { + const bool is_within_lat_dist_threshold = + o.lat_dist_from_obstacle_to_traj < p.yield_lat_distance_threshold; + if (is_within_lat_dist_threshold) moving_obstacles.push_back(o); + return; + } + // lat threshold is larger for stopped obstacles + const bool is_within_lat_dist_threshold = + o.lat_dist_from_obstacle_to_traj < + p.yield_lat_distance_threshold + p.max_lat_dist_between_obstacles; + if (is_within_lat_dist_threshold) stopped_obstacles.push_back(o); + return; + }); + + if (stopped_obstacles.empty() || moving_obstacles.empty()) return std::nullopt; + + std::sort( + stopped_obstacles.begin(), stopped_obstacles.end(), [](const auto & o1, const auto & o2) { + return o1.ego_to_obstacle_distance < o2.ego_to_obstacle_distance; + }); + + std::sort(moving_obstacles.begin(), moving_obstacles.end(), [](const auto & o1, const auto & o2) { + return o1.ego_to_obstacle_distance < o2.ego_to_obstacle_distance; + }); + + std::vector yield_obstacles; + for (const auto & moving_obstacle : moving_obstacles) { + for (const auto & stopped_obstacle : stopped_obstacles) { + const bool is_moving_obs_behind_of_stopped_obs = + moving_obstacle.ego_to_obstacle_distance < stopped_obstacle.ego_to_obstacle_distance; + const bool is_moving_obs_ahead_of_ego_front = + moving_obstacle.ego_to_obstacle_distance > vehicle_info_.vehicle_length_m; + + if (!is_moving_obs_ahead_of_ego_front || !is_moving_obs_behind_of_stopped_obs) continue; + + const double lateral_distance_between_obstacles = std::abs( + moving_obstacle.lat_dist_from_obstacle_to_traj - + stopped_obstacle.lat_dist_from_obstacle_to_traj); + + const double longitudinal_distance_between_obstacles = std::abs( + moving_obstacle.ego_to_obstacle_distance - stopped_obstacle.ego_to_obstacle_distance); + + const double moving_obstacle_speed = + std::hypot(moving_obstacle.twist.linear.x, moving_obstacle.twist.linear.y); + + const bool are_obstacles_aligned = + lateral_distance_between_obstacles < p.max_lat_dist_between_obstacles; + const bool obstacles_collide_within_threshold_time = + longitudinal_distance_between_obstacles / moving_obstacle_speed < + p.max_obstacles_collision_time; + if (are_obstacles_aligned && obstacles_collide_within_threshold_time) { + const auto yield_obstacle = createYieldCruiseObstacle(moving_obstacle, traj_points); + if (yield_obstacle) { + yield_obstacles.push_back(*yield_obstacle); + using autoware::objects_of_interest_marker_interface::ColorName; + objects_of_interest_marker_interface_.insertObjectData( + stopped_obstacle.pose, stopped_obstacle.shape, ColorName::RED); + } + } + } + } + if (yield_obstacles.empty()) return std::nullopt; + return yield_obstacles; +} + +std::optional> +ObstacleCruisePlannerNode::createCollisionPointsForInsideCruiseObstacle( + const std::vector & traj_points, const std::vector & traj_polys, + const Obstacle & obstacle) const +{ + const auto & object_id = obstacle.uuid.substr(0, 4); + const auto & p = behavior_determination_param_; + + // check label + if (!isInsideCruiseObstacle(obstacle.classification.label)) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore inside obstacle (%s) since its type is not designated.", object_id.c_str()); + return std::nullopt; + } + + { // consider hysteresis + // const bool is_prev_obstacle_stop = getObstacleFromUuid(prev_stop_obstacles_, + // obstacle.uuid).has_value(); + const bool is_prev_obstacle_cruise = + getObstacleFromUuid(prev_cruise_object_obstacles_, obstacle.uuid).has_value(); + + if (is_prev_obstacle_cruise) { + if (obstacle.longitudinal_velocity < p.obstacle_velocity_threshold_from_cruise_to_stop) { + return std::nullopt; + } + // NOTE: else is keeping cruise + } else { // if (is_prev_obstacle_stop) { + // TODO(murooka) consider hysteresis for slow down + // If previous obstacle is stop or does not exist. + if (obstacle.longitudinal_velocity < p.obstacle_velocity_threshold_from_stop_to_cruise) { + return std::nullopt; + } + // NOTE: else is cruise from stop + } + } + + // Get highest confidence predicted path + const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths( + obstacle.predicted_paths, p.prediction_resampling_time_interval, + p.prediction_resampling_time_horizon, 1); + + if (resampled_predicted_paths.empty()) { + return std::nullopt; + } + + // calculate nearest collision point + std::vector collision_index; + const auto collision_points = polygon_utils::getCollisionPoints( + traj_points, traj_polys, obstacle.stamp, resampled_predicted_paths.front(), obstacle.shape, + now(), is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise)); + return collision_points; +} + +std::optional> +ObstacleCruisePlannerNode::createCollisionPointsForOutsideCruiseObstacle( + const std::vector & traj_points, const std::vector & traj_polys, + const Obstacle & obstacle) const +{ + const auto & p = behavior_determination_param_; + const auto & object_id = obstacle.uuid.substr(0, 4); + + // check label + if (!isOutsideCruiseObstacle(obstacle.classification.label)) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore outside obstacle (%s) since its type is not designated.", object_id.c_str()); + return std::nullopt; + } + + if ( + std::hypot(obstacle.twist.linear.x, obstacle.twist.linear.y) < + p.outside_obstacle_min_velocity_threshold) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore outside obstacle (%s) since the obstacle velocity is low.", + object_id.c_str()); + return std::nullopt; + } + + // Get the highest confidence predicted paths + const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths( + obstacle.predicted_paths, p.prediction_resampling_time_interval, + p.prediction_resampling_time_horizon, p.num_of_predicted_paths_for_outside_cruise_obstacle); + + // calculate collision condition for cruise + std::vector collision_index; + const auto getCollisionPoints = [&]() -> std::vector { + for (const auto & predicted_path : resampled_predicted_paths) { + const auto collision_points = polygon_utils::getCollisionPoints( + traj_points, traj_polys, obstacle.stamp, predicted_path, obstacle.shape, now(), + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + p.max_lat_margin_for_cruise), + p.max_prediction_time_for_collision_check); + if (!collision_points.empty()) { + return collision_points; + } + } + return {}; + }; + + const auto collision_points = getCollisionPoints(); + + if (collision_points.empty()) { + // Ignore vehicle obstacles outside the trajectory without collision + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore outside obstacle (%s) since there are no collision points.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } + + const double overlap_time = + (rclcpp::Time(collision_points.back().stamp) - rclcpp::Time(collision_points.front().stamp)) + .seconds(); + if (overlap_time < p.ego_obstacle_overlap_time_threshold) { + // Ignore vehicle obstacles outside the trajectory, whose predicted path + // overlaps the ego trajectory in a certain time. + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Cruise] Ignore outside obstacle (%s) since it will not collide with the ego.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } + + // Ignore obstacles behind the ego vehicle. + // Note: Only using isFrontObstacle(), behind obstacles cannot be filtered + // properly when the trajectory is crossing or overlapping. + const size_t first_collision_index = collision_index.front(); + if (!isFrontCollideObstacle(traj_points, obstacle, first_collision_index)) { + return std::nullopt; + } + return collision_points; +} + +std::optional> +ObstacleCruisePlannerNode::createCollisionPointForOutsideStopObstacle( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const PredictedPath & resampled_predicted_path, double max_lat_margin_for_stop) const +{ + const auto & object_id = obstacle.uuid.substr(0, 4); + const auto & p = behavior_determination_param_; + + std::vector collision_index; + const auto collision_points = polygon_utils::getCollisionPoints( + traj_points, traj_polys, obstacle.stamp, resampled_predicted_path, obstacle.shape, now(), + is_driving_forward_, collision_index, + calcObstacleMaxLength(obstacle.shape) + p.decimate_trajectory_step_length + + std::hypot( + vehicle_info_.vehicle_length_m, + vehicle_info_.vehicle_width_m * 0.5 + max_lat_margin_for_stop)); + if (collision_points.empty()) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore outside obstacle (%s) since there is no collision point between the " + "predicted path " + "and the ego.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } + + const double collision_time_margin = + calcCollisionTimeMargin(odometry, collision_points, traj_points, is_driving_forward_); + if (p.collision_time_margin < collision_time_margin) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore outside obstacle (%s) since it will not collide with the ego.", + object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } + + return polygon_utils::getCollisionPoint( + traj_points, collision_index.front(), collision_points, is_driving_forward_, vehicle_info_); +} + +std::optional ObstacleCruisePlannerNode::createStopObstacleForPredictedObject( + const Odometry & odometry, const std::vector & traj_points, + const std::vector & traj_polys, const Obstacle & obstacle, + const double precise_lat_dist) const +{ + const auto & p = behavior_determination_param_; + const auto & object_id = obstacle.uuid.substr(0, 4); + + if (!isStopObstacle(obstacle.classification.label)) { + return std::nullopt; + } + const double max_lat_margin_for_stop = + (obstacle.classification.label == ObjectClassification::UNKNOWN) + ? p.max_lat_margin_for_stop_against_unknown + : p.max_lat_margin_for_stop; + + // Obstacle that is not inside of trajectory + if (precise_lat_dist > std::max(max_lat_margin_for_stop, 1e-3)) { + if (!isOutsideStopObstacle(obstacle.classification.label)) { + return std::nullopt; + } + + const auto time_to_traj = precise_lat_dist / std::max(1e-6, obstacle.approach_velocity); + if (time_to_traj > p.max_lat_time_margin_for_stop) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore outside obstacle (%s) since it's far from trajectory.", object_id.c_str()); + return std::nullopt; + } + + // brkay54: For the pedestrians and bicycles, we need to check the collision point by thinking + // they will stop with a predefined deceleration rate to avoid unnecessary stops. + double resample_time_horizon = p.prediction_resampling_time_horizon; + if (obstacle.classification.label == ObjectClassification::PEDESTRIAN) { + resample_time_horizon = + std::sqrt(std::pow(obstacle.twist.linear.x, 2) + std::pow(obstacle.twist.linear.y, 2)) / + (2.0 * p.pedestrian_deceleration_rate); + } else if (obstacle.classification.label == ObjectClassification::BICYCLE) { + resample_time_horizon = + std::sqrt(std::pow(obstacle.twist.linear.x, 2) + std::pow(obstacle.twist.linear.y, 2)) / + (2.0 * p.bicycle_deceleration_rate); + } + + // Get the highest confidence predicted path + const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths( + obstacle.predicted_paths, p.prediction_resampling_time_interval, resample_time_horizon, + p.num_of_predicted_paths_for_outside_stop_obstacle); + if (resampled_predicted_paths.empty()) { + return std::nullopt; + } + + const auto getCollisionPoint = + [&]() -> std::optional> { + for (const auto & predicted_path : resampled_predicted_paths) { + const auto collision_point = createCollisionPointForOutsideStopObstacle( + odometry, traj_points, traj_polys, obstacle, predicted_path, max_lat_margin_for_stop); + if (collision_point) { + return collision_point; + } + } + return std::nullopt; + }; + + const auto collision_point = getCollisionPoint(); + + if (collision_point) { + return StopObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.classification, + obstacle.pose, + obstacle.shape, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + collision_point->first, + collision_point->second}; + } + return std::nullopt; + } + + // Obstacle inside the trajectory + if (!isInsideStopObstacle(obstacle.classification.label)) { + return std::nullopt; + } + + // calculate collision points with trajectory with lateral stop margin + const auto traj_polys_with_lat_margin = + createOneStepPolygons(traj_points, vehicle_info_, odometry.pose.pose, max_lat_margin_for_stop); + + const auto collision_point = polygon_utils::getCollisionPoint( + traj_points, traj_polys_with_lat_margin, obstacle, is_driving_forward_, vehicle_info_); + if (!collision_point) { + return std::nullopt; + } + + // check transient obstacle or not + const double abs_ego_offset = + min_behavior_stop_margin_ + (is_driving_forward_ + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m)); + + const double time_to_reach_stop_point = + calcTimeToReachCollisionPoint(odometry, collision_point->first, traj_points, abs_ego_offset); + const bool is_transient_obstacle = [&]() { + if (time_to_reach_stop_point <= p.collision_time_margin) { + return false; + } + // get the predicted position of the obstacle when ego reaches the collision point + const auto resampled_predicted_paths = resampleHighestConfidencePredictedPaths( + obstacle.predicted_paths, p.prediction_resampling_time_interval, + p.prediction_resampling_time_horizon, 1); + if (resampled_predicted_paths.empty() || resampled_predicted_paths.front().path.empty()) { + return false; + } + const auto future_obj_pose = autoware::object_recognition_utils::calcInterpolatedPose( + resampled_predicted_paths.front(), time_to_reach_stop_point - p.collision_time_margin); + + Obstacle tmp_future_obs = obstacle; + tmp_future_obs.pose = + future_obj_pose ? future_obj_pose.value() : resampled_predicted_paths.front().path.back(); + const auto future_collision_point = polygon_utils::getCollisionPoint( + traj_points, traj_polys_with_lat_margin, tmp_future_obs, is_driving_forward_, vehicle_info_); + + return !future_collision_point; + }(); + + if (is_transient_obstacle) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[Stop] Ignore inside obstacle (%s) since it is transient obstacle.", object_id.c_str()); + debug_data_ptr_->intentionally_ignored_obstacles.push_back(obstacle); + return std::nullopt; + } + + return StopObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.classification, + obstacle.pose, + obstacle.shape, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + collision_point->first, + collision_point->second}; +} + +std::optional ObstacleCruisePlannerNode::createStopObstacleForPointCloud( + const std::vector & traj_points, const Obstacle & obstacle, + const double precise_lat_dist) const +{ + const auto & p = behavior_determination_param_; + + if (!use_pointcloud_for_stop_) { + return std::nullopt; + } + + if (!obstacle.stop_collision_point) { + return std::nullopt; + } + + const double max_lat_margin_for_stop = + (obstacle.classification.label == ObjectClassification::UNKNOWN) + ? p.max_lat_margin_for_stop_against_unknown + : p.max_lat_margin_for_stop; + + if (precise_lat_dist > std::max(max_lat_margin_for_stop, 1e-3)) { + return std::nullopt; + } + + const auto dist_to_collide_on_traj = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, *obstacle.stop_collision_point); + + return StopObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.classification, + obstacle.pose, + obstacle.shape, + {}, + {}, + *obstacle.stop_collision_point, + dist_to_collide_on_traj}; +} + +std::optional ObstacleCruisePlannerNode::createSlowDownObstacleForPredictedObject( + const Odometry & odometry, const std::vector & traj_points, + const Obstacle & obstacle, const double precise_lat_dist) +{ + const auto & object_id = obstacle.uuid.substr(0, 4); + const auto & p = behavior_determination_param_; + slow_down_condition_counter_.addCurrentUuid(obstacle.uuid); + + const bool is_prev_obstacle_slow_down = + getObstacleFromUuid(prev_slow_down_object_obstacles_, obstacle.uuid).has_value(); + + if (!enable_slow_down_planning_ || !isSlowDownObstacle(obstacle.classification.label)) { + return std::nullopt; + } + + // check lateral distance considering hysteresis + const bool is_lat_dist_low = isLowerConsideringHysteresis( + precise_lat_dist, is_prev_obstacle_slow_down, + p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down / 2.0, + p.max_lat_margin_for_slow_down - p.lat_hysteresis_margin_for_slow_down / 2.0); + + const bool is_slow_down_required = [&]() { + if (is_prev_obstacle_slow_down) { + // check if exiting slow down + if (!is_lat_dist_low) { + const int count = slow_down_condition_counter_.decreaseCounter(obstacle.uuid); + if (count <= -p.successive_num_to_exit_slow_down_condition) { + slow_down_condition_counter_.reset(obstacle.uuid); + return false; + } + } + return true; + } + // check if entering slow down + if (is_lat_dist_low) { + const int count = slow_down_condition_counter_.increaseCounter(obstacle.uuid); + if (p.successive_num_to_entry_slow_down_condition <= count) { + slow_down_condition_counter_.reset(obstacle.uuid); + return true; + } + } + return false; + }(); + if (!is_slow_down_required) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[SlowDown] Ignore obstacle (%s) since it's far from trajectory. (%f [m])", object_id.c_str(), + precise_lat_dist); + return std::nullopt; + } + + const auto obstacle_poly = autoware::universe_utils::toPolygon2d(obstacle.pose, obstacle.shape); + + // calculate collision points with trajectory with lateral stop margin + // NOTE: For additional margin, hysteresis is not divided by two. + const auto traj_polys_with_lat_margin = createOneStepPolygons( + traj_points, vehicle_info_, odometry.pose.pose, + p.max_lat_margin_for_slow_down + p.lat_hysteresis_margin_for_slow_down); + + std::vector front_collision_polygons; + size_t front_seg_idx = 0; + std::vector back_collision_polygons; + size_t back_seg_idx = 0; + for (size_t i = 0; i < traj_polys_with_lat_margin.size(); ++i) { + std::vector collision_polygons; + bg::intersection(traj_polys_with_lat_margin.at(i), obstacle_poly, collision_polygons); + + if (!collision_polygons.empty()) { + if (front_collision_polygons.empty()) { + front_collision_polygons = collision_polygons; + front_seg_idx = i == 0 ? i : i - 1; + } + back_collision_polygons = collision_polygons; + back_seg_idx = i == 0 ? i : i - 1; + } else { + if (!back_collision_polygons.empty()) { + break; // for efficient calculation + } + } + } + + if (front_collision_polygons.empty() || back_collision_polygons.empty()) { + RCLCPP_INFO_EXPRESSION( + get_logger(), enable_debug_info_, + "[SlowDown] Ignore obstacle (%s) since there is no collision point", object_id.c_str()); + return std::nullopt; + } + + // calculate front collision point + double front_min_dist = std::numeric_limits::max(); + geometry_msgs::msg::Point front_collision_point; + for (const auto & collision_poly : front_collision_polygons) { + for (const auto & collision_point : collision_poly.outer()) { + const auto collision_geom_point = toGeomPoint(collision_point); + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( + traj_points, front_seg_idx, collision_geom_point); + if (dist < front_min_dist) { + front_min_dist = dist; + front_collision_point = collision_geom_point; + } + } + } + + // calculate back collision point + double back_max_dist = -std::numeric_limits::max(); + geometry_msgs::msg::Point back_collision_point = front_collision_point; + for (const auto & collision_poly : back_collision_polygons) { + for (const auto & collision_point : collision_poly.outer()) { + const auto collision_geom_point = toGeomPoint(collision_point); + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( + traj_points, back_seg_idx, collision_geom_point); + if (back_max_dist < dist) { + back_max_dist = dist; + back_collision_point = collision_geom_point; + } + } + } + + return SlowDownObstacle{ + obstacle.uuid, + obstacle.stamp, + obstacle.classification, + obstacle.pose, + obstacle.longitudinal_velocity, + obstacle.approach_velocity, + precise_lat_dist, + front_collision_point, + back_collision_point}; +} + +std::optional ObstacleCruisePlannerNode::createSlowDownObstacleForPointCloud( + const Obstacle & obstacle, const double precise_lat_dist) +{ + if (!enable_slow_down_planning_ || !use_pointcloud_for_slow_down_) { + return std::nullopt; + } + + if (!obstacle.slow_down_front_collision_point) { + return std::nullopt; + } + + auto front_collision_point = *obstacle.slow_down_front_collision_point; + auto back_collision_point = + obstacle.slow_down_back_collision_point.value_or(front_collision_point); + + return SlowDownObstacle{ + obstacle.uuid, obstacle.stamp, obstacle.classification, obstacle.pose, {}, {}, + precise_lat_dist, front_collision_point, back_collision_point}; +} + +void ObstacleCruisePlannerNode::checkConsistency( + const rclcpp::Time & current_time, const PredictedObjects & predicted_objects, + std::vector & stop_obstacles) +{ + for (const auto & prev_closest_stop_obstacle : prev_closest_stop_object_obstacles_) { + const auto predicted_object_itr = std::find_if( + predicted_objects.objects.begin(), predicted_objects.objects.end(), + [&prev_closest_stop_obstacle](const PredictedObject & po) { + return autoware::universe_utils::toHexString(po.object_id) == + prev_closest_stop_obstacle.uuid; + }); + // If previous closest obstacle disappear from the perception result, do nothing anymore. + if (predicted_object_itr == predicted_objects.objects.end()) { + continue; + } + + const auto is_disappeared_from_stop_obstacle = std::none_of( + stop_obstacles.begin(), stop_obstacles.end(), + [&prev_closest_stop_obstacle](const StopObstacle & so) { + return so.uuid == prev_closest_stop_obstacle.uuid; + }); + if (is_disappeared_from_stop_obstacle) { + // re-evaluate as a stop candidate, and overwrite the current decision if "maintain stop" + // condition is satisfied + const double elapsed_time = (current_time - prev_closest_stop_obstacle.stamp).seconds(); + if ( + predicted_object_itr->kinematics.initial_twist_with_covariance.twist.linear.x < + behavior_determination_param_.obstacle_velocity_threshold_from_stop_to_cruise && + elapsed_time < behavior_determination_param_.stop_obstacle_hold_time_threshold) { + stop_obstacles.push_back(prev_closest_stop_obstacle); + } + } + } + + prev_closest_stop_object_obstacles_ = + obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); +} + +bool ObstacleCruisePlannerNode::isObstacleCrossing( + const std::vector & traj_points, const Obstacle & obstacle) const +{ + const double diff_angle = calcDiffAngleAgainstTrajectory(traj_points, obstacle.pose); + + // NOTE: Currently predicted objects does not have orientation availability even + // though sometimes orientation is not available. + const bool is_obstacle_crossing_trajectory = + behavior_determination_param_.crossing_obstacle_traj_angle_threshold < std::abs(diff_angle) && + behavior_determination_param_.crossing_obstacle_traj_angle_threshold < + M_PI - std::abs(diff_angle); + if (!is_obstacle_crossing_trajectory) { + return false; + } + + // Only obstacles crossing the ego's trajectory with high speed are considered. + return true; +} + +double ObstacleCruisePlannerNode::calcCollisionTimeMargin( + const Odometry & odometry, const std::vector & collision_points, + const std::vector & traj_points, const bool is_driving_forward) const +{ + const auto & p = behavior_determination_param_; + const double abs_ego_offset = + min_behavior_stop_margin_ + (is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m)); + const double time_to_reach_stop_point = calcTimeToReachCollisionPoint( + odometry, collision_points.front().point, traj_points, abs_ego_offset); + + const double time_to_leave_collision_point = + time_to_reach_stop_point + + abs_ego_offset / + std::max(p.min_velocity_to_reach_collision_point, odometry.twist.twist.linear.x); + + const double time_to_start_cross = + (rclcpp::Time(collision_points.front().stamp) - now()).seconds(); + const double time_to_end_cross = (rclcpp::Time(collision_points.back().stamp) - now()).seconds(); + + if (time_to_leave_collision_point < time_to_start_cross) { // Ego goes first. + return time_to_start_cross - time_to_reach_stop_point; + } + if (time_to_end_cross < time_to_reach_stop_point) { // Obstacle goes first. + return time_to_reach_stop_point - time_to_end_cross; + } + return 0.0; // Ego and obstacle will collide. +} + +PlannerData ObstacleCruisePlannerNode::createPlannerData( + const Odometry & odometry, const AccelWithCovarianceStamped & acc, + const std::vector & traj_points) const +{ + PlannerData planner_data; + planner_data.current_time = now(); + planner_data.traj_points = traj_points; + planner_data.ego_pose = odometry.pose.pose; + planner_data.ego_vel = odometry.twist.twist.linear.x; + planner_data.ego_acc = acc.accel.accel.linear.x; + planner_data.is_driving_forward = is_driving_forward_; + return planner_data; +} + +void ObstacleCruisePlannerNode::publishVelocityLimit( + const std::optional & vel_limit, const std::string & module_name) +{ + if (vel_limit) { + vel_limit_pub_->publish(*vel_limit); + need_to_clear_vel_limit_.at(module_name) = true; + return; + } + + if (!need_to_clear_vel_limit_.at(module_name)) { + return; + } + + // clear velocity limit + const auto clear_vel_limit_msg = createVelocityLimitClearCommandMessage(now(), module_name); + clear_vel_limit_pub_->publish(clear_vel_limit_msg); + need_to_clear_vel_limit_.at(module_name) = false; +} + +void ObstacleCruisePlannerNode::publishDebugMarker() const +{ + stop_watch_.tic(__func__); + + // 1. publish debug marker + MarkerArray debug_marker; + + // obstacles to cruise + std::vector stop_collision_points; + for (size_t i = 0; i < debug_data_ptr_->obstacles_to_cruise.size(); ++i) { + // obstacle + const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker( + debug_data_ptr_->obstacles_to_cruise.at(i).pose, i, "obstacles_to_cruise", 1.0, 0.6, 0.1); + debug_marker.markers.push_back(obstacle_marker); + + // collision points + for (size_t j = 0; j < debug_data_ptr_->obstacles_to_cruise.at(i).collision_points.size(); + ++j) { + stop_collision_points.push_back( + debug_data_ptr_->obstacles_to_cruise.at(i).collision_points.at(j).point); + } + } + for (size_t i = 0; i < stop_collision_points.size(); ++i) { + auto collision_point_marker = autoware::universe_utils::createDefaultMarker( + "map", now(), "cruise_collision_points", i, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + collision_point_marker.pose.position = stop_collision_points.at(i); + debug_marker.markers.push_back(collision_point_marker); + } + + // obstacles to stop + for (size_t i = 0; i < debug_data_ptr_->obstacles_to_stop.size(); ++i) { + // obstacle + const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker( + debug_data_ptr_->obstacles_to_stop.at(i).pose, i, "obstacles_to_stop", 1.0, 0.0, 0.0); + debug_marker.markers.push_back(obstacle_marker); + + // collision point + auto collision_point_marker = autoware::universe_utils::createDefaultMarker( + "map", now(), "stop_collision_points", 0, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + collision_point_marker.pose.position = debug_data_ptr_->obstacles_to_stop.at(i).collision_point; + debug_marker.markers.push_back(collision_point_marker); + } + + // obstacles to slow down + for (size_t i = 0; i < debug_data_ptr_->obstacles_to_slow_down.size(); ++i) { + // obstacle + const auto obstacle_marker = obstacle_cruise_utils::getObjectMarker( + debug_data_ptr_->obstacles_to_slow_down.at(i).pose, i, "obstacles_to_slow_down", 0.7, 0.7, + 0.0); + debug_marker.markers.push_back(obstacle_marker); + + // collision points + auto front_collision_point_marker = autoware::universe_utils::createDefaultMarker( + "map", now(), "slow_down_collision_points", i * 2 + 0, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + front_collision_point_marker.pose.position = + debug_data_ptr_->obstacles_to_slow_down.at(i).front_collision_point; + auto back_collision_point_marker = autoware::universe_utils::createDefaultMarker( + "map", now(), "slow_down_collision_points", i * 2 + 1, Marker::SPHERE, + autoware::universe_utils::createMarkerScale(0.25, 0.25, 0.25), + autoware::universe_utils::createMarkerColor(1.0, 0.0, 0.0, 0.999)); + back_collision_point_marker.pose.position = + debug_data_ptr_->obstacles_to_slow_down.at(i).back_collision_point; + + debug_marker.markers.push_back(front_collision_point_marker); + debug_marker.markers.push_back(back_collision_point_marker); + } + + // intentionally ignored obstacles to cruise or stop + for (size_t i = 0; i < debug_data_ptr_->intentionally_ignored_obstacles.size(); ++i) { + const auto marker = obstacle_cruise_utils::getObjectMarker( + debug_data_ptr_->intentionally_ignored_obstacles.at(i).pose, i, + "intentionally_ignored_obstacles", 0.0, 1.0, 0.0); + debug_marker.markers.push_back(marker); + } + + { // footprint polygons + auto marker = autoware::universe_utils::createDefaultMarker( + "map", now(), "detection_polygons", 0, Marker::LINE_LIST, + autoware::universe_utils::createMarkerScale(0.01, 0.0, 0.0), + autoware::universe_utils::createMarkerColor(0.0, 1.0, 0.0, 0.999)); + + for (const auto & detection_polygon : debug_data_ptr_->detection_polygons) { + for (size_t dp_idx = 0; dp_idx < detection_polygon.outer().size(); ++dp_idx) { + const auto & current_point = detection_polygon.outer().at(dp_idx); + const auto & next_point = + detection_polygon.outer().at((dp_idx + 1) % detection_polygon.outer().size()); + + marker.points.push_back( + autoware::universe_utils::createPoint(current_point.x(), current_point.y(), 0.0)); + marker.points.push_back( + autoware::universe_utils::createPoint(next_point.x(), next_point.y(), 0.0)); + } + } + debug_marker.markers.push_back(marker); + } + + // slow down debug wall marker + autoware::universe_utils::appendMarkerArray( + debug_data_ptr_->slow_down_debug_wall_marker, &debug_marker); + + debug_marker_pub_->publish(debug_marker); + + // 2. publish virtual wall for cruise and stop + debug_cruise_wall_marker_pub_->publish(debug_data_ptr_->cruise_wall_marker); + debug_stop_wall_marker_pub_->publish(debug_data_ptr_->stop_wall_marker); + debug_slow_down_wall_marker_pub_->publish(debug_data_ptr_->slow_down_wall_marker); + + // 3. print calculation time + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner"), enable_calculation_time_info_, " %s := %f [ms]", + __func__, calculation_time); +} + +void ObstacleCruisePlannerNode::publishDebugInfo() const +{ + // stop + const auto stop_debug_msg = planner_ptr_->getStopPlanningDebugMessage(now()); + debug_stop_planning_info_pub_->publish(stop_debug_msg); + + // cruise + const auto cruise_debug_msg = planner_ptr_->getCruisePlanningDebugMessage(now()); + debug_cruise_planning_info_pub_->publish(cruise_debug_msg); + + // slow_down + const auto slow_down_debug_msg = planner_ptr_->getSlowDownPlanningDebugMessage(now()); + debug_slow_down_planning_info_pub_->publish(slow_down_debug_msg); +} + +void ObstacleCruisePlannerNode::publishCalculationTime(const double calculation_time) const +{ + Float64Stamped calculation_time_msg; + calculation_time_msg.stamp = now(); + calculation_time_msg.data = calculation_time; + debug_calculation_time_pub_->publish(calculation_time_msg); +} + +double ObstacleCruisePlannerNode::calcTimeToReachCollisionPoint( + const Odometry & odometry, const geometry_msgs::msg::Point & collision_point, + const std::vector & traj_points, const double abs_ego_offset) const +{ + const auto & p = behavior_determination_param_; + const double dist_from_ego_to_obstacle = + std::abs(autoware::motion_utils::calcSignedArcLength( + traj_points, odometry.pose.pose.position, collision_point)) - + abs_ego_offset; + return dist_from_ego_to_obstacle / + std::max(p.min_velocity_to_reach_collision_point, std::abs(odometry.twist.twist.linear.x)); +} } // namespace autoware::motion_planning #include diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp new file mode 100644 index 0000000000000..62edf82beec5a --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/optimization_based_planner.cpp @@ -0,0 +1,754 @@ +// 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 "autoware/obstacle_cruise_planner/optimization_based_planner/optimization_based_planner.hpp" + +#include "autoware/interpolation/linear_interpolation.hpp" +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/interpolation/zero_order_hold.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/interpolation.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" + +#include +#include +#include +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +constexpr double ZERO_VEL_THRESHOLD = 0.01; +constexpr double CLOSE_S_DIST_THRESHOLD = 1e-3; + +OptimizationBasedPlanner::OptimizationBasedPlanner( + rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) +: PlannerInterface(node, longitudinal_info, vehicle_info, ego_nearest_param, debug_data_ptr) +{ + smoothed_traj_sub_ = node.create_subscription( + "/planning/scenario_planning/trajectory", rclcpp::QoS{1}, + [this](const Trajectory::ConstSharedPtr msg) { smoothed_trajectory_ptr_ = msg; }); + + // parameter + dense_resampling_time_interval_ = node.declare_parameter( + "cruise.optimization_based_planner.dense_resampling_time_interval"); + sparse_resampling_time_interval_ = node.declare_parameter( + "cruise.optimization_based_planner.sparse_resampling_time_interval"); + dense_time_horizon_ = + node.declare_parameter("cruise.optimization_based_planner.dense_time_horizon"); + max_time_horizon_ = + node.declare_parameter("cruise.optimization_based_planner.max_time_horizon"); + + t_dangerous_ = node.declare_parameter("cruise.optimization_based_planner.t_dangerous"); + velocity_margin_ = + node.declare_parameter("cruise.optimization_based_planner.velocity_margin"); + + replan_vel_deviation_ = + node.declare_parameter("cruise.optimization_based_planner.replan_vel_deviation"); + engage_velocity_ = + node.declare_parameter("cruise.optimization_based_planner.engage_velocity"); + engage_acceleration_ = + node.declare_parameter("cruise.optimization_based_planner.engage_acceleration"); + engage_exit_ratio_ = + node.declare_parameter("cruise.optimization_based_planner.engage_exit_ratio"); + stop_dist_to_prohibit_engage_ = node.declare_parameter( + "cruise.optimization_based_planner.stop_dist_to_prohibit_engage"); + + const double max_s_weight = + node.declare_parameter("cruise.optimization_based_planner.max_s_weight"); + const double max_v_weight = + node.declare_parameter("cruise.optimization_based_planner.max_v_weight"); + const double over_s_safety_weight = + node.declare_parameter("cruise.optimization_based_planner.over_s_safety_weight"); + const double over_s_ideal_weight = + node.declare_parameter("cruise.optimization_based_planner.over_s_ideal_weight"); + const double over_v_weight = + node.declare_parameter("cruise.optimization_based_planner.over_v_weight"); + const double over_a_weight = + node.declare_parameter("cruise.optimization_based_planner.over_a_weight"); + const double over_j_weight = + node.declare_parameter("cruise.optimization_based_planner.over_j_weight"); + + // velocity optimizer + velocity_optimizer_ptr_ = std::make_shared( + max_s_weight, max_v_weight, over_s_safety_weight, over_s_ideal_weight, over_v_weight, + over_a_weight, over_j_weight); + + // publisher + 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); + debug_wall_marker_pub_ = node.create_publisher("~/debug/wall_marker", 1); +} + +std::vector OptimizationBasedPlanner::generateCruiseTrajectory( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const std::vector & obstacles, + [[maybe_unused]] std::optional & vel_limit) +{ + // 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_ = stop_traj_points; + return stop_traj_points; + } + + // Get the nearest point on the trajectory + const size_t closest_idx = findEgoSegmentIndex(stop_traj_points, planner_data.ego_pose); + + // Compute maximum velocity + double v_max = 0.0; + 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, stop_traj_points, prev_output_); + a0 = std::min(longitudinal_info_.max_accel, std::max(a0, longitudinal_info_.min_accel)); + + // Check trajectory size + if (stop_traj_points.size() - closest_idx <= 2) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "The number of points on the trajectory is too small or failed to calculate front offset"); + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // Check if reached goal + if (checkHasReachedGoal(planner_data, stop_traj_points)) { + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // Get S Boundaries from the obstacle + const auto s_boundaries = getSBoundaries(planner_data, stop_traj_points, obstacles, time_vec); + if (!s_boundaries) { + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // Optimization + VelocityOptimizer::OptimizationData data; + data.time_vec = time_vec; + data.s0 = 0.0; + data.a0 = a0; + data.v_max = v_max; + data.a_max = longitudinal_info_.max_accel; + data.a_min = longitudinal_info_.min_accel; + data.j_max = longitudinal_info_.max_jerk; + data.j_min = longitudinal_info_.min_jerk; + data.limit_a_max = longitudinal_info_.limit_max_accel; + data.limit_a_min = longitudinal_info_.limit_min_accel; + data.limit_j_max = longitudinal_info_.limit_max_jerk; + data.limit_j_min = longitudinal_info_.limit_min_jerk; + data.t_dangerous = t_dangerous_; + data.idling_time = longitudinal_info_.idling_time; + data.s_boundary = *s_boundaries; + data.v0 = v0; + RCLCPP_DEBUG(rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), "v0 %f", v0); + + const auto optimized_result = velocity_optimizer_ptr_->optimize(data); + + // Publish Debug trajectories + const double traj_front_to_vehicle_offset = + autoware::motion_utils::calcSignedArcLength(stop_traj_points, 0, closest_idx); + publishDebugTrajectory( + planner_data, traj_front_to_vehicle_offset, time_vec, *s_boundaries, optimized_result); + + // Transformation from t to s + const auto processed_result = + processOptimizedResult(data.v0, optimized_result, traj_front_to_vehicle_offset); + if (!processed_result) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Processed Result is empty"); + prev_output_ = stop_traj_points; + return stop_traj_points; + } + const auto & opt_position = processed_result->s; + const auto & opt_velocity = processed_result->v; + + // Check Size + if (opt_position.size() == 1 && opt_velocity.front() < ZERO_VEL_THRESHOLD) { + auto output = stop_traj_points; + output.at(closest_idx).longitudinal_velocity_mps = data.v0; + for (size_t i = closest_idx + 1; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = 0.0; + } + prev_output_ = output; + debug_data_ptr_->cruise_metrics = + makeMetrics("OptimizationBasedPlanner", "cruise", planner_data); + return output; + } else if (opt_position.size() == 1) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "Optimized Trajectory is too small"); + prev_output_ = stop_traj_points; + return stop_traj_points; + } + + // 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) { + closest_stop_dist = std::min(closest_stop_dist, opt_position.at(i)); + break; + } + } + const auto traj_stop_dist = + autoware::motion_utils::calcDistanceToForwardStopPoint(stop_traj_points, closest_idx); + if (traj_stop_dist) { + closest_stop_dist = std::min(*traj_stop_dist + traj_front_to_vehicle_offset, closest_stop_dist); + } + + // Resample Optimum Velocity + size_t break_id = stop_traj_points.size(); + std::vector resampled_opt_position; + for (size_t i = closest_idx; i < stop_traj_points.size(); ++i) { + const double query_s = std::max( + autoware::motion_utils::calcSignedArcLength(stop_traj_points, 0, i), opt_position.front()); + if (query_s > opt_position.back()) { + break_id = i; + break; + } + resampled_opt_position.push_back(query_s); + } + // resample optimum velocity for original each position + auto resampled_opt_velocity = + autoware::interpolation::lerp(opt_position, opt_velocity, resampled_opt_position); + for (size_t i = break_id; i < stop_traj_points.size(); ++i) { + resampled_opt_velocity.push_back(stop_traj_points.at(i).longitudinal_velocity_mps); + } + + // Create Output Data + std::vector output = stop_traj_points; + for (size_t i = 0; i < closest_idx; ++i) { + output.at(i).longitudinal_velocity_mps = data.v0; + } + for (size_t i = closest_idx; i < output.size(); ++i) { + output.at(i).longitudinal_velocity_mps = + resampled_opt_velocity.at(i - closest_idx) + velocity_margin_; + } + output.back().longitudinal_velocity_mps = 0.0; // terminal velocity is zero + + // Insert Closest Stop Point + autoware::motion_utils::insertStopPoint(0, closest_stop_dist, output); + + debug_data_ptr_->cruise_metrics = makeMetrics("OptimizationBasedPlanner", "cruise", planner_data); + prev_output_ = output; + return output; +} + +std::vector OptimizationBasedPlanner::createTimeVector() +{ + std::vector time_vec; + for (double t = 0.0; t < dense_time_horizon_; t += dense_resampling_time_interval_) { + time_vec.push_back(t); + } + if (dense_time_horizon_ - time_vec.back() < 1e-3) { + time_vec.back() = dense_time_horizon_; + } else { + time_vec.push_back(dense_time_horizon_); + } + + for (double t = dense_time_horizon_ + sparse_resampling_time_interval_; t < max_time_horizon_; + t += sparse_resampling_time_interval_) { + time_vec.push_back(t); + } + if (max_time_horizon_ - time_vec.back() < 1e-3) { + time_vec.back() = max_time_horizon_; + } else { + time_vec.push_back(max_time_horizon_); + } + + return time_vec; +} + +// v0, a0 +std::tuple OptimizationBasedPlanner::calcInitialMotion( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const std::vector & prev_traj_points) +{ + const auto & ego_vel = planner_data.ego_vel; + const auto & ego_pose = planner_data.ego_pose; + const auto & input_traj_points = stop_traj_points; + const double vehicle_speed{std::abs(ego_vel)}; + const auto current_closest_point = + ego_nearest_param_.calcInterpolatedPoint(input_traj_points, planner_data.ego_pose); + const double target_vel{std::abs(current_closest_point.longitudinal_velocity_mps)}; + + double initial_vel{}; + double initial_acc{}; + + // first time + if (prev_traj_points.empty()) { + initial_vel = vehicle_speed; + initial_acc = 0.0; + return std::make_tuple(initial_vel, initial_acc); + } + + TrajectoryPoint prev_output_closest_point; + if (smoothed_trajectory_ptr_) { + prev_output_closest_point = autoware::motion_utils::calcInterpolatedPoint( + *smoothed_trajectory_ptr_, ego_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); + } else { + prev_output_closest_point = + ego_nearest_param_.calcInterpolatedPoint(prev_traj_points, ego_pose); + } + + // when velocity tracking deviation is large + const double desired_vel{prev_output_closest_point.longitudinal_velocity_mps}; + const double vel_error{vehicle_speed - std::abs(desired_vel)}; + if (std::abs(vel_error) > replan_vel_deviation_) { + initial_vel = vehicle_speed; // use current vehicle speed + initial_acc = prev_output_closest_point.acceleration_mps2; + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "calcInitialMotion : Large deviation error for speed control. Use current speed for " + "initial value, desired_vel = %f, vehicle_speed = %f, vel_error = %f, error_thr = %f", + desired_vel, vehicle_speed, vel_error, replan_vel_deviation_); + return std::make_tuple(initial_vel, initial_acc); + } + + // if current vehicle velocity is low && base_desired speed is high, + // use engage_velocity for engage vehicle + const double engage_vel_thr = engage_velocity_ * engage_exit_ratio_; + if (vehicle_speed < engage_vel_thr) { + if (target_vel >= engage_velocity_) { + const auto stop_dist = autoware::motion_utils::calcDistanceToForwardStopPoint( + input_traj_points, ego_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); + if (!stop_dist.has_value()) { + 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)", + vehicle_speed, target_vel, engage_velocity_, engage_vel_thr); + return std::make_tuple(engage_velocity_, engage_acceleration_); + } else if (stop_dist.value() > stop_dist_to_prohibit_engage_) { + 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.value()); + return std::make_tuple(engage_velocity_, engage_acceleration_); + } else { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "calcInitialMotion : stop point is close (%.3f[m]). no engage.", stop_dist.value()); + } + } else if (target_vel > 0.0) { + auto clock{rclcpp::Clock{RCL_ROS_TIME}}; + RCLCPP_WARN_THROTTLE( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), clock, 3000, + "calcInitialMotion : target velocity(%.3f[m/s]) is lower than engage velocity(%.3f[m/s]). ", + target_vel, engage_velocity_); + } + } + + // normal update: use closest in prev_output + initial_vel = prev_output_closest_point.longitudinal_velocity_mps; + initial_acc = prev_output_closest_point.acceleration_mps2; + return std::make_tuple(initial_vel, initial_acc); +} + +bool OptimizationBasedPlanner::checkHasReachedGoal( + const PlannerData & planner_data, const std::vector & stop_traj_points) +{ + // If goal is close and current velocity is low, we don't optimize trajectory + const auto closest_stop_dist = autoware::motion_utils::calcDistanceToForwardStopPoint( + stop_traj_points, planner_data.ego_pose, ego_nearest_param_.dist_threshold, + ego_nearest_param_.yaw_threshold); + if (closest_stop_dist && *closest_stop_dist < 0.5 && planner_data.ego_vel < 0.6) { + return true; + } + + return false; +} + +std::optional OptimizationBasedPlanner::getSBoundaries( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const std::vector & obstacles, const std::vector & time_vec) +{ + if (stop_traj_points.empty()) { + return std::nullopt; + } + + const auto traj_length = + calcTrajectoryLengthFromCurrentPose(stop_traj_points, planner_data.ego_pose); + if (!traj_length) { + return {}; + } + + SBoundaries s_boundaries(time_vec.size()); + for (size_t i = 0; i < s_boundaries.size(); ++i) { + s_boundaries.at(i).max_s = *traj_length; + } + + double min_slow_down_point_length = std::numeric_limits::max(); + std::optional min_slow_down_idx = {}; + for (size_t o_idx = 0; o_idx < obstacles.size(); ++o_idx) { + const auto & obj = obstacles.at(o_idx); + if (obj.collision_points.empty()) { + continue; + } + + // Step1 Get S boundary from the obstacle + const auto obj_s_boundaries = + getSBoundaries(planner_data, stop_traj_points, obj, time_vec, *traj_length); + if (!obj_s_boundaries) { + continue; + } + + // Step2 update s boundaries + for (size_t i = 0; i < obj_s_boundaries->size(); ++i) { + if (obj_s_boundaries->at(i).max_s < s_boundaries.at(i).max_s) { + s_boundaries.at(i) = obj_s_boundaries->at(i); + } + } + + // Step3 search nearest obstacle to follow for rviz marker + const double obj_vel = std::abs(obj.velocity); + const double rss_dist = calcRSSDistance(planner_data.ego_vel, obj_vel); + + const auto & safe_distance_margin = longitudinal_info_.safe_distance_margin; + const double ego_obj_length = autoware::motion_utils::calcSignedArcLength( + stop_traj_points, planner_data.ego_pose.position, obj.collision_points.front().point); + const double slow_down_point_length = ego_obj_length - (rss_dist + safe_distance_margin); + + if (slow_down_point_length < min_slow_down_point_length) { + min_slow_down_point_length = slow_down_point_length; + min_slow_down_idx = o_idx; + } + } + + // Publish wall marker for slowing down or stopping + if (min_slow_down_idx) { + const auto & current_time = planner_data.current_time; + + const auto marker_pose = autoware::motion_utils::calcLongitudinalOffsetPose( + stop_traj_points, planner_data.ego_pose.position, min_slow_down_point_length); + + if (marker_pose) { + MarkerArray wall_msg; + + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + marker_pose.value(), "obstacle to follow", current_time, 0); + autoware::universe_utils::appendMarkerArray(markers, &wall_msg); + + // publish rviz marker + debug_wall_marker_pub_->publish(wall_msg); + } + } + + return s_boundaries; +} + +std::optional OptimizationBasedPlanner::getSBoundaries( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const CruiseObstacle & object, const std::vector & time_vec, const double traj_length) +{ + if (object.collision_points.empty()) { + return {}; + } + + const bool onEgoTrajectory = + checkOnTrajectory(planner_data, stop_traj_points, object.collision_points.front()); + const auto & safe_distance_margin = longitudinal_info_.safe_distance_margin; + + // If the object is on the current ego trajectory, + // we assume the object travels along ego trajectory + if (onEgoTrajectory) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner::OptimizationBasedPlanner"), + "On Trajectory Object"); + + return getSBoundariesForOnTrajectoryObject( + planner_data, time_vec, safe_distance_margin, object, traj_length); + } + + // Ignore low velocity objects that are not on the trajectory + return getSBoundariesForOffTrajectoryObject( + planner_data, time_vec, safe_distance_margin, object, traj_length); +} + +std::optional OptimizationBasedPlanner::getSBoundariesForOnTrajectoryObject( + const PlannerData & planner_data, const std::vector & time_vec, + const double safety_distance, const CruiseObstacle & object, const double traj_length) +{ + const double & min_object_accel_for_rss = longitudinal_info_.min_object_accel_for_rss; + + SBoundaries s_boundaries(time_vec.size()); + for (size_t i = 0; i < s_boundaries.size(); ++i) { + s_boundaries.at(i).max_s = traj_length; + } + + const double v_obj = std::abs(object.velocity); + + const double dist_to_collision_point = + calcDistanceToCollisionPoint(planner_data, object.collision_points.front().point); + + double current_s_obj = std::max(dist_to_collision_point - safety_distance, 0.0); + const double current_v_obj = 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); + s_boundaries.front().is_object = true; + for (size_t i = 1; i < time_vec.size(); ++i) { + const double dt = time_vec.at(i) - time_vec.at(i - 1); + current_s_obj = current_s_obj + current_v_obj * dt; + + const double s_upper_bound = + current_s_obj + (current_v_obj * current_v_obj) / (2 * std::fabs(min_object_accel_for_rss)); + s_boundaries.at(i).max_s = std::clamp(s_upper_bound, 0.0, s_boundaries.at(i).max_s); + s_boundaries.at(i).is_object = true; + } + + return s_boundaries; +} + +std::optional OptimizationBasedPlanner::getSBoundariesForOffTrajectoryObject( + const PlannerData & planner_data, const std::vector & time_vec, + const double safety_distance, const CruiseObstacle & object, const double traj_length) +{ + const auto & current_time = planner_data.current_time; + const double & min_object_accel_for_rss = longitudinal_info_.min_object_accel_for_rss; + + const double v_obj = std::abs(object.velocity); + + SBoundaries s_boundaries(time_vec.size()); + for (size_t i = 0; i < s_boundaries.size(); ++i) { + s_boundaries.at(i).max_s = traj_length; + } + + for (const auto & collision_point : object.collision_points) { + const double object_time = (rclcpp::Time(collision_point.stamp) - current_time).seconds(); + if (object_time < 0) { + // Ignore Past Positions + continue; + } + + const double dist_to_collision_point = + calcDistanceToCollisionPoint(planner_data, collision_point.point); + if (!dist_to_collision_point) { + continue; + } + + const double current_s_obj = std::max(dist_to_collision_point - safety_distance, 0.0); + const double s_upper_bound = + current_s_obj + (v_obj * v_obj) / (2 * std::fabs(min_object_accel_for_rss)); + + size_t object_time_segment_idx = 0; + for (size_t i = 0; i < time_vec.size() - 1; ++i) { + if (time_vec.at(i) < object_time && object_time < time_vec.at(i + 1)) { + object_time_segment_idx = i; + break; + } + } + + for (size_t i = 0; i <= object_time_segment_idx + 1; ++i) { + if (time_vec.at(i) < object_time && s_upper_bound < s_boundaries.at(i).max_s) { + s_boundaries.at(i).max_s = std::max(0.0, s_upper_bound); + s_boundaries.at(i).is_object = true; + } + } + } + + return s_boundaries; +} + +bool OptimizationBasedPlanner::checkOnTrajectory( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const PointWithStamp & point) +{ + // If the collision point is in the future, we return false + if ((rclcpp::Time(point.stamp) - planner_data.current_time).seconds() > 0.1) { + return false; + } + + const double lateral_offset = + std::fabs(autoware::motion_utils::calcLateralOffset(stop_traj_points, point.point)); + + if (lateral_offset < vehicle_info_.max_lateral_offset_m + 0.1) { + return true; + } + + return false; +} + +std::optional OptimizationBasedPlanner::calcTrajectoryLengthFromCurrentPose( + const std::vector & traj_points, const geometry_msgs::msg::Pose & ego_pose) +{ + const size_t ego_segment_idx = ego_nearest_param_.findSegmentIndex(traj_points, ego_pose); + + const double traj_length = autoware::motion_utils::calcSignedArcLength( + traj_points, ego_pose.position, ego_segment_idx, traj_points.size() - 1); + + const auto dist_to_closest_stop_point = autoware::motion_utils::calcDistanceToForwardStopPoint( + traj_points, ego_pose, ego_nearest_param_.dist_threshold, ego_nearest_param_.yaw_threshold); + if (dist_to_closest_stop_point) { + return std::min(traj_length, dist_to_closest_stop_point.value()); + } + + return traj_length; +} + +geometry_msgs::msg::Pose OptimizationBasedPlanner::transformBaseLink2Center( + const geometry_msgs::msg::Pose & pose_base_link) +{ + tf2::Transform tf_map2base; + tf2::fromMsg(pose_base_link, tf_map2base); + + // set vertices at map coordinate + tf2::Vector3 base2center; + base2center.setX(std::abs(vehicle_info_.vehicle_length_m / 2.0 - vehicle_info_.rear_overhang_m)); + base2center.setY(0.0); + base2center.setZ(0.0); + base2center.setW(1.0); + + // transform vertices from map coordinate to object coordinate + const auto map2center = tf_map2base * base2center; + + geometry_msgs::msg::Pose center_pose; + center_pose.position = + autoware::universe_utils::createPoint(map2center.x(), map2center.y(), map2center.z()); + center_pose.orientation = pose_base_link.orientation; + + return center_pose; +} + +std::optional +OptimizationBasedPlanner::processOptimizedResult( + const double v0, const VelocityOptimizer::OptimizationResult & opt_result, const double offset) +{ + if ( + opt_result.t.empty() || opt_result.s.empty() || opt_result.v.empty() || opt_result.a.empty() || + opt_result.j.empty()) { + return std::nullopt; + } + + size_t break_id = opt_result.s.size(); + VelocityOptimizer::OptimizationResult processed_result; + processed_result.t.push_back(0.0); + processed_result.s.push_back(offset); + processed_result.v.push_back(v0); + processed_result.a.push_back(opt_result.a.front()); + processed_result.j.push_back(opt_result.j.front()); + + for (size_t i = 1; i < opt_result.s.size(); ++i) { + const double prev_s = processed_result.s.back(); + const double current_s = std::max(opt_result.s.at(i), 0.0) + offset; + const double current_v = opt_result.v.at(i); + if (prev_s >= current_s) { + processed_result.v.back() = 0.0; + break_id = i; + break; + } else if (current_v < ZERO_VEL_THRESHOLD) { + break_id = i; + break; + } else if (std::fabs(current_s - prev_s) < CLOSE_S_DIST_THRESHOLD) { + processed_result.v.back() = current_v; + processed_result.s.back() = current_s; + continue; + } + + processed_result.t.push_back(opt_result.t.at(i)); + processed_result.s.push_back(current_s); + processed_result.v.push_back(current_v); + processed_result.a.push_back(opt_result.a.at(i)); + processed_result.j.push_back(opt_result.j.at(i)); + } + + // Padding Zero Velocity after break id + for (size_t i = break_id; i < opt_result.s.size(); ++i) { + const double prev_s = processed_result.s.back(); + const double current_s = std::max(opt_result.s.at(i), 0.0) + offset; + if (prev_s >= current_s) { + processed_result.v.back() = 0.0; + continue; + } else if (std::fabs(current_s - prev_s) < CLOSE_S_DIST_THRESHOLD) { + processed_result.v.back() = 0.0; + processed_result.s.back() = current_s; + continue; + } + processed_result.t.push_back(opt_result.t.at(i)); + processed_result.s.push_back(current_s); + processed_result.v.push_back(0.0); + processed_result.a.push_back(0.0); + processed_result.j.push_back(0.0); + } + + return processed_result; +} + +void OptimizationBasedPlanner::publishDebugTrajectory( + const PlannerData & planner_data, const double offset, const std::vector & time_vec, + const SBoundaries & s_boundaries, const VelocityOptimizer::OptimizationResult & opt_result) +{ + const auto & current_time = planner_data.current_time; + // Publish optimized result and boundary + Trajectory boundary_traj; + boundary_traj.header.stamp = current_time; + boundary_traj.points.resize(s_boundaries.size()); + double boundary_s_max = 0.0; + for (size_t i = 0; i < s_boundaries.size(); ++i) { + const double bound_s = s_boundaries.at(i).max_s; + const double bound_t = time_vec.at(i); + boundary_traj.points.at(i).pose.position.x = bound_s; + boundary_traj.points.at(i).pose.position.y = bound_t; + boundary_s_max = std::max(bound_s, boundary_s_max); + } + boundary_pub_->publish(boundary_traj); + + Trajectory optimized_sv_traj; + optimized_sv_traj.header.stamp = current_time; + optimized_sv_traj.points.resize(opt_result.s.size()); + for (size_t i = 0; i < opt_result.s.size(); ++i) { + const double s = opt_result.s.at(i); + const double v = opt_result.v.at(i); + optimized_sv_traj.points.at(i).pose.position.x = s + offset; + optimized_sv_traj.points.at(i).pose.position.y = v; + } + optimized_sv_pub_->publish(optimized_sv_traj); + + Trajectory optimized_st_graph; + optimized_st_graph.header.stamp = current_time; + optimized_st_graph.points.resize(opt_result.s.size()); + for (size_t i = 0; i < opt_result.s.size(); ++i) { + const double bound_s = opt_result.s.at(i); + const double bound_t = opt_result.t.at(i); + optimized_st_graph.points.at(i).pose.position.x = bound_s; + optimized_st_graph.points.at(i).pose.position.y = bound_t; + } + optimized_st_graph_pub_->publish(optimized_st_graph); +} diff --git a/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp new file mode 100644 index 0000000000000..39a35204cda8f --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/src/optimization_based_planner/velocity_optimizer.cpp @@ -0,0 +1,284 @@ +// 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 "autoware/obstacle_cruise_planner/optimization_based_planner/velocity_optimizer.hpp" + +#include + +#include +#include +#include + +VelocityOptimizer::VelocityOptimizer( + const double max_s_weight, const double max_v_weight, const double over_s_safety_weight, + const double over_s_ideal_weight, const double over_v_weight, const double over_a_weight, + const double over_j_weight) +: max_s_weight_(max_s_weight), + max_v_weight_(max_v_weight), + over_s_safety_weight_(over_s_safety_weight), + over_s_ideal_weight_(over_s_ideal_weight), + over_v_weight_(over_v_weight), + over_a_weight_(over_a_weight), + over_j_weight_(over_j_weight) +{ + qp_solver_.updateMaxIter(200000); + qp_solver_.updateRhoInterval(0); // 0 means automatic + qp_solver_.updateEpsRel(1.0e-4); // def: 1.0e-4 + qp_solver_.updateEpsAbs(1.0e-8); // def: 1.0e-4 + qp_solver_.updateVerbose(false); +} + +VelocityOptimizer::OptimizationResult VelocityOptimizer::optimize(const OptimizationData & data) +{ + const std::vector time_vec = data.time_vec; + const size_t N = time_vec.size(); + const double s0 = data.s0; + const double v0 = data.v0; + const double a0 = data.a0; + const double v_max = std::max(data.v_max, 0.1); + const double a_max = data.a_max; + const double a_min = data.a_min; + const double limit_a_max = data.limit_a_max; + const double limit_a_min = data.limit_a_min; + const double limit_j_max = data.limit_j_max; + const double limit_j_min = data.limit_j_min; + const double j_max = data.j_max; + const double j_min = data.j_min; + const double a_range = std::max(a_max - a_min, 0.1); + const double j_range = std::max(j_max - j_min, 0.1); + const double t_dangerous = data.t_dangerous; + const double t_idling = data.idling_time; + const auto s_boundary = data.s_boundary; + + // Variables: s_i, v_i, a_i, j_i, over_s_safety_i, over_s_ideal_i, over_v_i, over_a_i, over_j_i + const int IDX_S0 = 0; + const int IDX_V0 = N; + const int IDX_A0 = 2 * N; + const int IDX_J0 = 3 * N; + const int IDX_OVER_S_SAFETY0 = 4 * N; + const int IDX_OVER_S_IDEAL0 = 5 * N; + const int IDX_OVER_V0 = 6 * N; + const int IDX_OVER_A0 = 7 * N; + const int IDX_OVER_J0 = 8 * N; + const int l_variables = 9 * N; + const int l_constraints = 7 * N + 3 * (N - 1) + 3; + + // the matrix size depends on constraint numbers. + Eigen::MatrixXd A = Eigen::MatrixXd::Zero(l_constraints, l_variables); + std::vector lower_bound(l_constraints, 0.0); + std::vector upper_bound(l_constraints, 0.0); + + // Object Variables + Eigen::MatrixXd P = Eigen::MatrixXd::Zero(l_variables, l_variables); + std::vector q(l_variables, 0.0); + + // Object Function + // min w_s*(s_i - s_ideal_i)^2 + w_v * v0 * ((v_i - v_max)^2 / v_max^2) + // + over_s_safety^2 + over_s_ideal^2 + over_v_ideal^2 + over_a_ideal^2 + // + over_j_ideal^2 + constexpr double MINIMUM_MAX_S_BOUND = 0.01; + for (size_t i = 0; i < N; ++i) { + const double dt = + i < N - 1 ? time_vec.at(i + 1) - time_vec.at(i) : time_vec.at(N - 1) - time_vec.at(N - 2); + const double max_s = std::max(s_boundary.at(i).max_s, MINIMUM_MAX_S_BOUND); + const double v_coeff = v0 / (2 * std::fabs(a_min)) + t_idling; + if (s_boundary.at(i).is_object) { + q.at(IDX_S0 + i) += -max_s_weight_ / max_s * dt; + q.at(IDX_V0 + i) += -max_s_weight_ / max_s * v_coeff * dt; + } else { + q.at(IDX_S0 + i) += -max_s_weight_ / max_s * dt; + } + + q.at(IDX_V0 + i) += -max_v_weight_ / v_max * dt; + } + + for (size_t i = 0; i < N; ++i) { + const double dt = + i < N - 1 ? time_vec.at(i + 1) - time_vec.at(i) : time_vec.at(N - 1) - time_vec.at(N - 2); + const double max_s = std::max(s_boundary.at(i).max_s, MINIMUM_MAX_S_BOUND); + P(IDX_OVER_S_SAFETY0 + i, IDX_OVER_S_SAFETY0 + i) += + over_s_safety_weight_ / (max_s * max_s) * dt; + P(IDX_OVER_S_IDEAL0 + i, IDX_OVER_S_IDEAL0 + i) += over_s_ideal_weight_ / (max_s * max_s) * dt; + P(IDX_OVER_V0 + i, IDX_OVER_V0 + i) += over_v_weight_ / (v_max * v_max) * dt; + P(IDX_OVER_A0 + i, IDX_OVER_A0 + i) += over_a_weight_ / a_range * dt; + P(IDX_OVER_J0 + i, IDX_OVER_J0 + i) += over_j_weight_ / j_range * dt; + + if (s_boundary.at(i).is_object) { + const double v_coeff = v0 / (2 * std::fabs(a_min)) + t_idling; + P(IDX_S0 + i, IDX_S0 + i) += max_s_weight_ / (max_s * max_s) * dt; + P(IDX_V0 + i, IDX_V0 + i) += max_s_weight_ / (max_s * max_s) * v_coeff * v_coeff * dt; + P(IDX_S0 + i, IDX_V0 + i) += max_s_weight_ / (max_s * max_s) * v_coeff * dt; + P(IDX_V0 + i, IDX_S0 + i) += max_s_weight_ / (max_s * max_s) * v_coeff * dt; + } else { + P(IDX_S0 + i, IDX_S0 + i) += max_s_weight_ / (max_s * max_s) * dt; + } + + P(IDX_V0 + i, IDX_V0 + i) += max_v_weight_ / (v_max * v_max) * dt; + } + + // Constraint + size_t constr_idx = 0; + + // Safety Position Constraint: s_boundary_min < s_i + v_i*t_dangerous + v0*v_i/(2*|a_min|) - + // over_s_safety_i < s_boundary_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + const double v_coeff = v0 / (2 * std::fabs(a_min)) + t_dangerous; + A(constr_idx, IDX_S0 + i) = 1.0; // s_i + if (s_boundary.at(i).is_object) { + A(constr_idx, IDX_V0 + i) = v_coeff; // v_i * (t_dangerous + v0/(2*|a_min|)) + } + A(constr_idx, IDX_OVER_S_SAFETY0 + i) = -1.0; // over_s_safety_i + upper_bound.at(constr_idx) = s_boundary.at(i).max_s; + lower_bound.at(constr_idx) = 0.0; + } + + // Ideal Position Constraint: s_boundary_min < s_i + v_i * t_idling + v0*v_i/(2*|a_min|) - + // over_s_ideal_i < s_boundary_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + const double v_coeff = v0 / (2 * std::fabs(a_min)) + t_idling; + A(constr_idx, IDX_S0 + i) = 1.0; // s_i + if (s_boundary.at(i).is_object) { + A(constr_idx, IDX_V0 + i) = v_coeff; // v_i * (t_idling + v0/(2*|a_min|)) + } + A(constr_idx, IDX_OVER_S_IDEAL0 + i) = -1.0; // over_s_ideal_i + upper_bound.at(constr_idx) = s_boundary.at(i).max_s; + lower_bound.at(constr_idx) = 0.0; + } + + // Soft Velocity Constraint: 0 < v_i - over_v_i < v_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_V0 + i) = 1.0; // v_i + A(constr_idx, IDX_OVER_V0 + i) = -1.0; // over_v_i + upper_bound.at(constr_idx) = i == N - 1 ? 0.0 : v_max; + lower_bound.at(constr_idx) = 0.0; + } + + // Soft Acceleration Constraint: a_min < a_i - over_a_i < a_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_A0 + i) = 1.0; // a_i + A(constr_idx, IDX_OVER_A0 + i) = -1.0; // over_a_i + upper_bound.at(constr_idx) = i == N - 1 ? 0.0 : a_max; + lower_bound.at(constr_idx) = i == N - 1 ? 0.0 : a_min; + } + + // Hard Acceleration Constraint: limit_a_min < a_i < a_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_A0 + i) = 1.0; // a_i + upper_bound.at(constr_idx) = limit_a_max; + lower_bound.at(constr_idx) = limit_a_min; + } + + // Soft Jerk Constraint: j_min < j_i - over_j_i < j_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_J0 + i) = 1.0; // j_i + A(constr_idx, IDX_OVER_J0 + i) = -1.0; // over_j_i + upper_bound.at(constr_idx) = i == N - 1 ? 0.0 : j_max; + lower_bound.at(constr_idx) = i == N - 1 ? 0.0 : j_min; + } + + // Hard Jerk Constraint: limit_j_min < j_i < limit_j_max + for (size_t i = 0; i < N; ++i, ++constr_idx) { + A(constr_idx, IDX_J0 + i) = 1.0; // j_i + upper_bound.at(constr_idx) = limit_j_max; + lower_bound.at(constr_idx) = limit_j_min; + } + + // Dynamic Constraint + // s_i+1 = s_i + v_i * dt + 0.5 * a_i * dt^2 + 1/6 * j_i * dt^3 + for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { + const double dt = time_vec.at(i + 1) - time_vec.at(i); + A(constr_idx, IDX_S0 + i + 1) = 1.0; // s_i+1 + A(constr_idx, IDX_S0 + i) = -1.0; // -s_i + A(constr_idx, IDX_V0 + i) = -dt; // -v_i*dt + A(constr_idx, IDX_A0 + i) = -0.5 * dt * dt; // -0.5 * a_i * dt^2 + A(constr_idx, IDX_J0 + i) = -1.0 / 6.0 * dt * dt * dt; // -1.0/6.0 * j_i * dt^3 + upper_bound.at(constr_idx) = 0.0; + lower_bound.at(constr_idx) = 0.0; + } + + // v_i+1 = v_i + a_i * dt + 0.5 * j_i * dt^2 + for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { + const double dt = time_vec.at(i + 1) - time_vec.at(i); + A(constr_idx, IDX_V0 + i + 1) = 1.0; // v_i+1 + A(constr_idx, IDX_V0 + i) = -1.0; // -v_i + A(constr_idx, IDX_A0 + i) = -dt; // -a_i * dt + A(constr_idx, IDX_J0 + i) = -0.5 * dt * dt; // -0.5 * j_i * dt^2 + upper_bound.at(constr_idx) = 0.0; + lower_bound.at(constr_idx) = 0.0; + } + + // a_i+1 = a_i + j_i * dt + for (size_t i = 0; i < N - 1; ++i, ++constr_idx) { + const double dt = time_vec.at(i + 1) - time_vec.at(i); + A(constr_idx, IDX_A0 + i + 1) = 1.0; // a_i+1 + A(constr_idx, IDX_A0 + i) = -1.0; // -a_i + A(constr_idx, IDX_J0 + i) = -dt; // -j_i * dt + upper_bound.at(constr_idx) = 0.0; + lower_bound.at(constr_idx) = 0.0; + } + + // initial condition + { + A(constr_idx, IDX_S0) = 1.0; // s0 + upper_bound[constr_idx] = s0; + lower_bound[constr_idx] = s0; + ++constr_idx; + + A(constr_idx, IDX_V0) = 1.0; // v0 + upper_bound[constr_idx] = v0; + lower_bound[constr_idx] = v0; + ++constr_idx; + + A(constr_idx, IDX_A0) = 1.0; // a0 + upper_bound[constr_idx] = a0; + lower_bound[constr_idx] = a0; + } + + // execute optimization + const auto result = qp_solver_.optimize(P, A, q, lower_bound, upper_bound); + const std::vector optval = std::get<0>(result); + + const int status_val = std::get<3>(result); + if (status_val != 1) + std::cerr << "optimization failed : " << qp_solver_.getStatusMessage().c_str() << std::endl; + + const auto has_nan = + std::any_of(optval.begin(), optval.end(), [](const auto v) { return std::isnan(v); }); + if (has_nan) std::cerr << "optimization failed : result contains NaN values\n"; + + OptimizationResult optimized_result; + const auto is_optimization_failed = status_val != 1 || has_nan; + if (!is_optimization_failed) { + std::vector opt_time = time_vec; + std::vector opt_pos(N); + std::vector opt_vel(N); + std::vector opt_acc(N); + std::vector opt_jerk(N); + for (size_t i = 0; i < N; ++i) { + opt_pos.at(i) = optval.at(IDX_S0 + i); + opt_vel.at(i) = std::max(optval.at(IDX_V0 + i), 0.0); + opt_acc.at(i) = optval.at(IDX_A0 + i); + opt_jerk.at(i) = optval.at(IDX_J0 + i); + } + opt_vel.back() = 0.0; + + optimized_result.t = opt_time; + optimized_result.s = opt_pos; + optimized_result.v = opt_vel; + optimized_result.a = opt_acc; + optimized_result.j = opt_jerk; + } + + return optimized_result; +} diff --git a/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp new file mode 100644 index 0000000000000..20b564addf149 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/src/pid_based_planner/pid_based_planner.cpp @@ -0,0 +1,724 @@ +// 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 "autoware/obstacle_cruise_planner/pid_based_planner/pid_based_planner.hpp" + +#include "autoware/interpolation/spline_interpolation.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/obstacle_cruise_planner/utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" +#include "autoware/universe_utils/ros/update_param.hpp" + +#include "tier4_planning_msgs/msg/velocity_limit.hpp" + +#include +#include +#include +#include + +using autoware::signal_processing::LowpassFilter1d; + +namespace +{ +VelocityLimit createVelocityLimitMsg( + const rclcpp::Time & current_time, const double vel, const double acc, const double max_jerk, + const double min_jerk) +{ + VelocityLimit msg; + msg.stamp = current_time; + msg.sender = "obstacle_cruise_planner.cruise"; + msg.use_constraints = true; + + msg.max_velocity = vel; + if (acc < 0) { + msg.constraints.min_acceleration = acc; + } + msg.constraints.max_jerk = max_jerk; + msg.constraints.min_jerk = min_jerk; + + return msg; +} + +template +T getSign(const T val) +{ + if (0 < val) { + return static_cast(1); + } else if (val < 0) { + return static_cast(-1); + } + return static_cast(0); +} +} // namespace + +PIDBasedPlanner::PIDBasedPlanner( + rclcpp::Node & node, const LongitudinalInfo & longitudinal_info, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info, + const EgoNearestParam & ego_nearest_param, const std::shared_ptr debug_data_ptr) +: PlannerInterface(node, longitudinal_info, vehicle_info, ego_nearest_param, debug_data_ptr) +{ + min_accel_during_cruise_ = + node.declare_parameter("cruise.pid_based_planner.min_accel_during_cruise"); + + use_velocity_limit_based_planner_ = + node.declare_parameter("cruise.pid_based_planner.use_velocity_limit_based_planner"); + + { // velocity limit based planner + const double kp = + node.declare_parameter("cruise.pid_based_planner.velocity_limit_based_planner.kp"); + const double ki = + node.declare_parameter("cruise.pid_based_planner.velocity_limit_based_planner.ki"); + const double kd = + node.declare_parameter("cruise.pid_based_planner.velocity_limit_based_planner.kd"); + velocity_limit_based_planner_param_.pid_vel_controller = + std::make_unique(kp, ki, kd); + + velocity_limit_based_planner_param_.output_ratio_during_accel = node.declare_parameter( + "cruise.pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel"); + velocity_limit_based_planner_param_.vel_to_acc_weight = node.declare_parameter( + "cruise.pid_based_planner.velocity_limit_based_planner.vel_to_acc_weight"); + + velocity_limit_based_planner_param_.enable_jerk_limit_to_output_acc = + node.declare_parameter( + "cruise.pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc"); + + velocity_limit_based_planner_param_.disable_target_acceleration = node.declare_parameter( + "cruise.pid_based_planner.velocity_limit_based_planner.disable_target_acceleration"); + } + + { // velocity insertion based planner + // pid controller for acc + const double kp_acc = node.declare_parameter( + "cruise.pid_based_planner.velocity_insertion_based_planner.kp_acc"); + const double ki_acc = node.declare_parameter( + "cruise.pid_based_planner.velocity_insertion_based_planner.ki_acc"); + const double kd_acc = node.declare_parameter( + "cruise.pid_based_planner.velocity_insertion_based_planner.kd_acc"); + velocity_insertion_based_planner_param_.pid_acc_controller = + std::make_unique(kp_acc, ki_acc, kd_acc); + + // pid controller for jerk + const double kp_jerk = node.declare_parameter( + "cruise.pid_based_planner.velocity_insertion_based_planner.kp_jerk"); + const double ki_jerk = node.declare_parameter( + "cruise.pid_based_planner.velocity_insertion_based_planner.ki_jerk"); + const double kd_jerk = node.declare_parameter( + "cruise.pid_based_planner.velocity_insertion_based_planner.kd_jerk"); + velocity_insertion_based_planner_param_.pid_jerk_controller = + std::make_unique(kp_jerk, ki_jerk, kd_jerk); + + velocity_insertion_based_planner_param_.output_acc_ratio_during_accel = + node.declare_parameter( + "cruise.pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel"); + velocity_insertion_based_planner_param_.output_jerk_ratio_during_accel = + node.declare_parameter( + "cruise.pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel"); + + velocity_insertion_based_planner_param_ + .enable_jerk_limit_to_output_acc = node.declare_parameter( + "cruise.pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc"); + } + + min_cruise_target_vel_ = + node.declare_parameter("cruise.pid_based_planner.min_cruise_target_vel"); + time_to_evaluate_rss_ = + node.declare_parameter("cruise.pid_based_planner.time_to_evaluate_rss"); + const auto error_function_type = + node.declare_parameter("cruise.pid_based_planner.error_function_type"); + error_func_ = [&]() -> std::function { + if (error_function_type == "linear") { + return [](const double val) { return val; }; + } else if (error_function_type == "quadratic") { + return [](const double val) { return getSign(val) * std::pow(val, 2); }; + } + throw std::domain_error("error function type is not supported"); + }(); + + // low pass filter + const double lpf_normalized_error_cruise_dist_gain = node.declare_parameter( + "cruise.pid_based_planner.lpf_normalized_error_cruise_dist_gain"); + lpf_normalized_error_cruise_dist_ptr_ = + std::make_shared(lpf_normalized_error_cruise_dist_gain); + lpf_dist_to_obstacle_ptr_ = std::make_shared(0.5); + lpf_obstacle_vel_ptr_ = std::make_shared(0.5); + lpf_error_cruise_dist_ptr_ = std::make_shared(0.5); + lpf_output_vel_ptr_ = std::make_shared(0.5); + lpf_output_acc_ptr_ = std::make_shared(0.5); + lpf_output_jerk_ptr_ = std::make_shared(0.5); +} + +std::vector PIDBasedPlanner::generateCruiseTrajectory( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const std::vector & obstacles, std::optional & vel_limit) +{ + stop_watch_.tic(__func__); + cruise_planning_debug_info_.reset(); + + // calc obstacles to cruise + const auto cruise_obstacle_info = calcObstacleToCruise(planner_data, obstacles); + + // plan cruise + const auto cruise_traj_points = + planCruise(planner_data, stop_traj_points, vel_limit, cruise_obstacle_info); + + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_calculation_time_info_, + " %s := %f [ms]", __func__, calculation_time); + + prev_traj_points_ = cruise_traj_points; + return cruise_traj_points; +} + +std::optional PIDBasedPlanner::calcObstacleToCruise( + const PlannerData & planner_data, const std::vector & obstacles) +{ + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.ego_vel); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::EGO_ACCELERATION, planner_data.ego_acc); + + auto modified_target_obstacles = obstacles; // TODO(murooka) what is this variable? + + // search highest probability obstacle for cruise + std::optional cruise_obstacle_info; + for (size_t o_idx = 0; o_idx < obstacles.size(); ++o_idx) { + const auto & obstacle = obstacles.at(o_idx); + + if (obstacle.collision_points.empty()) { + continue; + } + + // NOTE: from ego's front to obstacle's back + // TODO(murooka) this is not dist to obstacle + const double dist_to_obstacle = + calcDistanceToCollisionPoint(planner_data, obstacle.collision_points.front().point) + + (obstacle.velocity - planner_data.ego_vel) * time_to_evaluate_rss_; + + // calculate distance between ego and obstacle based on RSS + const double target_dist_to_obstacle = calcRSSDistance( + planner_data.ego_vel, obstacle.velocity, longitudinal_info_.safe_distance_margin); + + // calculate error distance and normalized one + const double error_cruise_dist = dist_to_obstacle - target_dist_to_obstacle; + if (cruise_obstacle_info) { + if (error_cruise_dist > cruise_obstacle_info->error_cruise_dist) { + continue; + } + } + cruise_obstacle_info = + CruiseObstacleInfo(obstacle, error_cruise_dist, dist_to_obstacle, target_dist_to_obstacle); + } + + if (!cruise_obstacle_info) { // if obstacle to cruise was not found + lpf_dist_to_obstacle_ptr_->reset(); + lpf_obstacle_vel_ptr_->reset(); + lpf_error_cruise_dist_ptr_->reset(); + return {}; + } + + // if obstacle to cruise was found + { // debug data + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_RELATIVE_EGO_VELOCITY, + planner_data.ego_vel - cruise_obstacle_info->obstacle.velocity); + const double non_zero_relative_vel = [&]() { + const double relative_vel = planner_data.ego_vel - cruise_obstacle_info->obstacle.velocity; + constexpr double epsilon = 0.001; + if (epsilon < std::abs(relative_vel)) { + return relative_vel; + } + return getSign(relative_vel) * epsilon; + }(); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TIME_TO_COLLISION, + cruise_obstacle_info->dist_to_obstacle / non_zero_relative_vel); + } + + { // dist to obstacle + const double raw_dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle; + const double filtered_dist_to_obstacle = + lpf_dist_to_obstacle_ptr_->filter(cruise_obstacle_info->dist_to_obstacle); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_RAW, raw_dist_to_obstacle); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_DISTANCE_FILTERED, + filtered_dist_to_obstacle); + + cruise_obstacle_info->dist_to_obstacle = filtered_dist_to_obstacle; + } + + { // obstacle velocity + const double raw_obstacle_velocity = cruise_obstacle_info->obstacle.velocity; + const double filtered_obstacle_velocity = lpf_obstacle_vel_ptr_->filter(raw_obstacle_velocity); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_RAW, raw_obstacle_velocity); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_CURRENT_OBSTACLE_VELOCITY_FILTERED, + filtered_obstacle_velocity); + + cruise_obstacle_info->obstacle.velocity = filtered_obstacle_velocity; + } + + { // error distance for cruising + const double raw_error_cruise_dist = cruise_obstacle_info->error_cruise_dist; + const double filtered_error_cruise_dist = + lpf_error_cruise_dist_ptr_->filter(cruise_obstacle_info->error_cruise_dist); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_RAW, raw_error_cruise_dist); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_ERROR_DISTANCE_FILTERED, filtered_error_cruise_dist); + + cruise_obstacle_info->error_cruise_dist = filtered_error_cruise_dist; + } + + { // target dist for cruising + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_OBSTACLE_DISTANCE, + cruise_obstacle_info->target_dist_to_obstacle); + } + + return cruise_obstacle_info; +} + +std::vector PIDBasedPlanner::planCruise( + const PlannerData & planner_data, const std::vector & stop_traj_points, + std::optional & vel_limit, + const std::optional & cruise_obstacle_info) +{ + // do cruise + if (cruise_obstacle_info) { + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_, + "cruise planning"); + + { // update debug marker + // virtual wall marker for cruise + const double error_cruise_dist = cruise_obstacle_info->error_cruise_dist; + const double dist_to_obstacle = cruise_obstacle_info->dist_to_obstacle; + const size_t ego_idx = findEgoIndex(stop_traj_points, planner_data.ego_pose); + const double abs_ego_offset = planner_data.is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + const double dist_to_rss_wall = + std::min(error_cruise_dist + abs_ego_offset, dist_to_obstacle + abs_ego_offset); + const size_t wall_idx = obstacle_cruise_utils::getIndexWithLongitudinalOffset( + stop_traj_points, dist_to_rss_wall, ego_idx); + + const std::string wall_reason_string = cruise_obstacle_info->obstacle.is_yield_obstacle + ? "obstacle cruise (yield)" + : "obstacle cruise"; + auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + stop_traj_points.at(wall_idx).pose, wall_reason_string, planner_data.current_time, 0); + // NOTE: use a different color from slow down one to visualize cruise and slow down + // separately. + markers.markers.front().color = + autoware::universe_utils::createMarkerColor(1.0, 0.6, 0.1, 0.5); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); + + // cruise obstacle + debug_data_ptr_->obstacles_to_cruise.push_back(cruise_obstacle_info->obstacle); + debug_data_ptr_->cruise_metrics = makeMetrics("PIDBasedPlanner", "cruise", planner_data); + + planning_factor_interface_->add( + stop_traj_points, planner_data.ego_pose, stop_traj_points.at(wall_idx).pose, + tier4_planning_msgs::msg::PlanningFactor::NONE, + tier4_planning_msgs::msg::SafetyFactorArray{}); + } + + // do cruise planning + if (!use_velocity_limit_based_planner_) { + const auto cruise_traj = + doCruiseWithTrajectory(planner_data, stop_traj_points, *cruise_obstacle_info); + return cruise_traj; + } + + vel_limit = doCruiseWithVelocityLimit(planner_data, *cruise_obstacle_info); + return stop_traj_points; + } + + // reset previous cruise data if adaptive cruise is not enabled + prev_target_acc_ = {}; + lpf_normalized_error_cruise_dist_ptr_->reset(); + lpf_output_acc_ptr_->reset(); + lpf_output_vel_ptr_->reset(); + lpf_output_jerk_ptr_->reset(); + + // delete marker + const auto markers = + autoware::motion_utils::createDeletedSlowDownVirtualWallMarker(planner_data.current_time, 0); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->cruise_wall_marker); + + return stop_traj_points; +} + +VelocityLimit PIDBasedPlanner::doCruiseWithVelocityLimit( + const PlannerData & planner_data, const CruiseObstacleInfo & cruise_obstacle_info) +{ + const auto & p = velocity_limit_based_planner_param_; + + const double filtered_normalized_error_cruise_dist = [&]() { + const double normalized_error_cruise_dist = + cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle; + return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist); + }(); + + const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist); + + // calculate target velocity with acceleration limit by PID controller + const double pid_output_vel = p.pid_vel_controller->calc(modified_error_cruise_dist); + const double additional_vel = [&]() { + if (modified_error_cruise_dist > 0) { + return pid_output_vel * p.output_ratio_during_accel; + } + return pid_output_vel; + }(); + + const double positive_target_vel = lpf_output_vel_ptr_->filter( + std::max(min_cruise_target_vel_, planner_data.ego_vel + additional_vel)); + + // calculate target acceleration + const double target_acc = [&]() { + if (p.disable_target_acceleration) { + return min_accel_during_cruise_; + } + + double target_acc = p.vel_to_acc_weight * additional_vel; + + // apply acc limit + target_acc = std::clamp( + target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); // apply acc limit + + if (!prev_target_acc_) { + return lpf_output_acc_ptr_->filter(target_acc); + } + + if (p.enable_jerk_limit_to_output_acc) { // apply jerk limit + const double jerk = (target_acc - *prev_target_acc_) / 0.1; // TODO(murooka) 0.1 + const double limited_jerk = + std::clamp(jerk, longitudinal_info_.min_jerk, longitudinal_info_.max_jerk); + target_acc = *prev_target_acc_ + limited_jerk * 0.1; + } + + return lpf_output_acc_ptr_->filter(target_acc); + }(); + prev_target_acc_ = target_acc; + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_VELOCITY, positive_target_vel); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc); + + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_, + "target_velocity %f", positive_target_vel); + + // set target longitudinal motion + const auto vel_limit = createVelocityLimitMsg( + planner_data.current_time, positive_target_vel, target_acc, longitudinal_info_.max_jerk, + longitudinal_info_.min_jerk); + + return vel_limit; +} + +std::vector PIDBasedPlanner::doCruiseWithTrajectory( + const PlannerData & planner_data, const std::vector & stop_traj_points, + const CruiseObstacleInfo & cruise_obstacle_info) +{ + const auto & p = velocity_insertion_based_planner_param_; + + const double filtered_normalized_error_cruise_dist = [&]() { + const double normalized_error_cruise_dist = + cruise_obstacle_info.error_cruise_dist / cruise_obstacle_info.dist_to_obstacle; + return lpf_normalized_error_cruise_dist_ptr_->filter(normalized_error_cruise_dist); + }(); + + const double modified_error_cruise_dist = error_func_(filtered_normalized_error_cruise_dist); + + // calculate target velocity with acceleration limit by PID controller + // calculate target acceleration + const double target_acc = [&]() { + double target_acc = p.pid_acc_controller->calc(modified_error_cruise_dist); + + if (0 < filtered_normalized_error_cruise_dist) { + target_acc *= p.output_acc_ratio_during_accel; + } + + target_acc = std::clamp( + target_acc, min_accel_during_cruise_, longitudinal_info_.max_accel); // apply acc limit + + if (!prev_target_acc_) { + return target_acc; + } + + if (p.enable_jerk_limit_to_output_acc) { + const double jerk = (target_acc - *prev_target_acc_) / 0.1; // TODO(murooka) 0.1 + const double limited_jerk = + std::clamp(jerk, longitudinal_info_.min_jerk, longitudinal_info_.max_jerk); + target_acc = *prev_target_acc_ + limited_jerk * 0.1; + } + + return target_acc; + }(); + prev_target_acc_ = target_acc; + + const double target_jerk_ratio = [&]() { + double target_jerk_ratio = p.pid_jerk_controller->calc(modified_error_cruise_dist); + + if (0 < filtered_normalized_error_cruise_dist) { + target_jerk_ratio *= p.output_jerk_ratio_during_accel; + } + + target_jerk_ratio = std::clamp(std::abs(target_jerk_ratio), 0.0, 1.0); + return target_jerk_ratio; + }(); + + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_ACCELERATION, target_acc); + cruise_planning_debug_info_.set( + CruisePlanningDebugInfo::TYPE::CRUISE_TARGET_JERK_RATIO, target_jerk_ratio); + + // set target longitudinal motion + const auto prev_traj_closest_point = [&]() -> TrajectoryPoint { + // if (smoothed_trajectory_ptr_) { + // return autoware::motion_utils::calcInterpolatedPoint( + // *smoothed_trajectory_ptr_, planner_data.ego_pose, nearest_dist_deviation_threshold_, + // nearest_yaw_deviation_threshold_); + // } + return ego_nearest_param_.calcInterpolatedPoint(prev_traj_points_, planner_data.ego_pose); + }(); + const double v0 = prev_traj_closest_point.longitudinal_velocity_mps; + const double a0 = prev_traj_closest_point.acceleration_mps2; + + auto cruise_traj_points = getAccelerationLimitedTrajectory( + stop_traj_points, planner_data.ego_pose, v0, a0, target_acc, target_jerk_ratio); + + const auto zero_vel_idx_opt = autoware::motion_utils::searchZeroVelocityIndex(cruise_traj_points); + if (!zero_vel_idx_opt) { + return cruise_traj_points; + } + + for (size_t i = zero_vel_idx_opt.value(); i < cruise_traj_points.size(); ++i) { + cruise_traj_points.at(i).longitudinal_velocity_mps = 0.0; + } + + return cruise_traj_points; +} + +std::vector PIDBasedPlanner::getAccelerationLimitedTrajectory( + const std::vector traj_points, const geometry_msgs::msg::Pose & start_pose, + const double v0, const double a0, const double target_acc, const double target_jerk_ratio) const +{ + // calculate vt function + const auto vt = [&]( + const double v0, const double a0, const double jerk, const double t, + const double target_acc) { + const double t1 = (target_acc - a0) / jerk; + + const double v = [&]() { + if (t < t1) { + return v0 + a0 * t + jerk * t * t / 2.0; + } + + const double v1 = v0 + a0 * t1 + jerk * t1 * t1 / 2.0; + return v1 + target_acc * (t - t1); + }(); + + constexpr double max_vel = 100.0; + return std::clamp(v, 0.0, max_vel); + }; + + // calculate sv graph + const double s_traj = autoware::motion_utils::calcArcLength(traj_points); + // const double t_max = 10.0; + const double s_max = 50.0; + const double max_sampling_num = 100.0; + + const double t_delta_min = 0.1; + const double s_delta_min = 0.1; + + // NOTE: v0 of obstacle_cruise_planner may be smaller than v0 of motion_velocity_smoother + // Therefore, we calculate v1 (velocity at next step) and use it for initial velocity. + const double v1 = v0; // + 0.1; // a0 * 0.1; // + jerk * 0.1 * 0.1 / 2.0; + const double a1 = a0; // + jerk * 0.1; + const double jerk = target_acc > a1 ? longitudinal_info_.max_jerk * target_jerk_ratio + : longitudinal_info_.min_jerk * target_jerk_ratio; + + double t_current = 0.0; + std::vector s_vec{0.0}; + std::vector v_vec{v1}; + for (double tmp_idx = 0.0; tmp_idx < max_sampling_num; ++tmp_idx) { + if (v_vec.back() <= 0.0) { + s_vec.push_back(s_vec.back() + s_delta_min); + v_vec.push_back(0.0); + } else { + const double v_current = vt( + v1, a1, jerk, t_current + t_delta_min, + target_acc); // TODO(murooka) + t_delta_min is not required + + const double s_delta = std::max(s_delta_min, v_current * t_delta_min); + const double s_current = s_vec.back() + s_delta; + + s_vec.push_back(s_current); + v_vec.push_back(v_current); + + const double t_delta = [&]() { + if (v_current <= 0) { + return 0.0; + } + + constexpr double t_delta_max = 100.0; // NOTE: to avoid computation explosion + return std::min(t_delta_max, s_delta / v_current); + }(); + + t_current += t_delta; + } + + if (s_traj < s_vec.back() /*|| t_max < t_current*/ || s_max < s_vec.back()) { + break; + } + } + + std::vector unique_s_vec{s_vec.front()}; + std::vector unique_v_vec{v_vec.front()}; + for (size_t i = 0; i < s_vec.size(); ++i) { + if (s_vec.at(i) == unique_s_vec.back()) { + continue; + } + + unique_s_vec.push_back(s_vec.at(i)); + unique_v_vec.push_back(v_vec.at(i)); + } + + if (unique_s_vec.size() < 2) { + return traj_points; + } + + auto acc_limited_traj_points = traj_points; + const size_t ego_seg_idx = findEgoIndex(acc_limited_traj_points, start_pose); + double sum_dist = 0.0; + for (size_t i = ego_seg_idx; i < acc_limited_traj_points.size(); ++i) { + if (i != ego_seg_idx) { + sum_dist += autoware::universe_utils::calcDistance2d( + acc_limited_traj_points.at(i - 1), acc_limited_traj_points.at(i)); + } + + const double vel = [&]() { + if (unique_s_vec.back() < sum_dist) { + return unique_v_vec.back(); + } + return autoware::interpolation::spline(unique_s_vec, unique_v_vec, {sum_dist}).front(); + }(); + + acc_limited_traj_points.at(i).longitudinal_velocity_mps = std::clamp( + vel, 0.0, + static_cast( + acc_limited_traj_points.at(i) + .longitudinal_velocity_mps)); // TODO(murooka) this assumes forward driving + } + + return acc_limited_traj_points; +} + +void PIDBasedPlanner::updateCruiseParam(const std::vector & parameters) +{ + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.min_accel_during_cruise", min_accel_during_cruise_); + + { // velocity limit based planner + auto & p = velocity_limit_based_planner_param_; + + double kp = p.pid_vel_controller->getKp(); + double ki = p.pid_vel_controller->getKi(); + double kd = p.pid_vel_controller->getKd(); + + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kp", kp); + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.velocity_limit_based_planner.ki", ki); + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.velocity_limit_based_planner.kd", kd); + p.pid_vel_controller->updateParam(kp, ki, kd); + + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.velocity_limit_based_planner.output_ratio_during_accel", + p.output_ratio_during_accel); + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.vel_to_acc_weight", p.vel_to_acc_weight); + + autoware::universe_utils::updateParam( + parameters, + "cruise.pid_based_planner.velocity_limit_based_planner.enable_jerk_limit_to_output_acc", + p.enable_jerk_limit_to_output_acc); + + autoware::universe_utils::updateParam( + parameters, + "cruise.pid_based_planner.velocity_limit_based_planner.disable_target_acceleration", + p.disable_target_acceleration); + } + + { // velocity insertion based planner + auto & p = velocity_insertion_based_planner_param_; + + // pid controller for acc + double kp_acc = p.pid_acc_controller->getKp(); + double ki_acc = p.pid_acc_controller->getKi(); + double kd_acc = p.pid_acc_controller->getKd(); + + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_acc", kp_acc); + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_acc", ki_acc); + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_acc", kd_acc); + p.pid_acc_controller->updateParam(kp_acc, ki_acc, kd_acc); + + // pid controller for jerk + double kp_jerk = p.pid_jerk_controller->getKp(); + double ki_jerk = p.pid_jerk_controller->getKi(); + double kd_jerk = p.pid_jerk_controller->getKd(); + + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kp_jerk", kp_jerk); + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.ki_jerk", ki_jerk); + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.velocity_insertion_based_planner.kd_jerk", kd_jerk); + p.pid_jerk_controller->updateParam(kp_jerk, ki_jerk, kd_jerk); + + autoware::universe_utils::updateParam( + parameters, + "cruise.pid_based_planner.velocity_insertion_based_planner.output_acc_ratio_during_accel", + p.output_acc_ratio_during_accel); + autoware::universe_utils::updateParam( + parameters, + "cruise.pid_based_planner.velocity_insertion_based_planner.output_jerk_ratio_during_accel", + p.output_jerk_ratio_during_accel); + + autoware::universe_utils::updateParam( + parameters, + "cruise.pid_based_planner.velocity_insertion_based_planner.enable_jerk_limit_to_output_acc", + p.enable_jerk_limit_to_output_acc); + } + + // min_cruise_target_vel + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.min_cruise_target_vel", min_cruise_target_vel_); + autoware::universe_utils::updateParam( + parameters, "cruise.pid_based_planner.time_to_evaluate_rss", time_to_evaluate_rss_); +} diff --git a/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp new file mode 100644 index 0000000000000..b32d7f3a1bcbb --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/src/planner_interface.cpp @@ -0,0 +1,865 @@ +// 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 "autoware/obstacle_cruise_planner/planner_interface.hpp" + +#include "autoware/motion_utils/distance/distance.hpp" +#include "autoware/motion_utils/marker/marker_helper.hpp" +#include "autoware/motion_utils/resample/resample.hpp" +#include "autoware/motion_utils/trajectory/conversion.hpp" +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/signal_processing/lowpass_filter_1d.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" + +#include +#include + +#include +#include +#include +#include +#include +#include + +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; +} + +double calcMinimumDistanceToStop( + const double initial_vel, const double max_acc, const double min_acc) +{ + if (initial_vel < 0.0) { + return -std::pow(initial_vel, 2) / 2.0 / max_acc; + } + + return -std::pow(initial_vel, 2) / 2.0 / min_acc; +} + +template +std::optional getObjectFromUuid(const std::vector & objects, const std::string & target_uuid) +{ + const auto itr = std::find_if(objects.begin(), objects.end(), [&](const auto & object) { + return object.uuid == target_uuid; + }); + + if (itr == objects.end()) { + return std::nullopt; + } + return *itr; +} + +// TODO(murooka) following two functions are copied from behavior_velocity_planner. +// These should be refactored. +double findReachTime( + const double jerk, const double accel, const double velocity, const double distance, + const double t_min, const double t_max) +{ + const double j = jerk; + const double a = accel; + const double v = velocity; + const double d = distance; + const double min = t_min; + const double max = t_max; + auto f = [](const double t, const double j, const double a, const double v, const double d) { + return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; + }; + if (f(min, j, a, v, d) > 0 || f(max, j, a, v, d) < 0) { + throw std::logic_error("[obstacle_cruise_planner](findReachTime): search range is invalid"); + } + const double eps = 1e-5; + const int warn_iter = 100; + double lower = min; + double upper = max; + double t; + int iter = 0; + for (int i = 0;; i++) { + t = 0.5 * (lower + upper); + const double fx = f(t, j, a, v, d); + // std::cout<<"fx: "< 0.0) { + upper = t; + } else { + lower = t; + } + iter++; + if (iter > warn_iter) + std::cerr << "[obstacle_cruise_planner](findReachTime): current iter is over warning" + << std::endl; + } + return t; +} + +double calcDecelerationVelocityFromDistanceToTarget( + const double max_slowdown_jerk, const double max_slowdown_accel, const double current_accel, + const double current_velocity, const double distance_to_target) +{ + if (max_slowdown_jerk > 0 || max_slowdown_accel > 0) { + throw std::logic_error("max_slowdown_jerk and max_slowdown_accel should be negative"); + } + // case0: distance to target is behind ego + if (distance_to_target <= 0) return current_velocity; + auto ft = [](const double t, const double j, const double a, const double v, const double d) { + return j * t * t * t / 6.0 + a * t * t / 2.0 + v * t - d; + }; + auto vt = [](const double t, const double j, const double a, const double v) { + return j * t * t / 2.0 + a * t + v; + }; + const double j_max = max_slowdown_jerk; + const double a0 = current_accel; + const double a_max = max_slowdown_accel; + const double v0 = current_velocity; + const double l = distance_to_target; + const double t_const_jerk = (a_max - a0) / j_max; + const double d_const_jerk_stop = ft(t_const_jerk, j_max, a0, v0, 0.0); + const double d_const_acc_stop = l - d_const_jerk_stop; + + if (d_const_acc_stop < 0) { + // case0: distance to target is within constant jerk deceleration + // use binary search instead of solving cubic equation + const double t_jerk = findReachTime(j_max, a0, v0, l, 0, t_const_jerk); + const double velocity = vt(t_jerk, j_max, a0, v0); + return velocity; + } else { + const double v1 = vt(t_const_jerk, j_max, a0, v0); + const double discriminant_of_stop = 2.0 * a_max * d_const_acc_stop + v1 * v1; + // case3: distance to target is farther than distance to stop + if (discriminant_of_stop <= 0) { + return 0.0; + } + // case2: distance to target is within constant accel deceleration + // solve d = 0.5*a^2+v*t by t + const double t_acc = (-v1 + std::sqrt(discriminant_of_stop)) / a_max; + return vt(t_acc, 0.0, a_max, v1); + } + return current_velocity; +} + +std::vector resampleTrajectoryPoints( + const std::vector & traj_points, const double interval) +{ + const auto traj = autoware::motion_utils::convertToTrajectory(traj_points); + const auto resampled_traj = autoware::motion_utils::resampleTrajectory(traj, interval); + return autoware::motion_utils::convertToTrajectoryPointArray(resampled_traj); +} + +autoware::universe_utils::Point2d convertPoint(const geometry_msgs::msg::Point & p) +{ + return autoware::universe_utils::Point2d{p.x, p.y}; +} +} // namespace + +std::vector PlannerInterface::generateStopTrajectory( + const PlannerData & planner_data, const std::vector & stop_obstacles) +{ + stop_watch_.tic(__func__); + + stop_planning_debug_info_.reset(); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.ego_vel); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::EGO_VELOCITY, planner_data.ego_acc); + + const double abs_ego_offset = planner_data.is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + + if (stop_obstacles.empty()) { + // delete marker + const auto markers = + autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + + prev_stop_distance_info_ = std::nullopt; + return planner_data.traj_points; + } + + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner::PIDBasedPlanner"), enable_debug_info_, + "stop planning"); + + std::optional determined_stop_obstacle{}; + std::optional determined_zero_vel_dist{}; + std::optional determined_desired_margin{}; + + const auto closest_stop_obstacles = + obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles); + for (const auto & stop_obstacle : closest_stop_obstacles) { + const auto ego_segment_idx = + ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); + const double dist_to_collide_on_ref_traj = + autoware::motion_utils::calcSignedArcLength(planner_data.traj_points, 0, ego_segment_idx) + + stop_obstacle.dist_to_collide_on_decimated_traj; + + const double desired_margin = [&]() { + const double margin_from_obstacle = + calculateMarginFromObstacleOnCurve(planner_data, stop_obstacle); + // Use terminal margin (terminal_safe_distance_margin) for obstacle stop + const auto ref_traj_length = autoware::motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.traj_points.size() - 1); + if (dist_to_collide_on_ref_traj > ref_traj_length) { + return longitudinal_info_.terminal_safe_distance_margin; + } + + // 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 auto closest_behavior_stop_idx = autoware::motion_utils::searchZeroVelocityIndex( + planner_data.traj_points, ego_segment_idx + 1); + if (closest_behavior_stop_idx) { + const double closest_behavior_stop_dist_on_ref_traj = + autoware::motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, *closest_behavior_stop_idx); + const double stop_dist_diff = closest_behavior_stop_dist_on_ref_traj - + (dist_to_collide_on_ref_traj - margin_from_obstacle); + if (0.0 < stop_dist_diff && stop_dist_diff < margin_from_obstacle) { + return min_behavior_stop_margin_; + } + } + return margin_from_obstacle; + }(); + + // calc stop point against the obstacle + double candidate_zero_vel_dist = std::max(0.0, dist_to_collide_on_ref_traj - desired_margin); + if (suppress_sudden_obstacle_stop_) { + const auto acceptable_stop_acc = [&]() -> std::optional { + if (stop_param_.getParamType(stop_obstacle.classification) == "default") { + return longitudinal_info_.limit_min_accel; + } + const double distance_to_judge_suddenness = std::min( + calcMinimumDistanceToStop( + planner_data.ego_vel, longitudinal_info_.limit_max_accel, + stop_param_.getParam(stop_obstacle.classification).sudden_object_acc_threshold), + stop_param_.getParam(stop_obstacle.classification).sudden_object_dist_threshold); + if (candidate_zero_vel_dist > distance_to_judge_suddenness) { + return longitudinal_info_.limit_min_accel; + } + if (stop_param_.getParam(stop_obstacle.classification).abandon_to_stop) { + RCLCPP_WARN( + rclcpp::get_logger("ObstacleCruisePlanner::StopPlanner"), + "[Cruise] abandon to stop against %s object", + stop_param_.types_maps.at(stop_obstacle.classification.label).c_str()); + return std::nullopt; + } else { + return stop_param_.getParam(stop_obstacle.classification).limit_min_acc; + } + }(); + if (!acceptable_stop_acc) { + continue; + } + + const double acceptable_stop_pos = + autoware::motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.ego_pose.position) + + calcMinimumDistanceToStop( + planner_data.ego_vel, longitudinal_info_.limit_max_accel, acceptable_stop_acc.value()); + if (acceptable_stop_pos > candidate_zero_vel_dist) { + candidate_zero_vel_dist = acceptable_stop_pos; + } + } + + if (determined_stop_obstacle) { + const bool is_same_param_types = + (stop_obstacle.classification.label == determined_stop_obstacle->classification.label); + if ( + (is_same_param_types && stop_obstacle.dist_to_collide_on_decimated_traj > + determined_stop_obstacle->dist_to_collide_on_decimated_traj) || + (!is_same_param_types && candidate_zero_vel_dist > determined_zero_vel_dist)) { + continue; + } + } + determined_zero_vel_dist = candidate_zero_vel_dist; + determined_stop_obstacle = stop_obstacle; + determined_desired_margin = desired_margin; + } + + if (!determined_zero_vel_dist) { + // delete marker + const auto markers = + autoware::motion_utils::createDeletedStopVirtualWallMarker(planner_data.current_time, 0); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + + prev_stop_distance_info_ = std::nullopt; + return planner_data.traj_points; + } + + // Hold previous stop distance if necessary + if ( + std::abs(planner_data.ego_vel) < longitudinal_info_.hold_stop_velocity_threshold && + prev_stop_distance_info_) { + // NOTE: We assume that the current trajectory's front point is ahead of the previous + // trajectory's front point. + const size_t traj_front_point_prev_seg_idx = + autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prev_stop_distance_info_->first, planner_data.traj_points.front().pose); + const double diff_dist_front_points = autoware::motion_utils::calcSignedArcLength( + prev_stop_distance_info_->first, 0, planner_data.traj_points.front().pose.position, + traj_front_point_prev_seg_idx); + + const double prev_zero_vel_dist = prev_stop_distance_info_->second - diff_dist_front_points; + if ( + std::abs(prev_zero_vel_dist - determined_zero_vel_dist.value()) < + longitudinal_info_.hold_stop_distance_threshold) { + determined_zero_vel_dist.value() = prev_zero_vel_dist; + } + } + + // Insert stop point + auto output_traj_points = planner_data.traj_points; + const auto zero_vel_idx = + autoware::motion_utils::insertStopPoint(0, *determined_zero_vel_dist, output_traj_points); + if (zero_vel_idx) { + // virtual wall marker for stop obstacle + const auto markers = autoware::motion_utils::createStopVirtualWallMarker( + output_traj_points.at(*zero_vel_idx).pose, "obstacle stop", planner_data.current_time, 0, + abs_ego_offset, "", planner_data.is_driving_forward); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->stop_wall_marker); + debug_data_ptr_->obstacles_to_stop.push_back(*determined_stop_obstacle); + + // Publish Stop Reason + const auto stop_pose = output_traj_points.at(*zero_vel_idx).pose; + planning_factor_interface_->add( + output_traj_points, planner_data.ego_pose, stop_pose, + tier4_planning_msgs::msg::PlanningFactor::STOP, + tier4_planning_msgs::msg::SafetyFactorArray{}); + // Store stop reason debug data + debug_data_ptr_->stop_metrics = + makeMetrics("PlannerInterface", "stop", planner_data, stop_pose, *determined_stop_obstacle); + // Publish if ego vehicle will over run against the stop point with a limit acceleration + + const bool will_over_run = determined_zero_vel_dist.value() > + autoware::motion_utils::calcSignedArcLength( + planner_data.traj_points, 0, planner_data.ego_pose.position) + + determined_stop_obstacle->dist_to_collide_on_decimated_traj + + determined_desired_margin.value() + 0.1; + const auto stop_speed_exceeded_msg = + createStopSpeedExceededMsg(planner_data.current_time, will_over_run); + stop_speed_exceeded_pub_->publish(stop_speed_exceeded_msg); + + prev_stop_distance_info_ = std::make_pair(output_traj_points, determined_zero_vel_dist.value()); + } + + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_DISTANCE, + determined_stop_obstacle->dist_to_collide_on_decimated_traj); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_CURRENT_OBSTACLE_VELOCITY, + determined_stop_obstacle->velocity); + stop_planning_debug_info_.set( + StopPlanningDebugInfo::TYPE::STOP_TARGET_OBSTACLE_DISTANCE, determined_desired_margin.value()); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_VELOCITY, 0.0); + stop_planning_debug_info_.set(StopPlanningDebugInfo::TYPE::STOP_TARGET_ACCELERATION, 0.0); + + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner::SlowDownPlanner"), enable_calculation_time_info_, + " %s := %f [ms]", __func__, calculation_time); + + return output_traj_points; +} + +double PlannerInterface::calculateMarginFromObstacleOnCurve( + const PlannerData & planner_data, const StopObstacle & stop_obstacle) const +{ + if (!enable_approaching_on_curve_ || use_pointcloud_) { + return longitudinal_info_.safe_distance_margin; + } + + const double abs_ego_offset = planner_data.is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + + // calculate short trajectory points towards obstacle + const size_t obj_segment_idx = autoware::motion_utils::findNearestSegmentIndex( + planner_data.traj_points, stop_obstacle.collision_point); + std::vector short_traj_points{planner_data.traj_points.at(obj_segment_idx + 1)}; + double sum_short_traj_length{0.0}; + for (int i = obj_segment_idx; 0 <= i; --i) { + short_traj_points.push_back(planner_data.traj_points.at(i)); + + if ( + 1 < short_traj_points.size() && + longitudinal_info_.safe_distance_margin + abs_ego_offset < sum_short_traj_length) { + break; + } + sum_short_traj_length += autoware::universe_utils::calcDistance2d( + planner_data.traj_points.at(i), planner_data.traj_points.at(i + 1)); + } + std::reverse(short_traj_points.begin(), short_traj_points.end()); + if (short_traj_points.size() < 2) { + return longitudinal_info_.safe_distance_margin; + } + + // calculate collision index between straight line from ego pose and object + const auto calculate_distance_from_straight_ego_path = + [&](const auto & ego_pose, const auto & object_polygon) { + const auto forward_ego_pose = autoware::universe_utils::calcOffsetPose( + ego_pose, longitudinal_info_.safe_distance_margin + 3.0, 0.0, 0.0); + const auto ego_straight_segment = autoware::universe_utils::Segment2d{ + convertPoint(ego_pose.position), convertPoint(forward_ego_pose.position)}; + return boost::geometry::distance(ego_straight_segment, object_polygon); + }; + const auto resampled_short_traj_points = resampleTrajectoryPoints(short_traj_points, 0.5); + const auto object_polygon = + autoware::universe_utils::toPolygon2d(stop_obstacle.pose, stop_obstacle.shape); + const auto collision_idx = [&]() -> std::optional { + for (size_t i = 0; i < resampled_short_traj_points.size(); ++i) { + const double dist_to_obj = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(i).pose, object_polygon); + if (dist_to_obj < vehicle_info_.vehicle_width_m / 2.0) { + return i; + } + } + return std::nullopt; + }(); + if (!collision_idx) { + return min_safe_distance_margin_on_curve_; + } + if (*collision_idx == 0) { + return longitudinal_info_.safe_distance_margin; + } + + // calculate margin from obstacle + const double partial_segment_length = [&]() { + const double collision_segment_length = autoware::universe_utils::calcDistance2d( + resampled_short_traj_points.at(*collision_idx - 1), + resampled_short_traj_points.at(*collision_idx)); + const double prev_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx - 1).pose, object_polygon); + const double next_dist = calculate_distance_from_straight_ego_path( + resampled_short_traj_points.at(*collision_idx).pose, object_polygon); + return (next_dist - vehicle_info_.vehicle_width_m / 2.0) / (next_dist - prev_dist) * + collision_segment_length; + }(); + + const double short_margin_from_obstacle = + partial_segment_length + + autoware::motion_utils::calcSignedArcLength( + resampled_short_traj_points, *collision_idx, stop_obstacle.collision_point) - + abs_ego_offset + additional_safe_distance_margin_on_curve_; + + return std::min( + longitudinal_info_.safe_distance_margin, + std::max(min_safe_distance_margin_on_curve_, short_margin_from_obstacle)); +} + +double PlannerInterface::calcDistanceToCollisionPoint( + const PlannerData & planner_data, const geometry_msgs::msg::Point & collision_point) +{ + const double offset = planner_data.is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + + const size_t ego_segment_idx = + ego_nearest_param_.findSegmentIndex(planner_data.traj_points, planner_data.ego_pose); + + const size_t collision_segment_idx = + autoware::motion_utils::findNearestSegmentIndex(planner_data.traj_points, collision_point); + + const auto dist_to_collision_point = autoware::motion_utils::calcSignedArcLength( + planner_data.traj_points, planner_data.ego_pose.position, ego_segment_idx, collision_point, + collision_segment_idx); + + return dist_to_collision_point - offset; +} + +std::vector PlannerInterface::generateSlowDownTrajectory( + const PlannerData & planner_data, const std::vector & cruise_traj_points, + const std::vector & obstacles, + [[maybe_unused]] std::optional & vel_limit) +{ + stop_watch_.tic(__func__); + auto slow_down_traj_points = cruise_traj_points; + slow_down_debug_multi_array_ = Float32MultiArrayStamped(); + + const double dist_to_ego = [&]() { + const size_t ego_seg_idx = + ego_nearest_param_.findSegmentIndex(slow_down_traj_points, planner_data.ego_pose); + return autoware::motion_utils::calcSignedArcLength( + slow_down_traj_points, 0, planner_data.ego_pose.position, ego_seg_idx); + }(); + const double abs_ego_offset = planner_data.is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + + // define function to insert slow down velocity to trajectory + const auto insert_point_in_trajectory = [&](const double lon_dist) -> std::optional { + const auto inserted_idx = + autoware::motion_utils::insertTargetPoint(0, lon_dist, slow_down_traj_points); + if (inserted_idx) { + if (inserted_idx.value() + 1 <= slow_down_traj_points.size() - 1) { + // zero-order hold for velocity interpolation + slow_down_traj_points.at(inserted_idx.value()).longitudinal_velocity_mps = + slow_down_traj_points.at(inserted_idx.value() + 1).longitudinal_velocity_mps; + } + return inserted_idx.value(); + } + return std::nullopt; + }; + + std::vector new_prev_slow_down_output; + for (size_t i = 0; i < obstacles.size(); ++i) { + const auto & obstacle = obstacles.at(i); + const auto prev_output = getObjectFromUuid(prev_slow_down_output_, obstacle.uuid); + + const bool is_obstacle_moving = [&]() -> bool { + const auto object_vel_norm = std::hypot(obstacle.velocity, obstacle.lat_velocity); + if (!prev_output) return object_vel_norm > moving_object_speed_threshold; + if (prev_output->is_moving) + return object_vel_norm > moving_object_speed_threshold - moving_object_hysteresis_range; + return object_vel_norm > moving_object_speed_threshold + moving_object_hysteresis_range; + }(); + + // calculate slow down start distance, and insert slow down velocity + const auto dist_vec_to_slow_down = calculateDistanceToSlowDownWithConstraints( + planner_data, slow_down_traj_points, obstacle, prev_output, dist_to_ego, is_obstacle_moving); + if (!dist_vec_to_slow_down) { + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner::PlannerInterface"), enable_debug_info_, + "[SlowDown] Ignore obstacle (%s) since distance to slow down is not valid", + obstacle.uuid.c_str()); + continue; + } + const auto dist_to_slow_down_start = std::get<0>(*dist_vec_to_slow_down); + const auto dist_to_slow_down_end = std::get<1>(*dist_vec_to_slow_down); + const auto feasible_slow_down_vel = std::get<2>(*dist_vec_to_slow_down); + + // calculate slow down end distance, and insert slow down velocity + // NOTE: slow_down_start_idx will not be wrong since inserted back point is after inserted + // front point. + const auto slow_down_start_idx = insert_point_in_trajectory(dist_to_slow_down_start); + const auto slow_down_end_idx = dist_to_slow_down_start < dist_to_slow_down_end + ? insert_point_in_trajectory(dist_to_slow_down_end) + : std::nullopt; + if (!slow_down_end_idx) { + continue; + } + + // calculate slow down velocity + const double stable_slow_down_vel = [&]() { + if (prev_output) { + return autoware::signal_processing::lowpassFilter( + feasible_slow_down_vel, prev_output->target_vel, slow_down_param_.lpf_gain_slow_down_vel); + } + return feasible_slow_down_vel; + }(); + + // insert slow down velocity between slow start and end + for (size_t j = (slow_down_start_idx ? *slow_down_start_idx : 0); j <= *slow_down_end_idx; + ++j) { + auto & traj_point = slow_down_traj_points.at(j); + traj_point.longitudinal_velocity_mps = + std::min(traj_point.longitudinal_velocity_mps, static_cast(stable_slow_down_vel)); + } + + // add debug data + slow_down_debug_multi_array_.data.push_back(obstacle.precise_lat_dist); + slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_start); + slow_down_debug_multi_array_.data.push_back(dist_to_slow_down_end); + slow_down_debug_multi_array_.data.push_back(feasible_slow_down_vel); + slow_down_debug_multi_array_.data.push_back(stable_slow_down_vel); + slow_down_debug_multi_array_.data.push_back(slow_down_start_idx ? *slow_down_start_idx : -1.0); + slow_down_debug_multi_array_.data.push_back(*slow_down_end_idx); + + // add virtual wall + if (slow_down_start_idx && slow_down_end_idx) { + const size_t ego_idx = + ego_nearest_param_.findIndex(slow_down_traj_points, planner_data.ego_pose); + const size_t slow_down_wall_idx = [&]() { + if (ego_idx < *slow_down_start_idx) return *slow_down_start_idx; + if (ego_idx < *slow_down_end_idx) return ego_idx; + return *slow_down_end_idx; + }(); + + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(slow_down_wall_idx).pose, "obstacle slow down", + planner_data.current_time, i, abs_ego_offset, "", planner_data.is_driving_forward); + autoware::universe_utils::appendMarkerArray(markers, &debug_data_ptr_->slow_down_wall_marker); + planning_factor_interface_->add( + slow_down_traj_points, planner_data.ego_pose, + slow_down_traj_points.at(*slow_down_start_idx).pose, + slow_down_traj_points.at(*slow_down_end_idx).pose, + tier4_planning_msgs::msg::PlanningFactor::SLOW_DOWN, + tier4_planning_msgs::msg::SafetyFactorArray{}, planner_data.is_driving_forward, + stable_slow_down_vel); + } + + // add debug virtual wall + if (slow_down_start_idx) { + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(*slow_down_start_idx).pose, "obstacle slow down start", + planner_data.current_time, i * 2, abs_ego_offset, "", planner_data.is_driving_forward); + autoware::universe_utils::appendMarkerArray( + markers, &debug_data_ptr_->slow_down_debug_wall_marker); + } + if (slow_down_end_idx) { + const auto markers = autoware::motion_utils::createSlowDownVirtualWallMarker( + slow_down_traj_points.at(*slow_down_end_idx).pose, "obstacle slow down end", + planner_data.current_time, i * 2 + 1, abs_ego_offset, "", planner_data.is_driving_forward); + autoware::universe_utils::appendMarkerArray( + markers, &debug_data_ptr_->slow_down_debug_wall_marker); + } + + // Add debug data + debug_data_ptr_->obstacles_to_slow_down.push_back(obstacle); + if (!debug_data_ptr_->stop_metrics.has_value()) { + debug_data_ptr_->slow_down_metrics = + makeMetrics("PlannerInterface", "slow_down", planner_data); + } + + // update prev_slow_down_output_ + new_prev_slow_down_output.push_back(SlowDownOutput{ + obstacle.uuid, slow_down_traj_points, slow_down_start_idx, slow_down_end_idx, + stable_slow_down_vel, feasible_slow_down_vel, obstacle.precise_lat_dist, is_obstacle_moving}); + } + + // update prev_slow_down_output_ + prev_slow_down_output_ = new_prev_slow_down_output; + + const double calculation_time = stop_watch_.toc(__func__); + RCLCPP_INFO_EXPRESSION( + rclcpp::get_logger("ObstacleCruisePlanner::SlowDownPlanner"), enable_calculation_time_info_, + " %s := %f [ms]", __func__, calculation_time); + + return slow_down_traj_points; +} + +double PlannerInterface::calculateSlowDownVelocity( + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const bool is_obstacle_moving) const +{ + const auto & p = + slow_down_param_.getObstacleParamByLabel(obstacle.classification, is_obstacle_moving); + const double stable_precise_lat_dist = [&]() { + if (prev_output) { + return autoware::signal_processing::lowpassFilter( + obstacle.precise_lat_dist, prev_output->precise_lat_dist, + slow_down_param_.lpf_gain_lat_dist); + } + return obstacle.precise_lat_dist; + }(); + + const double ratio = std::clamp( + (std::abs(stable_precise_lat_dist) - p.min_lat_margin) / (p.max_lat_margin - p.min_lat_margin), + 0.0, 1.0); + const double slow_down_vel = + p.min_ego_velocity + ratio * (p.max_ego_velocity - p.min_ego_velocity); + + return slow_down_vel; +} + +std::optional> +PlannerInterface::calculateDistanceToSlowDownWithConstraints( + const PlannerData & planner_data, const std::vector & traj_points, + const SlowDownObstacle & obstacle, const std::optional & prev_output, + const double dist_to_ego, const bool is_obstacle_moving) const +{ + const double abs_ego_offset = planner_data.is_driving_forward + ? std::abs(vehicle_info_.max_longitudinal_offset_m) + : std::abs(vehicle_info_.min_longitudinal_offset_m); + const double obstacle_vel = obstacle.velocity; + // calculate slow down velocity + const double slow_down_vel = calculateSlowDownVelocity(obstacle, prev_output, is_obstacle_moving); + + // calculate distance to collision points + const double dist_to_front_collision = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, obstacle.front_collision_point); + const double dist_to_back_collision = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, obstacle.back_collision_point); + + // calculate offset distance to first collision considering relative velocity + const double relative_vel = planner_data.ego_vel - obstacle.velocity; + const double offset_dist_to_collision = [&]() { + if (dist_to_front_collision < dist_to_ego + abs_ego_offset) { + return 0.0; + } + + // NOTE: This min_relative_vel forces the relative velocity positive if the ego velocity is + // lower than the obstacle velocity. Without this, the slow down feature will flicker where + // the ego velocity is very close to the obstacle velocity. + constexpr double min_relative_vel = 1.0; + const double time_to_collision = (dist_to_front_collision - dist_to_ego - abs_ego_offset) / + std::max(min_relative_vel, relative_vel); + + constexpr double time_to_collision_margin = 1.0; + const double cropped_time_to_collision = + std::max(0.0, time_to_collision - time_to_collision_margin); + return obstacle_vel * cropped_time_to_collision; + }(); + + // calculate distance during deceleration, slow down preparation, and slow down + const double min_slow_down_prepare_dist = 3.0; + const double slow_down_prepare_dist = std::max( + min_slow_down_prepare_dist, slow_down_vel * slow_down_param_.time_margin_on_target_velocity); + const double deceleration_dist = offset_dist_to_collision + dist_to_front_collision - + abs_ego_offset - dist_to_ego - slow_down_prepare_dist; + const double slow_down_dist = + dist_to_back_collision - dist_to_front_collision + slow_down_prepare_dist; + + // calculate distance to start/end slow down + const double dist_to_slow_down_start = dist_to_ego + deceleration_dist; + const double dist_to_slow_down_end = dist_to_ego + deceleration_dist + slow_down_dist; + if (100.0 < dist_to_slow_down_start) { + // NOTE: distance to slow down is too far. + return std::nullopt; + } + + // apply low-pass filter to distance to start/end slow down + const auto apply_lowpass_filter = [&](const double dist_to_slow_down, const auto prev_point) { + if (prev_output && prev_point) { + const size_t seg_idx = + autoware::motion_utils::findNearestSegmentIndex(traj_points, prev_point->position); + const double prev_dist_to_slow_down = + autoware::motion_utils::calcSignedArcLength(traj_points, 0, prev_point->position, seg_idx); + return autoware::signal_processing::lowpassFilter( + dist_to_slow_down, prev_dist_to_slow_down, slow_down_param_.lpf_gain_dist_to_slow_down); + } + return dist_to_slow_down; + }; + const double filtered_dist_to_slow_down_start = + apply_lowpass_filter(dist_to_slow_down_start, prev_output->start_point); + const double filtered_dist_to_slow_down_end = + apply_lowpass_filter(dist_to_slow_down_end, prev_output->end_point); + + // calculate velocity considering constraints + const double feasible_slow_down_vel = [&]() { + if (deceleration_dist < 0) { + if (prev_output) { + return prev_output->target_vel; + } + return std::max(planner_data.ego_vel, slow_down_vel); + } + if (planner_data.ego_vel < slow_down_vel) { + return slow_down_vel; + } + + const double one_shot_feasible_slow_down_vel = [&]() { + if (planner_data.ego_acc < longitudinal_info_.min_accel) { + const double squared_vel = + std::pow(planner_data.ego_vel, 2) + 2 * deceleration_dist * longitudinal_info_.min_accel; + if (squared_vel < 0) { + return slow_down_vel; + } + return std::max(std::sqrt(squared_vel), slow_down_vel); + } + // TODO(murooka) Calculate more precisely. Final acceleration should be zero. + const double min_feasible_slow_down_vel = calcDecelerationVelocityFromDistanceToTarget( + longitudinal_info_.slow_down_min_jerk, longitudinal_info_.slow_down_min_accel, + planner_data.ego_acc, planner_data.ego_vel, deceleration_dist); + return min_feasible_slow_down_vel; + }(); + if (prev_output) { + // NOTE: If longitudinal controllability is not good, one_shot_slow_down_vel may be getting + // larger since we use actual ego's velocity and acceleration for its calculation. + // Suppress one_shot_slow_down_vel getting larger here. + const double feasible_slow_down_vel = + std::min(one_shot_feasible_slow_down_vel, prev_output->feasible_target_vel); + return std::max(slow_down_vel, feasible_slow_down_vel); + } + return std::max(slow_down_vel, one_shot_feasible_slow_down_vel); + }(); + + return std::make_tuple( + filtered_dist_to_slow_down_start, filtered_dist_to_slow_down_end, feasible_slow_down_vel); +} + +std::vector PlannerInterface::makeMetrics( + const std::string & module_name, const std::string & reason, + const std::optional & planner_data, + const std::optional & stop_pose, + const std::optional & stop_obstacle) +{ + auto metrics = std::vector(); + + // Create status + { + // Decision + Metric decision_metric; + decision_metric.name = module_name + "/decision"; + decision_metric.unit = "string"; + decision_metric.value = reason; + metrics.push_back(decision_metric); + } + + if (stop_pose.has_value() && planner_data.has_value()) { // Stop info + Metric stop_position_metric; + stop_position_metric.name = module_name + "/stop_position"; + stop_position_metric.unit = "string"; + const auto & p = stop_pose.value().position; + stop_position_metric.value = + "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; + metrics.push_back(stop_position_metric); + + Metric stop_orientation_metric; + stop_orientation_metric.name = module_name + "/stop_orientation"; + stop_orientation_metric.unit = "string"; + const auto & o = stop_pose.value().orientation; + stop_orientation_metric.value = "{" + std::to_string(o.w) + ", " + std::to_string(o.x) + ", " + + std::to_string(o.y) + ", " + std::to_string(o.z) + "}"; + metrics.push_back(stop_orientation_metric); + + const auto dist_to_stop_pose = autoware::motion_utils::calcSignedArcLength( + planner_data.value().traj_points, planner_data.value().ego_pose.position, + stop_pose.value().position); + + Metric dist_to_stop_pose_metric; + dist_to_stop_pose_metric.name = module_name + "/distance_to_stop_pose"; + dist_to_stop_pose_metric.unit = "double"; + dist_to_stop_pose_metric.value = std::to_string(dist_to_stop_pose); + metrics.push_back(dist_to_stop_pose_metric); + } + + if (stop_obstacle.has_value()) { + // Obstacle info + Metric collision_point_metric; + const auto & p = stop_obstacle.value().collision_point; + collision_point_metric.name = module_name + "/collision_point"; + collision_point_metric.unit = "string"; + collision_point_metric.value = + "{" + std::to_string(p.x) + ", " + std::to_string(p.y) + ", " + std::to_string(p.z) + "}"; + metrics.push_back(collision_point_metric); + } + return metrics; +} + +void PlannerInterface::publishMetrics(const rclcpp::Time & current_time) +{ + // create array + MetricArray metrics_msg; + metrics_msg.stamp = current_time; + + auto addMetrics = [&metrics_msg](std::optional> & opt_metrics) { + if (opt_metrics) { + metrics_msg.metric_array.insert( + metrics_msg.metric_array.end(), opt_metrics->begin(), opt_metrics->end()); + } + }; + addMetrics(debug_data_ptr_->stop_metrics); + addMetrics(debug_data_ptr_->slow_down_metrics); + addMetrics(debug_data_ptr_->cruise_metrics); + metrics_pub_->publish(metrics_msg); + clearMetrics(); +} + +void PlannerInterface::clearMetrics() +{ + debug_data_ptr_->stop_metrics = std::nullopt; + debug_data_ptr_->slow_down_metrics = std::nullopt; + debug_data_ptr_->cruise_metrics = std::nullopt; +} diff --git a/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp new file mode 100644 index 0000000000000..4d6e6568067f4 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/src/polygon_utils.cpp @@ -0,0 +1,201 @@ +// 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 "autoware/obstacle_cruise_planner/polygon_utils.hpp" + +#include "autoware/motion_utils/trajectory/trajectory.hpp" +#include "autoware/universe_utils/geometry/boost_polygon_utils.hpp" +#include "autoware/universe_utils/geometry/geometry.hpp" + +#include +#include +#include +#include + +namespace +{ +PointWithStamp calcNearestCollisionPoint( + const size_t first_within_idx, const std::vector & collision_points, + const std::vector & decimated_traj_points, const bool is_driving_forward) +{ + const size_t prev_idx = first_within_idx == 0 ? first_within_idx : first_within_idx - 1; + const size_t next_idx = first_within_idx == 0 ? first_within_idx + 1 : first_within_idx; + + std::vector segment_points{ + decimated_traj_points.at(prev_idx).pose, decimated_traj_points.at(next_idx).pose}; + if (!is_driving_forward) { + std::reverse(segment_points.begin(), segment_points.end()); + } + + std::vector dist_vec; + for (const auto & collision_point : collision_points) { + const double dist = autoware::motion_utils::calcLongitudinalOffsetToSegment( + segment_points, 0, collision_point.point); + dist_vec.push_back(dist); + } + + const size_t min_idx = std::min_element(dist_vec.begin(), dist_vec.end()) - dist_vec.begin(); + return collision_points.at(min_idx); +} + +// NOTE: max_dist is used for efficient calculation to suppress boost::geometry's polygon +// calculation. +std::optional>> getCollisionIndex( + const std::vector & traj_points, const std::vector & traj_polygons, + const geometry_msgs::msg::Pose & object_pose, const rclcpp::Time & object_time, + const Shape & object_shape, const double max_dist = std::numeric_limits::max()) +{ + const auto obj_polygon = autoware::universe_utils::toPolygon2d(object_pose, object_shape); + for (size_t i = 0; i < traj_polygons.size(); ++i) { + const double approximated_dist = + autoware::universe_utils::calcDistance2d(traj_points.at(i).pose, object_pose); + if (approximated_dist > max_dist) { + continue; + } + + std::vector collision_polygons; + boost::geometry::intersection(traj_polygons.at(i), obj_polygon, collision_polygons); + + std::vector collision_geom_points; + bool has_collision = false; + for (const auto & collision_polygon : collision_polygons) { + if (boost::geometry::area(collision_polygon) > 0.0) { + has_collision = true; + + for (const auto & collision_point : collision_polygon.outer()) { + PointWithStamp collision_geom_point; + collision_geom_point.stamp = object_time; + collision_geom_point.point.x = collision_point.x(); + collision_geom_point.point.y = collision_point.y(); + collision_geom_point.point.z = traj_points.at(i).pose.position.z; + collision_geom_points.push_back(collision_geom_point); + } + } + } + + if (has_collision) { + const auto collision_info = + std::pair>{i, collision_geom_points}; + return collision_info; + } + } + + return std::nullopt; +} +} // namespace + +namespace polygon_utils +{ +std::optional> getCollisionPoint( + const std::vector & traj_points, const std::vector & traj_polygons, + const Obstacle & obstacle, const bool is_driving_forward, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +{ + const auto collision_info = + getCollisionIndex(traj_points, traj_polygons, obstacle.pose, obstacle.stamp, obstacle.shape); + if (!collision_info) { + return std::nullopt; + } + + const double x_diff_to_bumper = is_driving_forward ? vehicle_info.max_longitudinal_offset_m + : vehicle_info.min_longitudinal_offset_m; + const auto bumper_pose = autoware::universe_utils::calcOffsetPose( + traj_points.at(collision_info->first).pose, x_diff_to_bumper, 0.0, 0.0); + + std::optional max_collision_length = std::nullopt; + std::optional max_collision_point = std::nullopt; + for (const auto & poly_vertex : collision_info->second) { + const double dist_from_bumper = + std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + + if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { + max_collision_length = dist_from_bumper; + max_collision_point = poly_vertex.point; + } + } + return std::make_pair( + *max_collision_point, + autoware::motion_utils::calcSignedArcLength(traj_points, 0, collision_info->first) - + *max_collision_length); +} + +std::optional> getCollisionPoint( + const std::vector & traj_points, const size_t collision_idx, + const std::vector & collision_points, const bool is_driving_forward, + const autoware::vehicle_info_utils::VehicleInfo & vehicle_info) +{ + std::pair> collision_info; + collision_info.first = collision_idx; + collision_info.second = collision_points; + + const double x_diff_to_bumper = is_driving_forward ? vehicle_info.max_longitudinal_offset_m + : vehicle_info.min_longitudinal_offset_m; + const auto bumper_pose = autoware::universe_utils::calcOffsetPose( + traj_points.at(collision_info.first).pose, x_diff_to_bumper, 0.0, 0.0); + + std::optional max_collision_length = std::nullopt; + std::optional max_collision_point = std::nullopt; + for (const auto & poly_vertex : collision_info.second) { + const double dist_from_bumper = + std::abs(autoware::universe_utils::inverseTransformPoint(poly_vertex.point, bumper_pose).x); + + if (!max_collision_length.has_value() || dist_from_bumper > *max_collision_length) { + max_collision_length = dist_from_bumper; + max_collision_point = poly_vertex.point; + } + } + return std::make_pair( + *max_collision_point, + autoware::motion_utils::calcSignedArcLength(traj_points, 0, collision_info.first) - + *max_collision_length); +} + +// NOTE: max_lat_dist is used for efficient calculation to suppress boost::geometry's polygon +// calculation. +std::vector getCollisionPoints( + const std::vector & traj_points, const std::vector & traj_polygons, + const rclcpp::Time & obstacle_stamp, const PredictedPath & predicted_path, const Shape & shape, + const rclcpp::Time & current_time, const bool is_driving_forward, + std::vector & collision_index, const double max_lat_dist, + const double max_prediction_time_for_collision_check) +{ + std::vector collision_points; + for (size_t i = 0; i < predicted_path.path.size(); ++i) { + if ( + max_prediction_time_for_collision_check < + rclcpp::Duration(predicted_path.time_step).seconds() * static_cast(i)) { + break; + } + + const auto object_time = + rclcpp::Time(obstacle_stamp) + rclcpp::Duration(predicted_path.time_step) * i; + // Ignore past position + if ((object_time - current_time).seconds() < 0.0) { + continue; + } + + const auto collision_info = getCollisionIndex( + traj_points, traj_polygons, predicted_path.path.at(i), object_time, shape, max_lat_dist); + if (collision_info) { + const auto nearest_collision_point = calcNearestCollisionPoint( + collision_info->first, collision_info->second, traj_points, is_driving_forward); + collision_points.push_back(nearest_collision_point); + collision_index.push_back(collision_info->first); + } + } + + return collision_points; +} + +} // namespace polygon_utils diff --git a/planning/autoware_obstacle_cruise_planner/src/utils.cpp b/planning/autoware_obstacle_cruise_planner/src/utils.cpp new file mode 100644 index 0000000000000..b27def0bfcbe5 --- /dev/null +++ b/planning/autoware_obstacle_cruise_planner/src/utils.cpp @@ -0,0 +1,121 @@ +// 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 "autoware/obstacle_cruise_planner/utils.hpp" + +#include "autoware/object_recognition_utils/predicted_path_utils.hpp" +#include "autoware/universe_utils/ros/marker_helper.hpp" + +#include +#include +#include + +namespace obstacle_cruise_utils +{ +namespace +{ +std::optional getCurrentObjectPoseFromPredictedPath( + const PredictedPath & predicted_path, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time) +{ + const double rel_time = (current_time - obj_base_time).seconds(); + if (rel_time < 0.0) { + return std::nullopt; + } + + const auto pose = + autoware::object_recognition_utils::calcInterpolatedPose(predicted_path, rel_time); + if (!pose) { + return std::nullopt; + } + return pose.get(); +} + +std::optional getCurrentObjectPoseFromPredictedPaths( + const std::vector & predicted_paths, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time) +{ + if (predicted_paths.empty()) { + return std::nullopt; + } + // Get the most reliable path + const auto predicted_path = std::max_element( + predicted_paths.begin(), predicted_paths.end(), + [](const PredictedPath & a, const PredictedPath & b) { return a.confidence < b.confidence; }); + + return getCurrentObjectPoseFromPredictedPath(*predicted_path, obj_base_time, current_time); +} +} // namespace + +visualization_msgs::msg::Marker getObjectMarker( + const geometry_msgs::msg::Pose & obj_pose, size_t idx, const std::string & ns, const double r, + const double g, const double b) +{ + const auto current_time = rclcpp::Clock().now(); + + auto marker = autoware::universe_utils::createDefaultMarker( + "map", current_time, ns, idx, visualization_msgs::msg::Marker::SPHERE, + autoware::universe_utils::createMarkerScale(2.0, 2.0, 2.0), + autoware::universe_utils::createMarkerColor(r, g, b, 0.8)); + + marker.pose = obj_pose; + + return marker; +} + +PoseWithStamp getCurrentObjectPose( + const PredictedObject & predicted_object, const rclcpp::Time & obj_base_time, + const rclcpp::Time & current_time, const bool use_prediction) +{ + const auto & pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + + if (!use_prediction) { + return PoseWithStamp{obj_base_time, pose}; + } + + std::vector predicted_paths; + for (const auto & path : predicted_object.kinematics.predicted_paths) { + predicted_paths.push_back(path); + } + const auto interpolated_pose = + getCurrentObjectPoseFromPredictedPaths(predicted_paths, obj_base_time, current_time); + + if (!interpolated_pose) { + RCLCPP_DEBUG( + rclcpp::get_logger("ObstacleCruisePlanner"), "Failed to find the interpolated obstacle pose"); + return PoseWithStamp{obj_base_time, pose}; + } + + return PoseWithStamp{obj_base_time, *interpolated_pose}; +} + +std::vector getClosestStopObstacles(const std::vector & stop_obstacles) +{ + std::vector candidates{}; + for (const auto & stop_obstacle : stop_obstacles) { + const auto itr = + std::find_if(candidates.begin(), candidates.end(), [&stop_obstacle](const StopObstacle & co) { + return co.classification.label == stop_obstacle.classification.label; + }); + if (itr == candidates.end()) { + candidates.emplace_back(stop_obstacle); + } else if ( + stop_obstacle.dist_to_collide_on_decimated_traj < itr->dist_to_collide_on_decimated_traj) { + *itr = stop_obstacle; + } + } + return candidates; +} + +} // namespace obstacle_cruise_utils diff --git a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_utils.cpp b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_utils.cpp index fa7cde5d9d424..b32508cc2c50e 100644 --- a/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_utils.cpp +++ b/planning/autoware_obstacle_cruise_planner/test/test_obstacle_cruise_planner_utils.cpp @@ -24,23 +24,22 @@ #include #include -// StopObstacle generate_stop_obstacle(uint8_t label, double dist) -// { -// const std::string uuid{}; -// const rclcpp::Time time{}; -// ObjectClassification object_classification{}; -// object_classification.label = label; -// const geometry_msgs::msg::Pose pose{}; -// const Shape shape{}; -// const double lon_velocity{}; -// const double lat_velocity{}; -// const geometry_msgs::msg::Point collision_point{}; -// -// return StopObstacle{uuid, time, object_classification, pose, shape, -// lon_velocity, lat_velocity, collision_point, dist}; -// } +StopObstacle generate_stop_obstacle(uint8_t label, double dist) +{ + const std::string uuid{}; + const rclcpp::Time time{}; + ObjectClassification object_classification{}; + object_classification.label = label; + const geometry_msgs::msg::Pose pose{}; + const Shape shape{}; + const double lon_velocity{}; + const double lat_velocity{}; + const geometry_msgs::msg::Point collision_point{}; + + return StopObstacle{uuid, time, object_classification, pose, shape, + lon_velocity, lat_velocity, collision_point, dist}; +} -/* TEST(ObstacleCruisePlannerUtilsTest, getClosestStopObstacles) { using autoware_perception_msgs::msg::ObjectClassification; @@ -75,4 +74,3 @@ TEST(ObstacleCruisePlannerUtilsTest, getClosestStopObstacles) stop_obstacles.emplace_back(generate_stop_obstacle(ObjectClassification::BUS, 10.0)); EXPECT_EQ(3, obstacle_cruise_utils::getClosestStopObstacles(stop_obstacles).size()); } -*/