diff --git a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml index 9fe692e16d962..7a49d3e5f82d3 100644 --- a/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml +++ b/control/vehicle_cmd_gate/config/vehicle_cmd_gate.param.yaml @@ -10,6 +10,7 @@ emergency_acceleration: -2.4 moderate_stop_service_acceleration: -1.5 stopped_state_entry_duration_time: 0.1 + stop_check_duration: 1.0 nominal: vel_lim: 25.0 lon_acc_lim: 5.0 diff --git a/control/vehicle_cmd_gate/package.xml b/control/vehicle_cmd_gate/package.xml index f2a3fc83e8d63..df1de9f370cb6 100644 --- a/control/vehicle_cmd_gate/package.xml +++ b/control/vehicle_cmd_gate/package.xml @@ -21,6 +21,7 @@ component_interface_utils diagnostic_updater geometry_msgs + motion_utils rclcpp rclcpp_components std_srvs diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp index 8af8c2426b430..25221c652e81d 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.cpp @@ -51,6 +51,9 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) rclcpp::QoS durable_qos{1}; durable_qos.transient_local(); + // Stop Checker + vehicle_stop_checker_ = std::make_unique(this); + // Publisher vehicle_cmd_emergency_pub_ = create_publisher("output/vehicle_cmd_emergency", durable_qos); @@ -145,6 +148,7 @@ VehicleCmdGate::VehicleCmdGate(const rclcpp::NodeOptions & node_options) emergency_acceleration_ = declare_parameter("emergency_acceleration"); moderate_stop_service_acceleration_ = declare_parameter("moderate_stop_service_acceleration"); + stop_check_duration_ = declare_parameter("stop_check_duration"); // Vehicle Parameter const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -492,7 +496,7 @@ AckermannControlCommand VehicleCmdGate::filterControlCommand(const AckermannCont const double dt = getDt(); const auto mode = current_operation_mode_; const auto current_status_cmd = getActualStatusAsCommand(); - const auto ego_is_stopped = std::abs(current_status_cmd.longitudinal.speed) < 1e-3; + const auto ego_is_stopped = vehicle_stop_checker_->isVehicleStopped(stop_check_duration_); const auto input_cmd_is_stopping = in.longitudinal.acceleration < 0.0; // Apply transition_filter when transiting from MANUAL to AUTO. diff --git a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp index 1a75d3700f346..17bfe99d45251 100644 --- a/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp +++ b/control/vehicle_cmd_gate/src/vehicle_cmd_gate.hpp @@ -20,6 +20,7 @@ #include "vehicle_cmd_filter.hpp" #include +#include #include #include @@ -70,6 +71,7 @@ using nav_msgs::msg::Odometry; using EngageMsg = autoware_auto_vehicle_msgs::msg::Engage; using EngageSrv = tier4_external_api_msgs::srv::Engage; +using motion_utils::VehicleStopChecker; struct Commands { AckermannControlCommand control; @@ -219,6 +221,10 @@ class VehicleCmdGate : public rclcpp::Node // Pause interface for API std::unique_ptr adapi_pause_; std::unique_ptr moderate_stop_interface_; + + // stop checker + std::unique_ptr vehicle_stop_checker_; + double stop_check_duration_; }; } // namespace vehicle_cmd_gate diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py index 6dc0b2bea85b3..6150abdafe7d6 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/behavior_planning.launch.py @@ -102,7 +102,7 @@ def launch_setup(context, *args, **kwargs): behavior_path_planner_param, vehicle_param, { - "lane_change.enable_abort_lane_change": LaunchConfiguration( + "lane_change.cancel.enable_on_lane_changing_phase": LaunchConfiguration( "use_experimental_lane_change_function" ), "lane_change.enable_collision_check_at_prepare_phase": LaunchConfiguration( diff --git a/perception/tensorrt_yolox/CMakeLists.txt b/perception/tensorrt_yolox/CMakeLists.txt index 08263d05fbe83..01ade9bac43d6 100644 --- a/perception/tensorrt_yolox/CMakeLists.txt +++ b/perception/tensorrt_yolox/CMakeLists.txt @@ -63,6 +63,8 @@ function(download FILE_NAME FILE_HASH) endfunction() download(yolox-tiny.onnx 97028baf73ce55e115599c9c60651b08) +download(yolox-sPlus-opt.onnx bf3b0155351f90fcdca2626acbfd3bcf) +download(yolox-sPlus-opt.EntropyV2-calibration.table c6e6f1999d5724a017516a956096701f) download(label.txt 9ceadca8b72b6169ee6aabb861fe3e1e) ########## diff --git a/perception/tensorrt_yolox/README.md b/perception/tensorrt_yolox/README.md index a8332ad9eeec1..3c253a0b68489 100644 --- a/perception/tensorrt_yolox/README.md +++ b/perception/tensorrt_yolox/README.md @@ -79,11 +79,17 @@ hence these parameters are ignored when users specify ONNX models including this This package accepts both `EfficientNMS_TRT` attached ONNXs and [models published from the official YOLOX repository](https://github.com/Megvii-BaseDetection/YOLOX/tree/main/demo/ONNXRuntime#download-onnx-models) (we referred to them as "plain" models). +In addition to `yolox-tiny.onnx`, a custom model named `yolox-sPlus-opt.onnx` is either available. +This model is based on YOLOX-s and tuned to perform more accurate detection with almost comparable execution speed with `yolox-tiny`. +To get better results with this model, users are recommended to use some specific running arguments +such as `precision:=int8`, `calibration_algorithm:=Entropy`, `clip_value:=6.0`. +Users can refer `launch/yolox_sPlus_opt.launch.xml` to see how this model can be used. + All models are automatically converted to TensorRT format. These converted files will be saved in the same directory as specified ONNX files with `.engine` filename extension and reused from the next run. -The conversion process may take a while (typically a few minutes) and the inference process is blocked -until complete the conversion, so it will take some time until detection results are published on the first run. +The conversion process may take a while (**typically 10 to 20 minutes**) and the inference process is blocked +until complete the conversion, so it will take some time until detection results are published (**even until appearing in the topic list**) on the first run ### Package acceptable model generation diff --git a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp index 2bdb9a83bfbf9..bdd6d6fe66861 100644 --- a/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp +++ b/perception/tensorrt_yolox/include/tensorrt_yolox/tensorrt_yolox.hpp @@ -81,9 +81,8 @@ class TrtYoloX const std::string & model_path, const std::string & precision, const int num_class = 8, const float score_threshold = 0.3, const float nms_threshold = 0.7, const tensorrt_common::BuildConfig build_config = tensorrt_common::BuildConfig(), - const bool use_gpu_preprocess = false, - const std::string & calibration_image_list_file = std::string(), const double norm_factor = 1.0, - const std::string & cache_dir = "", + const bool use_gpu_preprocess = false, std::string calibration_image_list_file = std::string(), + const double norm_factor = 1.0, const std::string & cache_dir = "", const tensorrt_common::BatchConfig & batch_config = {1, 1, 1}, const size_t max_workspace_size = (1 << 30)); @@ -101,7 +100,7 @@ class TrtYoloX * @warning if we don't allocate buffers using it, "preprocess_gpu" allocates buffers at the * beginning */ - void initPreprocesBuffer(int width, int height); + void initPreprocessBuffer(int width, int height); /** * @brief output TensorRT profiles for each layer @@ -169,11 +168,11 @@ class TrtYoloX CudaUniquePtrHost out_prob_h_; - // flag whether prepreceses are performed on GPU + // flag whether preprocess are performed on GPU bool use_gpu_preprocess_; - // host buffer for preprecessing on GPU + // host buffer for preprocessing on GPU CudaUniquePtrHost image_buf_h_; - // device buffer for preprecessing on GPU + // device buffer for preprocessing on GPU CudaUniquePtr image_buf_d_; // normalization factor used for preprocessing double norm_factor_; diff --git a/perception/tensorrt_yolox/launch/yolox.launch.xml b/perception/tensorrt_yolox/launch/yolox.launch.xml deleted file mode 100644 index d8c67e39e0b8a..0000000000000 --- a/perception/tensorrt_yolox/launch/yolox.launch.xml +++ /dev/null @@ -1,56 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/perception/tensorrt_yolox/launch/yolox.launch.xml b/perception/tensorrt_yolox/launch/yolox.launch.xml new file mode 120000 index 0000000000000..06ee1ae877c0c --- /dev/null +++ b/perception/tensorrt_yolox/launch/yolox.launch.xml @@ -0,0 +1 @@ +yolox_s_plus_opt.launch.xml \ No newline at end of file diff --git a/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml new file mode 100644 index 0000000000000..43f59e944f889 --- /dev/null +++ b/perception/tensorrt_yolox/launch/yolox_s_plus_opt.launch.xml @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml b/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml new file mode 100644 index 0000000000000..d8c67e39e0b8a --- /dev/null +++ b/perception/tensorrt_yolox/launch/yolox_tiny.launch.xml @@ -0,0 +1,56 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp index cddaddd1098bb..515ff1d3c6e9e 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox.cpp @@ -104,21 +104,31 @@ namespace tensorrt_yolox TrtYoloX::TrtYoloX( const std::string & model_path, const std::string & precision, const int num_class, const float score_threshold, const float nms_threshold, tensorrt_common::BuildConfig build_config, - const bool use_gpu_preprocess, const std::string & calibration_image_list_file, - const double norm_factor, [[maybe_unused]] const std::string & cache_dir, - const tensorrt_common::BatchConfig & batch_config, const size_t max_workspace_size) + const bool use_gpu_preprocess, std::string calibration_image_list_path, const double norm_factor, + [[maybe_unused]] const std::string & cache_dir, const tensorrt_common::BatchConfig & batch_config, + const size_t max_workspace_size) { src_width_ = -1; src_height_ = -1; norm_factor_ = norm_factor; if (precision == "int8") { - if (calibration_image_list_file.empty()) { - throw std::runtime_error( - "calibration_image_list_file should be passed to generate int8 engine."); + if (build_config.clip_value <= 0.0) { + if (calibration_image_list_path.empty()) { + throw std::runtime_error( + "calibration_image_list_path should be passed to generate int8 engine " + "or specify values larger than zero to clip_value."); + } + } else { + // if clip value is larger than zero, calibration file is not needed + calibration_image_list_path = ""; } + int max_batch_size = 1; nvinfer1::Dims input_dims = tensorrt_common::get_input_dims(model_path); - std::vector calibration_images = loadImageList(calibration_image_list_file, ""); + std::vector calibration_images; + if (calibration_image_list_path != "") { + calibration_images = loadImageList(calibration_image_list_path, ""); + } tensorrt_yolox::ImageStream stream(max_batch_size, input_dims, calibration_images); fs::path calibration_table{model_path}; std::string calibName = ""; @@ -231,9 +241,9 @@ TrtYoloX::TrtYoloX( } } -void TrtYoloX::initPreprocesBuffer(int width, int height) +void TrtYoloX::initPreprocessBuffer(int width, int height) { - // if size of source input has benn changed... + // if size of source input has been changed... if (src_width_ != -1 || src_height_ != -1) { if (width != src_width_ || height != src_height_) { // Free cuda memory to reallocate @@ -315,7 +325,7 @@ void TrtYoloX::preprocessGpu(const std::vector & images) CHECK_CUDA_ERROR(cudaMemcpyAsync( image_buf_d_.get(), image_buf_h_.get(), image.cols * image.rows * 3 * sizeof(unsigned char), cudaMemcpyHostToDevice, *stream_)); - // Preprcess on GPU + // Preprocess on GPU resize_bilinear_letterbox_nhwc_to_nchw32_gpu( input_d_.get(), image_buf_d_.get(), input_width, input_height, 3, image.cols, image.rows, 3, static_cast(norm_factor_), *stream_); diff --git a/planning/behavior_path_planner/README.md b/planning/behavior_path_planner/README.md index 8bf46fb3d3ae7..07d949aae8a39 100644 --- a/planning/behavior_path_planner/README.md +++ b/planning/behavior_path_planner/README.md @@ -16,16 +16,16 @@ The `behavior_path_planner` module is responsible to generate Behavior path planner has following scene modules. -| Name | Description | Details | -| :------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :----------------------------------------------------------- | -| Lane Following | this module generates reference path from lanelet centerline. | LINK | -| Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](./docs/behavior_path_planner_avoidance_design.md) | -| Avoidance By LC | this module generates lane change path when there is objects that should be avoid. | LINK | -| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](./docs/behavior_path_planner_lane_change_design.md) | -| External Lane Change | WIP | LINK | -| Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](./docs/behavior_path_planner_goal_planner_design.md) | -| Pull Out | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](./docs/behavior_path_planner_start_planner_design.md) | -| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](./docs/behavior_path_planner_side_shift_design.md) | +| Name | Description | Details | +| :------------------- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :---------------------------------------------------------------------- | +| Lane Following | this module generates reference path from lanelet centerline. | LINK | +| Avoidance | this module generates avoidance path when there is objects that should be avoid. | [LINK](./docs/behavior_path_planner_avoidance_design.md) | +| Avoidance By LC | this module generates lane change path when there is objects that should be avoid. | [LINK](./docs/behavior_path_planner_avoidance_by_lane_change_design.md) | +| Lane Change | this module is performed when it is necessary and a collision check with other vehicles is cleared. | [LINK](./docs/behavior_path_planner_lane_change_design.md) | +| External Lane Change | WIP | LINK | +| Goal Planner | this module is performed when ego-vehicle is in the road lane and goal is in the shoulder lane. ego-vehicle will stop at the goal. | [LINK](./docs/behavior_path_planner_goal_planner_design.md) | +| Pull Out | this module is performed when ego-vehicle is stationary and footprint of ego-vehicle is included in shoulder lane. This module ends when ego-vehicle merges into the road. | [LINK](./docs/behavior_path_planner_start_planner_design.md) | +| Side Shift | (for remote control) shift the path to left or right according to an external instruction. | [LINK](./docs/behavior_path_planner_side_shift_design.md) | ![behavior_modules](./image/behavior_modules.png) diff --git a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml index 79aca0867cce9..7c785efa0d5a9 100644 --- a/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml +++ b/planning/behavior_path_planner/config/avoidance/avoidance.param.yaml @@ -28,7 +28,7 @@ # avoidance is performed for the object type with true target_object: car: - enable: true # [-] + is_target: true # [-] moving_speed_threshold: 1.0 # [m/s] moving_time_threshold: 1.0 # [s] max_expand_ratio: 0.0 # [-] @@ -37,7 +37,7 @@ safety_buffer_lateral: 0.7 # [m] safety_buffer_longitudinal: 0.0 # [m] truck: - enable: true + is_target: true moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -46,7 +46,7 @@ safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 bus: - enable: true + is_target: true moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -55,7 +55,7 @@ safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 trailer: - enable: true + is_target: true moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -64,7 +64,7 @@ safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 unknown: - enable: false + is_target: false moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -73,7 +73,7 @@ safety_buffer_lateral: 0.7 safety_buffer_longitudinal: 0.0 bicycle: - enable: false + is_target: false moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -82,7 +82,7 @@ safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 motorcycle: - enable: false + is_target: false moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -91,7 +91,7 @@ safety_buffer_lateral: 1.0 safety_buffer_longitudinal: 1.0 pedestrian: - enable: false + is_target: false moving_speed_threshold: 1.0 moving_time_threshold: 1.0 max_expand_ratio: 0.0 @@ -104,12 +104,12 @@ # For target object filtering target_filtering: - # filtering moving objects - threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] + # params for avoidance of not-parked objects + threshold_time_force_avoidance_for_stopped_vehicle: 10.0 # [s] + object_ignore_section_traffic_light_in_front_distance: 30.0 # [m] + object_ignore_section_crosswalk_in_front_distance: 30.0 # [m] + object_ignore_section_crosswalk_behind_distance: 30.0 # [m] # detection range - object_ignore_distance_traffic_light: 30.0 # [m] - object_ignore_distance_crosswalk_forward: 30.0 # [m] - object_ignore_distance_crosswalk_backward: 30.0 # [m] object_check_forward_distance: 150.0 # [m] object_check_backward_distance: 2.0 # [m] object_check_goal_distance: 20.0 # [m] diff --git a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml index 95ff106eb4c00..6f5105c46eb99 100644 --- a/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml +++ b/planning/behavior_path_planner/config/lane_change/lane_change.param.yaml @@ -48,15 +48,16 @@ enable_prepare_segment_collision_check: true prepare_segment_ignore_object_velocity_thresh: 0.1 # [m/s] use_predicted_path_outside_lanelet: true - use_all_predicted_path: false + use_all_predicted_path: true - # abort - enable_cancel_lane_change: true - enable_abort_lane_change: false - - abort_delta_time: 1.0 # [s] - aborting_time: 5.0 # [s] - abort_max_lateral_jerk: 1000.0 # [m/s3] + # lane change cancel + cancel: + enable_on_prepare_phase: true + enable_on_lane_changing_phase: true + delta_time: 1.0 # [s] + duration: 5.0 # [s] + max_lateral_jerk: 1000.0 # [m/s3] + overhang_tolerance: 0.0 # [m] finish_judge_lateral_threshold: 0.2 # [m] diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design.md new file mode 100644 index 0000000000000..2c91cc43995ac --- /dev/null +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_by_lane_change_design.md @@ -0,0 +1,40 @@ +# Avoidance by lane change design + +This is a sub-module to avoid obstacles by lane change maneuver. + +## Purpose / Role + +This module is designed as one of the obstacle avoidance features and generates a lane change path if the following conditions are satisfied. + +- Exist lane changeable lanelet. +- Exist avoidance target objects on ego driving lane. + +![avoidance_by_lane_change](../image/avoidance_by_lane_change/avoidance_by_lane_change.svg) + +## Inner-workings / Algorithms + +Basically, this module is implemented by reusing the avoidance target filtering logic of the existing [Normal Avoidance Module](./behavior_path_planner_avoidance_design.md) and the path generation logic of the [Normal Lane Change Module](./behavior_path_planner_lane_change_design.md). On the other hand, the conditions under which the module is activated differ from those of a normal avoidance module. + +Check that the following conditions are satisfied after the filtering process for the avoidance target. + +### Number of the avoidance target objects + +This module is launched when the number of avoidance target objects on **EGO DRIVING LANE** is greater than `execute_object_num`. If there are no avoidance targets in the ego driving lane or their number is less than the parameter, the obstacle is avoided by normal avoidance behavior (if the normal avoidance module is registered). + +![trigger_1](../image/avoidance_by_lane_change/avoidance_by_lc_trigger_1.svg) + +### Lane change end point condition + +Unlike the normal avoidance module, which specifies the shift line end point, this module does not specify its end point when generating a lane change path. On the other hand, setting `execute_only_when_lane_change_finish_before_object` to `true` will activate this module only if the lane change can be completed before the avoidance target object. + +Although setting the parameter to `false` would increase the scene of avoidance by lane change, it is assumed that sufficient lateral margin may not be ensured in some cases because the vehicle passes by the side of obstacles during the lane change. + +![trigger_2](../image/avoidance_by_lane_change/avoidance_by_lc_trigger_2.svg) + +## Parameters + +| Name | Unit | Type | Description | Default value | +| :------------------------------------------------- | ---- | ------ | ---------------------------------------------------------------------------------------------------------------------------------------- | ------------- | +| execute_object_num | [-] | int | Number of avoidance target objects on ego driving lane is greater than this value, this module will be launched. | 1 | +| execute_object_longitudinal_margin | [m] | double | [maybe unused] Only when distance between the ego and avoidance target object is longer than this value, this module will be launched. | 0.0 | +| execute_only_when_lane_change_finish_before_object | [-] | bool | If this flag set `true`, this module will be launched only when the lane change end point is **NOT** behind the avoidance target object. | true | diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md index b023b0dff9602..b290a1efa90da 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_avoidance_design.md @@ -608,7 +608,7 @@ This module supports all object classes, and it can set following parameters ind ```yaml car: - enable: true # [-] + is_target: true # [-] moving_speed_threshold: 1.0 # [m/s] moving_time_threshold: 1.0 # [s] max_expand_ratio: 0.0 # [-] @@ -618,15 +618,15 @@ car: safety_buffer_longitudinal: 0.0 # [m] ``` -| Name | Unit | Type | Description | Default value | -| :------------------------- | ----- | ------ | ---------------------------------------------------------------------------------------------------------- | ------------- | -| enable | [-] | bool | Allow avoidance for object type CAR | false | -| moving_speed_threshold | [m/s] | double | Objects with speed greater than this will be judged as moving ones. | 1.0 | -| moving_time_threshold | [s] | double | Objects keep moving longer duration than this will be excluded from avoidance target. | 1.0 | -| envelope_buffer_margin | [m] | double | Allow avoidance for object type TRAILER | 0.3 | -| avoid_margin_lateral | [m] | double | The lateral distance between ego and avoidance targets. | 1.0 | -| safety_buffer_lateral | [m] | double | Creates an additional lateral gap that will prevent the vehicle from getting to near to the obstacle. | 0.5 | -| safety_buffer_longitudinal | [m] | double | Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle. | 0.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------- | ----- | ------ | ------------------------------------------------------------------------------------------------------------------------- | ------------- | +| is_target | [-] | bool | By setting this flag `true`, this module avoid those class objects. | false | +| moving_speed_threshold | [m/s] | double | Objects with speed greater than this will be judged as moving ones. | 1.0 | +| moving_time_threshold | [s] | double | Objects keep moving longer duration than this will be excluded from avoidance target. | 1.0 | +| envelope_buffer_margin | [m] | double | The buffer between raw boundary box of detected objects and enveloped polygon that is used for avoidance path generation. | 0.3 | +| avoid_margin_lateral | [m] | double | The lateral distance between ego and avoidance targets. | 1.0 | +| safety_buffer_lateral | [m] | double | Creates an additional lateral gap that will prevent the vehicle from getting to near to the obstacle. | 0.5 | +| safety_buffer_longitudinal | [m] | double | Creates an additional longitudinal gap that will prevent the vehicle from getting to near to the obstacle. | 0.0 | Parameters for the logic to compensate perception noise of the far objects. @@ -638,18 +638,18 @@ Parameters for the logic to compensate perception noise of the far objects. namespace: `avoidance.target_filtering.` -| Name | Unit | Type | Description | Default value | -| :---------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | -| threshold_distance_object_is_on_center | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 1.0 | -| object_ignore_distance_traffic_light | [m] | double | If the distance between traffic light and vehicle is less than this parameter, this module will ignore it. | 30.0 | -| object_ignore_distance_crosswalk_forward | [m] | double | If the front distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | -| object_ignore_distance_crosswalk_backward | [m] | double | If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | -| object_check_forward_distance | [m] | double | Forward distance to search the avoidance target. | 150.0 | -| object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 | -| object_check_goal_distance | [m] | double | Backward distance to search the avoidance target. | 20.0 | -| object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 | -| object_check_min_road_shoulder_width | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.5 | -| object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 | +| Name | Unit | Type | Description | Default value | +| :---------------------------------------------------- | :--- | :----- | :------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------- | :------------ | +| threshold_distance_object_is_on_center | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 1.0 | +| object_ignore_section_traffic_light_in_front_distance | [m] | double | If the distance between traffic light and vehicle is less than this parameter, this module will ignore it. | 30.0 | +| object_ignore_section_crosswalk_in_front_distance | [m] | double | If the front distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | +| object_ignore_section_crosswalk_behind_distance | [m] | double | If the back distance between crosswalk and vehicle is less than this parameter, this module will ignore it. | 30.0 | +| object_check_forward_distance | [m] | double | Forward distance to search the avoidance target. | 150.0 | +| object_check_backward_distance | [m] | double | Backward distance to search the avoidance target. | 2.0 | +| object_check_goal_distance | [m] | double | Backward distance to search the avoidance target. | 20.0 | +| object_check_shiftable_ratio | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.6 | +| object_check_min_road_shoulder_width | [m] | double | Vehicles around the center line within this distance will be excluded from avoidance target. | 0.5 | +| object_last_seen_threshold | [s] | double | For the compensation of the detection lost. The object is registered once it is observed as an avoidance target. When the detection loses, the timer will start and the object will be un-registered when the time exceeds this limit. | 2.0 | ### Safety check parameters diff --git a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md index 945ca579c434a..ae49542b3970b 100644 --- a/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md +++ b/planning/behavior_path_planner/docs/behavior_path_planner_lane_change_design.md @@ -240,7 +240,7 @@ detach Suppose the lane change trajectory is evaluated as unsafe. In that case, if the ego vehicle has not departed from the current lane yet, the trajectory will be reset, and the ego vehicle will resume the lane following the maneuver. -The function can be enabled by setting `enable_cancel_lane_change` to `true`. +The function can be enabled by setting `enable_on_prepare_phase` to `true`. The following image illustrates the cancel process. @@ -248,7 +248,7 @@ The following image illustrates the cancel process. #### Abort -Assume the ego vehicle has already departed from the current lane. In that case, it is dangerous to cancel the path, and it will cause the ego vehicle to change the heading direction abruptly. In this case, planning a trajectory that allows the ego vehicle to return to the current path while minimizing the heading changes is necessary. In this case, the lane change module will generate an abort path. The following images show an example of the abort path. Do note that the function DOESN'T GUARANTEE a safe abort process, as it didn't check the presence of the surrounding objects and/or their reactions. The function can be enable manually by setting both `enable_cancel_lane_change` and `enable_abort_lane_change` to `true`. The parameter `abort_max_lateral_jerk` need to be set to a high value in order for it to work. +Assume the ego vehicle has already departed from the current lane. In that case, it is dangerous to cancel the path, and it will cause the ego vehicle to change the heading direction abruptly. In this case, planning a trajectory that allows the ego vehicle to return to the current path while minimizing the heading changes is necessary. In this case, the lane change module will generate an abort path. The following images show an example of the abort path. Do note that the function DOESN'T GUARANTEE a safe abort process, as it didn't check the presence of the surrounding objects and/or their reactions. The function can be enable manually by setting both `enable_on_prepare_phase` and `enable_on_lane_changing_phase` to `true`. The parameter `max_lateral_jerk` need to be set to a high value in order for it to work. ![abort](../image/lane_change/cancel_and_abort/lane_change-abort.png) @@ -301,12 +301,14 @@ The following parameters are configurable in `behavior_path_planner.param.yaml`. The following parameters are configurable in `lane_change.param.yaml`. -| Name | Unit | Type | Description | Default value | -| :-------------------------- | ---- | ------- | --------------------------------------- | ------------- | -| `enable_cancel_lane_change` | [-] | boolean | Enable cancel lane change | true | -| `enable_abort_lane_change` | [-] | boolean | Enable abort lane change. | false | -| `abort_delta_time` | [s] | double | The time taken to start aborting. | 3.0 | -| `abort_max_lateral_jerk` | [s] | double | The maximum lateral jerk for abort path | 1000.0 | +| Name | Unit | Type | Description | Default value | +| :------------------------------------- | ------- | ------- | ---------------------------------------------------------------------------------------------------------------- | ------------- | +| `cancel.enable_on_prepare_phase` | [-] | boolean | Enable cancel lane change | true | +| `cancel.enable_on_lane_changing_phase` | [-] | boolean | Enable abort lane change. | false | +| `cancel.delta_time` | [s] | double | The time taken to start steering to return to the center line. | 3.0 | +| `cancel.duration` | [s] | double | The time taken to complete returning to the center line. | 3.0 | +| `cancel.max_lateral_jerk` | [m/sss] | double | The maximum lateral jerk for abort path | 1000.0 | +| `cancel.overhang_tolerance` | [m] | double | Lane change cancel is prohibited if the vehicle head exceeds the lane boundary more than this tolerance distance | 0.0 | ### Debug @@ -318,6 +320,33 @@ The following parameters are configurable in `lane_change.param.yaml`. ## Debug Marker & Visualization -To enable the debug marker, execute `ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner lane_change.publish_debug_marker true` (no restart is needed) or simply set the `publish_debug_marker` to `true` in the `lane_change.param.yaml` for permanent effect (restart is needed). Then add the marker `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lanechange` in `rviz2`. +To enable the debug marker, execute (no restart is needed) -![debug](../image/lane_change/lane_change-debug.png) +```shell +ros2 param set /planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner lane_change.publish_debug_marker true + +``` + +or simply set the `publish_debug_marker` to `true` in the `lane_change.param.yaml` for permanent effect (restart is needed). + +Then add the marker + +```shell +/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/lane_change_left +``` + +in `rviz2`. + +![debug](../image/lane_change/lane_change-debug-1.png) + +![debug2](../image/lane_change/lane_change-debug-2.png) + +![debug3](../image/lane_change/lane_change-debug-3.png) + +Available information + +1. Ego to object relation, plus safety check information +2. Ego vehicle interpolated pose up to the latest safety check position. +3. Object is safe or not, shown by the color of the polygon (Green = Safe, Red = unsafe) +4. Valid candidate paths. +5. Position when lane changing start and end. diff --git a/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lane_change.svg b/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lane_change.svg new file mode 100644 index 0000000000000..78ccb44806661 --- /dev/null +++ b/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lane_change.svg @@ -0,0 +1,54 @@ + + + + + + + + + + + + + + + + + + + diff --git a/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_1.svg b/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_1.svg new file mode 100644 index 0000000000000..db4987f5806b5 --- /dev/null +++ b/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_1.svg @@ -0,0 +1,205 @@ + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ parked on adjacent lane +
+
+
+ + parked on adjacent lane + + + + + + +
+
+
+ moving +
+
+
+ + moving + + + + + + + + +
+
+
+ outside of ego driving lane +
+
+
+ + outside of ego driving lane + + + + + + +
+
+
+ parked on ego driving lane +
+
+
+ + parked on ego driving lane + + + + + + + +
+
+
+ count only these objects +
+
+
+ + count only these objects + + + + + + +
+
+
+ ego +
+
+
+ + ego + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_2.svg b/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_2.svg new file mode 100644 index 0000000000000..304cc33614a90 --- /dev/null +++ b/planning/behavior_path_planner/image/avoidance_by_lane_change/avoidance_by_lc_trigger_2.svg @@ -0,0 +1,85 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ lane change end point +
+
+
+ + lane change end point + + + + + + + Text is not SVG - cannot display + + + diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-1.png b/planning/behavior_path_planner/image/lane_change/lane_change-debug-1.png new file mode 100644 index 0000000000000..7b6c4cf58ccee Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/lane_change-debug-1.png differ diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-2.png b/planning/behavior_path_planner/image/lane_change/lane_change-debug-2.png new file mode 100644 index 0000000000000..42e9611920128 Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/lane_change-debug-2.png differ diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug-3.png b/planning/behavior_path_planner/image/lane_change/lane_change-debug-3.png new file mode 100644 index 0000000000000..9ede6861920b8 Binary files /dev/null and b/planning/behavior_path_planner/image/lane_change/lane_change-debug-3.png differ diff --git a/planning/behavior_path_planner/image/lane_change/lane_change-debug.png b/planning/behavior_path_planner/image/lane_change/lane_change-debug.png deleted file mode 100644 index 501f03742f531..0000000000000 Binary files a/planning/behavior_path_planner/image/lane_change/lane_change-debug.png and /dev/null differ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp index c6bf547ab5a7a..2e204e8c672ad 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/marker_util/debug_utilities.hpp @@ -57,6 +57,10 @@ struct CollisionCheckDebug Pose expected_ego_pose{}; Pose expected_obj_pose{}; Pose relative_to_ego{}; + double rss_longitudinal{0.0}; + double ego_to_obj_margin{0.0}; + double longitudinal_offset{0.0}; + double lateral_offset{0.0}; bool is_front{false}; bool allow_lane_change{false}; std::vector lerped_path; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp index f8d80d8883b38..225b093018d95 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/base_class.hpp @@ -145,9 +145,12 @@ class LaneChangeBase LaneChangeParameters getLaneChangeParam() const { return *lane_change_parameters_; } - bool isCancelEnabled() const { return lane_change_parameters_->enable_cancel_lane_change; } + bool isCancelEnabled() const { return lane_change_parameters_->cancel.enable_on_prepare_phase; } - bool isAbortEnabled() const { return lane_change_parameters_->enable_abort_lane_change; } + bool isAbortEnabled() const + { + return lane_change_parameters_->cancel.enable_on_lane_changing_phase; + } bool isSafe() const { return status_.is_safe; } diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp index 187dca71ae6c8..0b016db9c9c2c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/avoidance/avoidance_module_data.hpp @@ -48,7 +48,7 @@ using geometry_msgs::msg::TransformStamped; struct ObjectParameter { - bool enable{false}; + bool is_target{false}; double moving_speed_threshold{0.0}; @@ -134,13 +134,13 @@ struct AvoidanceParameters double threshold_distance_object_is_on_center; // execute only when there is no intersection behind of the stopped vehicle. - double object_ignore_distance_traffic_light; + double object_ignore_section_traffic_light_in_front_distance; // execute only when there is no crosswalk near the stopped vehicle. - double object_ignore_distance_crosswalk_forward; + double object_ignore_section_crosswalk_in_front_distance; // execute only when there is no crosswalk near the stopped vehicle. - double object_ignore_distance_crosswalk_backward; + double object_ignore_section_crosswalk_behind_distance; // distance to avoid object detection double object_check_forward_distance; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp index 4def596f78746..67a23f23d5827 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/lane_change/lane_change_module_data.hpp @@ -26,6 +26,16 @@ namespace behavior_path_planner { + +struct LaneChangeCancelParameters +{ + bool enable_on_prepare_phase{true}; + bool enable_on_lane_changing_phase{false}; + double delta_time{1.0}; + double duration{5.0}; + double max_lateral_jerk{10.0}; + double overhang_tolerance{0.0}; +}; struct LaneChangeParameters { // trajectory generation @@ -63,12 +73,7 @@ struct LaneChangeParameters bool check_pedestrian{true}; // check object pedestrian // abort - bool enable_cancel_lane_change{true}; - bool enable_abort_lane_change{false}; - - double abort_delta_time{1.0}; - double aborting_time{5.0}; - double abort_max_lateral_jerk{10.0}; + LaneChangeCancelParameters cancel; double finish_judge_lateral_threshold{0.2}; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp index 858d742be867b..8f86af7b39ca1 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/safety_check.hpp @@ -62,9 +62,10 @@ bool isTargetObjectFront( Polygon2d createExtendedPolygon( const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double lon_length, const double lat_margin); + const double lon_length, const double lat_margin, CollisionCheckDebug & debug); Polygon2d createExtendedPolygon( - const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin); + const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, + CollisionCheckDebug & debug); double calcRssDistance( const double front_object_velocity, const double rear_object_velocity, diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp index b5da46148a9f3..cc2d109f0cf1c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utils/utils.hpp @@ -306,7 +306,7 @@ bool isEgoOutOfRoute( bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, - const BehaviorPathPlannerParameters & common_param); + const BehaviorPathPlannerParameters & common_param, const double outer_margin = 0.0); // path management diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 0384a702ff129..4ed96ebe463ae 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -581,7 +581,7 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() { const auto get_object_param = [&](std::string && ns) { ObjectParameter param{}; - param.enable = declare_parameter(ns + "enable"); + param.is_target = declare_parameter(ns + "is_target"); param.moving_speed_threshold = declare_parameter(ns + "moving_speed_threshold"); param.moving_time_threshold = declare_parameter(ns + "moving_time_threshold"); param.max_expand_ratio = declare_parameter(ns + "max_expand_ratio"); @@ -616,12 +616,12 @@ AvoidanceParameters BehaviorPathPlannerNode::getAvoidanceParam() std::string ns = "avoidance.target_filtering."; p.threshold_time_force_avoidance_for_stopped_vehicle = declare_parameter(ns + "threshold_time_force_avoidance_for_stopped_vehicle"); - p.object_ignore_distance_traffic_light = - declare_parameter(ns + "object_ignore_distance_traffic_light"); - p.object_ignore_distance_crosswalk_forward = - declare_parameter(ns + "object_ignore_distance_crosswalk_forward"); - p.object_ignore_distance_crosswalk_backward = - declare_parameter(ns + "object_ignore_distance_crosswalk_backward"); + p.object_ignore_section_traffic_light_in_front_distance = + declare_parameter(ns + "object_ignore_section_traffic_light_in_front_distance"); + p.object_ignore_section_crosswalk_in_front_distance = + declare_parameter(ns + "object_ignore_section_crosswalk_in_front_distance"); + p.object_ignore_section_crosswalk_behind_distance = + declare_parameter(ns + "object_ignore_section_crosswalk_behind_distance"); p.object_check_forward_distance = declare_parameter(ns + "object_check_forward_distance"); p.object_check_backward_distance = @@ -825,13 +825,15 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() p.check_pedestrian = declare_parameter(ns + "pedestrian"); } - // abort - p.enable_cancel_lane_change = declare_parameter(parameter("enable_cancel_lane_change")); - p.enable_abort_lane_change = declare_parameter(parameter("enable_abort_lane_change")); - - p.abort_delta_time = declare_parameter(parameter("abort_delta_time")); - p.aborting_time = declare_parameter(parameter("aborting_time")); - p.abort_max_lateral_jerk = declare_parameter(parameter("abort_max_lateral_jerk")); + // lane change cancel + p.cancel.enable_on_prepare_phase = + declare_parameter(parameter("cancel.enable_on_prepare_phase")); + p.cancel.enable_on_lane_changing_phase = + declare_parameter(parameter("cancel.enable_on_lane_changing_phase")); + p.cancel.delta_time = declare_parameter(parameter("cancel.delta_time")); + p.cancel.duration = declare_parameter(parameter("cancel.duration")); + p.cancel.max_lateral_jerk = declare_parameter(parameter("cancel.max_lateral_jerk")); + p.cancel.overhang_tolerance = declare_parameter(parameter("cancel.overhang_tolerance")); p.finish_judge_lateral_threshold = declare_parameter("lane_change.finish_judge_lateral_threshold"); @@ -850,11 +852,10 @@ LaneChangeParameters BehaviorPathPlannerNode::getLaneChangeParam() exit(EXIT_FAILURE); } - if (p.abort_delta_time < 1.0) { - RCLCPP_FATAL_STREAM( - get_logger(), "abort_delta_time: " << p.abort_delta_time << ", is too short.\n" - << "Terminating the program..."); - exit(EXIT_FAILURE); + if (p.cancel.delta_time < 1.0) { + RCLCPP_WARN_STREAM( + get_logger(), "cancel.delta_time: " << p.cancel.delta_time + << ", is too short. This could cause a danger behavior."); } return p; @@ -1252,7 +1253,10 @@ void BehaviorPathPlannerNode::run() } #ifndef USE_OLD_ARCHITECTURE - if (planner_data_->operation_mode->mode != OperationModeState::AUTONOMOUS) { + const auto controlled_by_autoware_autonomously = + planner_data_->operation_mode->mode == OperationModeState::AUTONOMOUS && + planner_data_->operation_mode->is_autoware_control_enabled; + if (!controlled_by_autoware_autonomously) { planner_manager_->resetRootLanelet(planner_data_); } #endif diff --git a/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp b/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp index 1ae1f8a03e0c0..2345d6f40fb18 100644 --- a/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp +++ b/planning/behavior_path_planner/src/marker_util/lane_change/debug.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "behavior_path_planner/marker_util/debug_utilities.hpp" #include "behavior_path_planner/utils/path_shifter/path_shifter.hpp" #include @@ -24,6 +25,7 @@ #include #include #include +#include #include #include @@ -37,7 +39,7 @@ using tier4_autoware_utils::createMarkerScale; MarkerArray showObjectInfo( const std::unordered_map & obj_debug_vec, std::string && ns) { - Marker marker = createDefaultMarker( + Marker obj_marker = createDefaultMarker( "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, 0L, Marker::TEXT_VIEW_FACING, createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(0.0, 1.0, 1.0, 0.999)); @@ -46,19 +48,24 @@ MarkerArray showObjectInfo( marker_array.markers.reserve(obj_debug_vec.size()); + int idx{0}; + for (const auto & [uuid, info] : obj_debug_vec) { - marker.id = ++id; - marker.pose = info.current_pose; + obj_marker.id = ++id; + obj_marker.pose = info.current_pose; std::ostringstream ss; - ss << info.failed_reason << "\nLon: " << std::setprecision(4) << info.relative_to_ego.position.x - << "\nLat: " << info.relative_to_ego.position.y - << "\nPosition: " << (info.is_front ? "front" : "back"); + ss << "Idx: " << ++idx << "\nReason: " << info.failed_reason + << "\nRSS dist: " << std::setprecision(4) << info.rss_longitudinal + << "\nEgo to obj: " << info.ego_to_obj_margin << "\nLateral offset: " << info.lateral_offset + << "\nLongitudinal offset: " << info.longitudinal_offset + << "\nPosition: " << (info.is_front ? "front" : "back") + << "\nSafe: " << (info.allow_lane_change ? "Yes" : "No"); - marker.text = ss.str(); + obj_marker.text = ss.str(); - marker_array.markers.push_back(marker); + marker_array.markers.push_back(obj_marker); } return marker_array; } @@ -167,23 +174,28 @@ MarkerArray showPolygon( constexpr float scale_val{0.2}; int32_t id{0}; + const auto now = rclcpp::Clock{RCL_ROS_TIME}.now(); Marker ego_marker = createDefaultMarker( - "map", rclcpp::Clock{RCL_ROS_TIME}.now(), ns, id, Marker::LINE_STRIP, - createMarkerScale(scale_val, scale_val, scale_val), createMarkerColor(1.0, 1.0, 1.0, 0.9)); + "map", now, ns, id, Marker::LINE_STRIP, createMarkerScale(scale_val, scale_val, scale_val), + createMarkerColor(1.0, 1.0, 1.0, 0.9)); Marker obj_marker = ego_marker; + + auto text_marker = createDefaultMarker( + "map", now, ns + "_text", id, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(1.5, 1.5, 1.5), createMarkerColor(1.0, 1.0, 1.0, 1.0)); + MarkerArray marker_array; - constexpr auto colors = colorsList(); - const auto loop_size = std::min(obj_debug_vec.size(), colors.size()); + constexpr auto red_color = colorsList().at(0); + constexpr auto green_color = colorsList().at(1); + const auto reserve_size = obj_debug_vec.size(); - marker_array.markers.reserve(loop_size * 2); - size_t idx{0}; + marker_array.markers.reserve(reserve_size * 4); + + int32_t idx = {0}; for (const auto & [uuid, info] : obj_debug_vec) { - if (idx >= loop_size) { - break; - } - const auto & color = colors.at(idx); + const auto & color = info.allow_lane_change ? green_color : red_color; const auto & ego_polygon = info.ego_polygon.outer(); const auto poly_z = info.current_pose.position.z; // temporally ego_marker.id = ++id; @@ -194,6 +206,14 @@ MarkerArray showPolygon( } marker_array.markers.push_back(ego_marker); + std::ostringstream ss; + text_marker.id = ego_marker.id; + ss << ++idx; + text_marker.text = ss.str(); + text_marker.pose = info.expected_ego_pose; + + marker_array.markers.push_back(text_marker); + const auto & obj_polygon = info.obj_polygon.outer(); obj_marker.id = ++id; obj_marker.color = createMarkerColor(color[0], color[1], color[2], 0.8); @@ -202,7 +222,11 @@ MarkerArray showPolygon( obj_marker.points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), poly_z)); } marker_array.markers.push_back(obj_marker); - ++idx; + + text_marker.id = obj_marker.id; + text_marker.pose = info.expected_obj_pose; + marker_array.markers.push_back(text_marker); + ego_marker.points.clear(); obj_marker.points.clear(); } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp index 90dbfa960dbfe..816ed94e5b822 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/manager.cpp @@ -47,7 +47,7 @@ void AvoidanceModuleManager::updateModuleParams(const std::vectorobject_parameters.at(semantic); - updateParam(parameters, ns + "enable", config.enable); + updateParam(parameters, ns + "is_target", config.is_target); updateParam(parameters, ns + "moving_speed_threshold", config.moving_speed_threshold); updateParam(parameters, ns + "moving_time_threshold", config.moving_time_threshold); updateParam(parameters, ns + "max_expand_ratio", config.max_expand_ratio); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp index b56e382cc52ce..6fb84577a3b99 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/normal.cpp @@ -74,6 +74,7 @@ std::pair NormalLaneChange::getSafePath(LaneChangePath & safe_path) LaneChangePaths valid_paths{}; const auto found_safe_path = getLaneChangePaths(current_lanes, target_lanes, direction_, &valid_paths); + debug_valid_path_ = valid_paths; if (valid_paths.empty()) { return {false, false}; @@ -387,7 +388,7 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const const double ego_velocity = std::max(getEgoVelocity(), planner_data_->parameters.minimum_lane_changing_velocity); - const double estimated_travel_dist = ego_velocity * lane_change_parameters_->abort_delta_time; + const double estimated_travel_dist = ego_velocity * lane_change_parameters_->cancel.delta_time; double dist = 0.0; for (size_t idx = nearest_idx; idx < status_.lane_change_path.path.points.size() - 1; ++idx) { @@ -395,7 +396,8 @@ bool NormalLaneChange::isAbleToReturnCurrentLane() const if (dist > estimated_travel_dist) { const auto & estimated_pose = status_.lane_change_path.path.points.at(idx + 1).point.pose; return utils::isEgoWithinOriginalLane( - status_.current_lanes, estimated_pose, planner_data_->parameters); + status_.current_lanes, estimated_pose, planner_data_->parameters, + lane_change_parameters_->cancel.overhang_tolerance); } } @@ -454,7 +456,7 @@ bool NormalLaneChange::hasFinishedAbort() const bool NormalLaneChange::isAbortState() const { - if (!lane_change_parameters_->enable_abort_lane_change) { + if (!lane_change_parameters_->cancel.enable_on_lane_changing_phase) { return false; } @@ -945,9 +947,9 @@ bool NormalLaneChange::getAbortPath() }; const auto [abort_start_idx, abort_start_dist] = - get_abort_idx_and_distance(lane_change_parameters_->abort_delta_time); + get_abort_idx_and_distance(lane_change_parameters_->cancel.delta_time); const auto [abort_return_idx, abort_return_dist] = get_abort_idx_and_distance( - lane_change_parameters_->abort_delta_time + lane_change_parameters_->aborting_time); + lane_change_parameters_->cancel.delta_time + lane_change_parameters_->cancel.delta_time); if (abort_start_idx >= abort_return_idx) { RCLCPP_ERROR(logger_, "abort start idx and return idx is equal. can't compute abort path."); @@ -983,7 +985,7 @@ bool NormalLaneChange::getAbortPath() const double & max_lateral_acc = lateral_acc_range.second; path_shifter.setLateralAccelerationLimit(max_lateral_acc); - if (lateral_jerk > lane_change_parameters_->abort_max_lateral_jerk) { + if (lateral_jerk > lane_change_parameters_->cancel.max_lateral_jerk) { RCLCPP_ERROR(logger_, "Aborting jerk is too strong. lateral_jerk = %f", lateral_jerk); return false; } diff --git a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp index 67ad82a489ade..b2cf74893666b 100644 --- a/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/start_planner/start_planner_module.cpp @@ -846,7 +846,8 @@ TurnSignalInfo StartPlannerModule::calcTurnSignalInfo() const const bool is_near_intersection = std::invoke([&]() { const double check_length = parameters_->intersection_search_length; double accumulated_length = 0.0; - for (size_t i = 0; i < path.points.size() - 1; ++i) { + const size_t current_idx = motion_utils::findNearestIndex(path.points, current_pose.position); + for (size_t i = current_idx; i < path.points.size() - 1; ++i) { const auto & p = path.points.at(i); for (const auto & lane : planner_data_->route_handler->getLaneletsFromIds(p.lane_ids)) { const std::string turn_direction = lane.attributeOr("turn_direction", "else"); diff --git a/planning/behavior_path_planner/src/turn_signal_decider.cpp b/planning/behavior_path_planner/src/turn_signal_decider.cpp index ac11941ec1eb6..78ea16dbc3c05 100644 --- a/planning/behavior_path_planner/src/turn_signal_decider.cpp +++ b/planning/behavior_path_planner/src/turn_signal_decider.cpp @@ -140,29 +140,94 @@ boost::optional TurnSignalDecider::getIntersectionTurnSignalInfo } } - std::queue signal_queue; + // combine consecutive lanes of the same turn direction + // stores lanes that have already been combine + std::set processed_lanes; + // since combined_lane does not inherit id and attribute, + // and ConstantLanelet does not rewrite the value, + // we keep front_lane together as a representative. + std::vector> combined_and_front_vec{}; for (const auto & lane_id : unique_lane_ids) { - const auto lane = route_handler.getLaneletsFromId(lane_id); - const double search_distance = lane.attributeOr("turn_signal_distance", base_search_distance); - if (lane.centerline3d().size() < 2) { + // Skip if already processed + if (processed_lanes.find(lane_id) != processed_lanes.end()) continue; + auto current_lane = route_handler.getLaneletsFromId(lane_id); + lanelet::ConstLanelets combined_lane_elems{}; + while (rclcpp::ok()) { + processed_lanes.insert(current_lane.id()); + combined_lane_elems.push_back(current_lane); + + // Get the lane and its attribute + const std::string lane_attribute = + current_lane.attributeOr("turn_direction", std::string("none")); + + // check next lanes + auto next_lanes = route_handler.getNextLanelets(current_lane); + if (next_lanes.empty()) { + break; + } + + // filter next lanes with same attribute in the path + std::vector next_lanes_in_path{}; + std::copy_if( + next_lanes.begin(), next_lanes.end(), std::back_inserter(next_lanes_in_path), + [&](const auto & next_lane) { + const bool is_in_unique_ids = + std::find(unique_lane_ids.begin(), unique_lane_ids.end(), next_lane.id()) != + unique_lane_ids.end(); + const bool has_same_attribute = + next_lane.attributeOr("turn_direction", std::string("none")) == lane_attribute; + return is_in_unique_ids && has_same_attribute; + }); + if (next_lanes_in_path.empty()) { + break; + } + + // assume that the next lane in the path is only one + current_lane = next_lanes_in_path.front(); + } + + if (!combined_lane_elems.empty()) { + // store combined lane and its front lane + const auto & combined_and_first = std::pair( + lanelet::utils::combineLaneletsShape(combined_lane_elems), combined_lane_elems.front()); + combined_and_front_vec.push_back(combined_and_first); + } + } + + std::queue signal_queue; + for (const auto & combined_and_front : combined_and_front_vec) { + // use combined_lane's centerline + const auto & combined_lane = combined_and_front.first; + if (combined_lane.centerline3d().size() < 2) { continue; } + // use front lane's id, attribute, and search distance as a representative + const auto & front_lane = combined_and_front.second; + const auto lane_id = front_lane.id(); + const double search_distance = + front_lane.attributeOr("turn_signal_distance", base_search_distance); + const std::string lane_attribute = + front_lane.attributeOr("turn_direction", std::string("none")); + // lane front and back pose Pose lane_front_pose; Pose lane_back_pose; - lane_front_pose.position = lanelet::utils::conversion::toGeomMsgPt(lane.centerline3d().front()); - lane_back_pose.position = lanelet::utils::conversion::toGeomMsgPt(lane.centerline3d().back()); + lane_front_pose.position = + lanelet::utils::conversion::toGeomMsgPt(combined_lane.centerline3d().front()); + lane_back_pose.position = + lanelet::utils::conversion::toGeomMsgPt(combined_lane.centerline3d().back()); { const auto & current_point = lane_front_pose.position; - const auto & next_point = lanelet::utils::conversion::toGeomMsgPt(lane.centerline3d()[1]); + const auto & next_point = + lanelet::utils::conversion::toGeomMsgPt(combined_lane.centerline3d()[1]); lane_front_pose.orientation = calc_orientation(current_point, next_point); } { const auto & current_point = lanelet::utils::conversion::toGeomMsgPt( - lane.centerline3d()[lane.centerline3d().size() - 2]); + combined_lane.centerline3d()[combined_lane.centerline3d().size() - 2]); const auto & next_point = lane_back_pose.position; lane_back_pose.orientation = calc_orientation(current_point, next_point); } @@ -195,7 +260,6 @@ boost::optional TurnSignalDecider::getIntersectionTurnSignalInfo continue; } - const std::string lane_attribute = lane.attributeOr("turn_direction", std::string("none")); if ( (lane_attribute == "right" || lane_attribute == "left" || lane_attribute == "straight") && dist_to_front_point < search_distance) { @@ -207,7 +271,7 @@ boost::optional TurnSignalDecider::getIntersectionTurnSignalInfo TurnSignalInfo turn_signal_info{}; turn_signal_info.desired_start_point = desired_start_point_map_.at(lane_id); turn_signal_info.required_start_point = lane_front_pose; - turn_signal_info.required_end_point = get_required_end_point(lane.centerline3d()); + turn_signal_info.required_end_point = get_required_end_point(combined_lane.centerline3d()); turn_signal_info.desired_end_point = lane_back_pose; turn_signal_info.turn_signal.command = signal_map.at(lane_attribute); signal_queue.push(turn_signal_info); diff --git a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp index 8d33faf82e645..5fd1457042693 100644 --- a/planning/behavior_path_planner/src/utils/avoidance/utils.cpp +++ b/planning/behavior_path_planner/src/utils/avoidance/utils.cpp @@ -107,7 +107,7 @@ bool isTargetObjectType( return false; } - return parameters->object_parameters.at(t).enable; + return parameters->object_parameters.at(t).is_target; } bool isVehicleTypeObject(const ObjectData & object) @@ -951,7 +951,8 @@ void filterTargetObjects( const auto to_traffic_light = utils::getDistanceToNextTrafficLight(object_pose, data.current_lanelets); { - not_parked_object = to_traffic_light < parameters->object_ignore_distance_traffic_light; + not_parked_object = + to_traffic_light < parameters->object_ignore_section_traffic_light_in_front_distance; } // check crosswalk @@ -961,8 +962,8 @@ void filterTargetObjects( o.longitudinal; { const auto stop_for_crosswalk = - to_crosswalk < parameters->object_ignore_distance_crosswalk_forward && - to_crosswalk > -1.0 * parameters->object_ignore_distance_crosswalk_backward; + to_crosswalk < parameters->object_ignore_section_crosswalk_in_front_distance && + to_crosswalk > -1.0 * parameters->object_ignore_section_crosswalk_behind_distance; not_parked_object = not_parked_object || stop_for_crosswalk; } diff --git a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp index 919541f5972a4..04224b380414e 100644 --- a/planning/behavior_path_planner/src/utils/lane_change/utils.cpp +++ b/planning/behavior_path_planner/src/utils/lane_change/utils.cpp @@ -396,8 +396,8 @@ PathSafetyStatus isLaneChangePathSafe( return std::make_pair(tier4_autoware_utils::toHexString(obj.object_id), debug); }; - const auto appendDebugInfo = - [&debug_data](std::pair & obj, bool && is_allowed) { + const auto updateDebugInfo = + [&debug_data](std::pair & obj, bool is_allowed) { const auto & key = obj.first; auto & element = obj.second; element.allow_lane_change = is_allowed; @@ -438,7 +438,7 @@ PathSafetyStatus isLaneChangePathSafe( lane_change_path.duration.prepare, lane_change_parameter.prepare_segment_ignore_object_velocity_thresh)) { path_safety_status.is_safe = false; - appendDebugInfo(current_debug_data, false); + updateDebugInfo(current_debug_data, path_safety_status.is_safe); if (isObjectIndexIncluded(i, dynamic_objects_indices.target_lane)) { const auto & obj_pose = obj.kinematics.initial_pose_with_covariance.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); @@ -448,7 +448,7 @@ PathSafetyStatus isLaneChangePathSafe( } } } - appendDebugInfo(current_debug_data, true); + updateDebugInfo(current_debug_data, path_safety_status.is_safe); } if (!lane_change_parameter.use_predicted_path_outside_lanelet) { @@ -469,13 +469,13 @@ PathSafetyStatus isLaneChangePathSafe( lane_change_parameter.prepare_segment_ignore_object_velocity_thresh, front_decel, rear_decel, current_debug_data.second)) { path_safety_status.is_safe = false; - appendDebugInfo(current_debug_data, false); + updateDebugInfo(current_debug_data, path_safety_status.is_safe); const auto & obj_pose = obj.kinematics.initial_pose_with_covariance.pose; const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, obj.shape); path_safety_status.is_object_coming_from_rear |= !utils::safety_check::isTargetObjectFront( path, current_pose, common_parameter.vehicle_info, obj_polygon); } - appendDebugInfo(current_debug_data, true); + updateDebugInfo(current_debug_data, path_safety_status.is_safe); } return path_safety_status; } diff --git a/planning/behavior_path_planner/src/utils/safety_check.cpp b/planning/behavior_path_planner/src/utils/safety_check.cpp index 72bba383a22f7..ef450c3538f65 100644 --- a/planning/behavior_path_planner/src/utils/safety_check.cpp +++ b/planning/behavior_path_planner/src/utils/safety_check.cpp @@ -14,6 +14,7 @@ #include "behavior_path_planner/utils/safety_check.hpp" +#include "behavior_path_planner/marker_util/debug_utilities.hpp" #include "motion_utils/trajectory/trajectory.hpp" #include "perception_utils/predicted_path_utils.hpp" @@ -49,14 +50,21 @@ bool isTargetObjectFront( Polygon2d createExtendedPolygon( const Pose & base_link_pose, const vehicle_info_util::VehicleInfo & vehicle_info, - const double lon_length, const double lat_margin) + const double lon_length, const double lat_margin, CollisionCheckDebug & debug) { const double & base_to_front = vehicle_info.max_longitudinal_offset_m; const double & width = vehicle_info.vehicle_width_m; const double & base_to_rear = vehicle_info.rear_overhang_m; const double lon_offset = std::max(lon_length + base_to_front, base_to_front); + const double lat_offset = width / 2.0 + lat_margin; + + { + debug.longitudinal_offset = lon_offset; + debug.lateral_offset = lat_offset; + } + const auto p1 = tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, lat_offset, 0.0); const auto p2 = tier4_autoware_utils::calcOffsetPose(base_link_pose, lon_offset, -lat_offset, 0.0); @@ -77,7 +85,8 @@ Polygon2d createExtendedPolygon( } Polygon2d createExtendedPolygon( - const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin) + const Pose & obj_pose, const Shape & shape, const double lon_length, const double lat_margin, + CollisionCheckDebug & debug) { const auto obj_polygon = tier4_autoware_utils::toPolygon2d(obj_pose, shape); if (obj_polygon.outer().empty()) { @@ -101,6 +110,12 @@ Polygon2d createExtendedPolygon( const double lon_offset = max_x + lon_length; const double left_lat_offset = max_y + lat_margin; const double right_lat_offset = min_y - lat_margin; + + { + debug.longitudinal_offset = lon_offset; + debug.lateral_offset = (left_lat_offset + right_lat_offset) / 2; + } + const auto p1 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, left_lat_offset, 0.0); const auto p2 = tier4_autoware_utils::calcOffsetPose(obj_pose, lon_offset, right_lat_offset, 0.0); const auto p3 = tier4_autoware_utils::calcOffsetPose(obj_pose, min_x, right_lat_offset, 0.0); @@ -183,9 +198,15 @@ bool isSafeInLaneletCollisionCheck( const auto & ego_pose = predicted_ego_poses.at(i).first; const auto & ego_polygon = predicted_ego_poses.at(i).second; + { + debug.lerped_path.push_back(ego_pose); + debug.expected_ego_pose = ego_pose; + debug.expected_obj_pose = *obj_pose; + debug.ego_polygon = ego_polygon; + debug.obj_polygon = obj_polygon; + } + // check overlap - debug.ego_polygon = ego_polygon; - debug.obj_polygon = obj_polygon; if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { debug.failed_reason = "overlap_polygon"; return false; @@ -211,17 +232,21 @@ bool isSafeInLaneletCollisionCheck( const auto & ego_vehicle_info = common_parameters.vehicle_info; const auto & lat_margin = common_parameters.lateral_distance_max_threshold; const auto & extended_ego_polygon = - is_object_front ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin) - : ego_polygon; + is_object_front + ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) + : ego_polygon; const auto & extended_obj_polygon = is_object_front ? obj_polygon - : createExtendedPolygon(*obj_pose, target_object.shape, lon_offset, lat_margin); - - debug.lerped_path.push_back(ego_pose); - debug.expected_ego_pose = ego_pose; - debug.expected_obj_pose = *obj_pose; - debug.is_front = is_object_front; + : createExtendedPolygon(*obj_pose, target_object.shape, lon_offset, lat_margin, debug); + + { + debug.rss_longitudinal = rss_dist; + debug.ego_to_obj_margin = min_lon_length; + debug.ego_polygon = extended_ego_polygon; + debug.obj_polygon = extended_obj_polygon; + debug.is_front = is_object_front; + } // check overlap with extended polygon if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { @@ -229,6 +254,7 @@ bool isSafeInLaneletCollisionCheck( return false; } } + return true; } @@ -259,16 +285,19 @@ bool isSafeInFreeSpaceCollisionCheck( const auto & ego_pose = interpolated_ego.at(i).first; const auto & ego_polygon = interpolated_ego.at(i).second; - debug.ego_polygon = ego_polygon; - debug.obj_polygon = obj_polygon; + { + debug.lerped_path.push_back(ego_pose); + debug.expected_ego_pose = ego_pose; + debug.expected_obj_pose = obj_pose; + debug.ego_polygon = ego_polygon; + debug.obj_polygon = obj_polygon; + } + if (boost::geometry::overlaps(ego_polygon, obj_polygon)) { debug.failed_reason = "overlap_polygon"; return false; } - debug.expected_ego_pose = ego_pose; - debug.expected_obj_pose = obj_pose; - // compute which one is at the front of the other const bool is_object_front = isTargetObjectFront(path, ego_pose, common_parameters.vehicle_info, obj_polygon); @@ -288,17 +317,21 @@ bool isSafeInFreeSpaceCollisionCheck( const auto & ego_vehicle_info = common_parameters.vehicle_info; const auto & lat_margin = common_parameters.lateral_distance_max_threshold; const auto & extended_ego_polygon = - is_object_front ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin) - : ego_polygon; + is_object_front + ? createExtendedPolygon(ego_pose, ego_vehicle_info, lon_offset, lat_margin, debug) + : ego_polygon; const auto & extended_obj_polygon = is_object_front ? obj_polygon - : createExtendedPolygon(obj_pose, target_object.shape, lon_offset, lat_margin); - - debug.lerped_path.push_back(ego_pose); - debug.expected_ego_pose = ego_pose; - debug.expected_obj_pose = obj_pose; - debug.is_front = is_object_front; + : createExtendedPolygon(obj_pose, target_object.shape, lon_offset, lat_margin, debug); + + { + debug.rss_longitudinal = rss_dist; + debug.ego_to_obj_margin = min_lon_length; + debug.ego_polygon = extended_ego_polygon; + debug.obj_polygon = extended_obj_polygon; + debug.is_front = is_object_front; + } if (boost::geometry::overlaps(extended_ego_polygon, extended_obj_polygon)) { debug.failed_reason = "overlap_extended_polygon"; diff --git a/planning/behavior_path_planner/src/utils/utils.cpp b/planning/behavior_path_planner/src/utils/utils.cpp index e1c2560b1c0d1..5b89b352d9574 100644 --- a/planning/behavior_path_planner/src/utils/utils.cpp +++ b/planning/behavior_path_planner/src/utils/utils.cpp @@ -1114,7 +1114,7 @@ bool isEgoOutOfRoute( bool isEgoWithinOriginalLane( const lanelet::ConstLanelets & current_lanes, const Pose & current_pose, - const BehaviorPathPlannerParameters & common_param) + const BehaviorPathPlannerParameters & common_param, const double outer_margin) { const auto lane_length = lanelet::utils::getLaneletLength2d(current_lanes); const auto lane_poly = lanelet::utils::getPolygonFromArcLength(current_lanes, 0, lane_length); @@ -1124,9 +1124,17 @@ bool isEgoWithinOriginalLane( const auto vehicle_poly = tier4_autoware_utils::toFootprint(current_pose, base_link2front, base_link2rear, vehicle_width); - // Check if the ego vehicle is entirely within the lane by checking if the vehicle's polygon - // is within the lane's polygon - return boost::geometry::within(vehicle_poly, lanelet::utils::to2D(lane_poly).basicPolygon()); + // Check if the ego vehicle is entirely within the lane with a given outer margin. + for (const auto & p : vehicle_poly.outer()) { + // When the point is in the polygon, the distance is 0. When it is out of the polygon, return a + // positive value. + const auto dist = boost::geometry::distance(p, lanelet::utils::to2D(lane_poly).basicPolygon()); + if (dist > std::max(outer_margin, 0.0)) { + return false; // out of polygon + } + } + + return true; // inside polygon } lanelet::ConstLanelets transformToLanelets(const DrivableLanes & drivable_lanes) diff --git a/planning/behavior_path_planner/test/test_safety_check.cpp b/planning/behavior_path_planner/test/test_safety_check.cpp index 07762ff96eb67..2174ef47a1c86 100644 --- a/planning/behavior_path_planner/test/test_safety_check.cpp +++ b/planning/behavior_path_planner/test/test_safety_check.cpp @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "behavior_path_planner/marker_util/debug_utilities.hpp" #include "behavior_path_planner/utils/safety_check.hpp" #include "tier4_autoware_utils/tier4_autoware_utils.hpp" @@ -40,6 +41,7 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) vehicle_info.max_longitudinal_offset_m = 4.0; vehicle_info.vehicle_width_m = 2.0; vehicle_info.rear_overhang_m = 1.0; + CollisionCheckDebug debug; { Pose ego_pose; @@ -49,7 +51,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; - const auto polygon = createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin); + const auto polygon = + createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -75,7 +78,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; - const auto polygon = createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin); + const auto polygon = + createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -102,7 +106,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedEgoPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; - const auto polygon = createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin); + const auto polygon = + createExtendedPolygon(ego_pose, vehicle_info, lon_length, lat_margin, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5)); @@ -149,7 +154,8 @@ TEST(BehaviorPathPlanningSafetyUtilsTest, createExtendedObjPolygon) const double lon_length = 10.0; const double lat_margin = 2.0; - const auto polygon = createExtendedPolygon(obj_pose, shape, lon_length, lat_margin); + CollisionCheckDebug debug; + const auto polygon = createExtendedPolygon(obj_pose, shape, lon_length, lat_margin, debug); EXPECT_EQ(polygon.outer().size(), static_cast(5));