From 13aed8c2691561153c4dff3761c10447b2f89319 Mon Sep 17 00:00:00 2001 From: Tomohito ANDO Date: Wed, 1 Jun 2022 11:16:31 +0900 Subject: [PATCH] feat(behavior_velocity): add run out module (#752) * fix(behavior_velocity): calculate detection area from the nearest point from ego (#730) * fix(behavior_velocity): calculate lateral distance from the beginning of the path Signed-off-by: Tomohito Ando * add argument of min_velocity Signed-off-by: Tomohito Ando * use veloicty from the nearest point from ego Signed-off-by: Tomohito Ando * pass struct by reference Signed-off-by: Tomohito Ando * fix to interpolate point in util Signed-off-by: Tomohito Ando * fix(longitudinal_controller_node, vehicle_cmd_gate): update stopped condition and behavior (#700) * fix(longitudinal_controller_node): parameterize stopped state entry condition Signed-off-by: satoshi-ota * fix(longitudinal_controller_node): simply set stopped velocity in STOPPED STATE Signed-off-by: satoshi-ota * fix(vehicle_cmd_gate): check time duration since the vehicle stopped Signed-off-by: satoshi-ota Signed-off-by: Tomohito Ando * docs(autoware_testing): fix link (#741) * docs(autoware_testing): fix link * fix typo Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Signed-off-by: Tomohito Ando * fix: trajectory visualizer (#745) Signed-off-by: tomoya.kimura Signed-off-by: Tomohito Ando * fix(tier4_autoware_utils): modify build error in rolling (#720) * fix(tier4_autoware_utils): modify build error in rolling Signed-off-by: wep21 * fix(lanelet2_extension): add target compile definition for geometry2 Signed-off-by: wep21 * fix(ekf_localizer): add target compile definition for geometry2 Signed-off-by: wep21 * fix(freespace_planning_algorithms): add target compile definition for geometry2 Signed-off-by: wep21 * fix(interpolation): add target compile definition for geometry2 Signed-off-by: wep21 * fix(freespace_planner): add target compile definition for geometry2 Signed-off-by: wep21 * fix(lane_departure_checker): add target compile definition for geometry2 Signed-off-by: wep21 * fix(map_based_prediction): add target compile definition for geometry2 Signed-off-by: wep21 * fix(ground_segmentation): add target compile definition for geometry2 Signed-off-by: wep21 * fix(motion_velocity_smoother): add target compile definition for geometry2 Signed-off-by: wep21 * fix(multi_object_tracker): add target compile definition for geometry2 Signed-off-by: wep21 * fix(trajectory_follower): add target compile definition for geometry2 Signed-off-by: wep21 * fix(control_performance_analysis): add target compile definition for geometry2 Signed-off-by: wep21 * fix(detected_object_validation): add target compile definition for geometry2 Signed-off-by: wep21 * fix(goal_distance_calculator): add target compile definition for geometry2 Signed-off-by: wep21 * fix(ndt_scan_matcher): add target compile definition for geometry2 Signed-off-by: wep21 * fix(route_handler): add target compile definition for geometry2 Signed-off-by: wep21 * fix(behavior_path_planner): add target compile definition for geometry2 Signed-off-by: wep21 * fix(mission_planner): add target compile definition for geometry2 Signed-off-by: wep21 * fix(obstacle_avoidance_planner): add target compile definition for geometry2 Signed-off-by: wep21 * fix(obstacle_stop_planner): add target compile definition for geometry2 Signed-off-by: wep21 * fix(obstacle_collision_checker): add target compile definition for geometry2 Signed-off-by: wep21 * fix(shape_estimation): add target compile definition for geometry2 Signed-off-by: wep21 * fix(behavior_velocity_planner): add target compile definition for geometry2 Signed-off-by: wep21 * fix(path_distance_calculator): add target compile definition for geometry2 Signed-off-by: wep21 * fix(detection_by_tracker): add target compile definition for geometry2 Signed-off-by: wep21 * fix(surround_obstacle_checker): add target compile definition for geometry2 Signed-off-by: wep21 * fix(probabilistic_occupancy_grid_map): add target compile definition for geometry2 Signed-off-by: wep21 * fix(tier4_debug_tools): add target compile definition for geometry2 Signed-off-by: wep21 * fix(tier4_vehicle_rviz_plugin): add target compile definition for geometry2 Signed-off-by: wep21 * fix(pure_pursuit): add target compile definition for geometry2 Signed-off-by: wep21 * fix(trajectory_follower_nodes): add target compile definition for geometry2 Signed-off-by: wep21 * fix(occupancy_grid_map_outlier_filter): add target compile definition for geometry2 Signed-off-by: wep21 * fix(traffic_light_map_based_detector): add target compile definition for geometry2 Signed-off-by: wep21 * fix(planning_error_monitor): add target compile definition for geometry2 Signed-off-by: wep21 * fix(planning_evaluator): add target compile definition for geometry2 Signed-off-by: wep21 * fix(lidar_centerpoint): add target compile definition for geometry2 Signed-off-by: wep21 Signed-off-by: Tomohito Ando * fix(behavior_velocity): handle the case when finding index failed (#746) Signed-off-by: Tomohito Ando * feat: add scene module of dynamic obstacle stop Signed-off-by: Tomohito Ando * fix warnings Signed-off-by: Tomohito Ando * add temporary debug value Signed-off-by: Tomohito Ando * add feature to go after stopping Signed-off-by: Tomohito Ando * fix parameter namespace Signed-off-by: Tomohito Ando * use planner util Signed-off-by: Tomohito Ando * fix calculation when multiple obstacles are detected in one step polygon Signed-off-by: Tomohito Ando * remove unnecessary debug Signed-off-by: Tomohito Ando * add option to apply limit jerk Signed-off-by: Tomohito Ando * Modify parameter name Signed-off-by: Tomohito Ando * Add param file Signed-off-by: Tomohito Ando * remove unnecessary comments Signed-off-by: Tomohito Ando * add feature to exclude obstacles outside of partition Signed-off-by: Tomohito Ando * modify search distance for partitions Signed-off-by: Tomohito Ando * apply voxel grid filter to input points Signed-off-by: Tomohito Ando * set smoother param by passing node instance Signed-off-by: Tomohito Ando * add parameter for velocity limit (temporary) Signed-off-by: Tomohito Ando * add dynamic reconfigure Signed-off-by: Tomohito Ando * add debug value Signed-off-by: Tomohito Ando * avoid acceleration when stopping for obstacles Signed-off-by: Tomohito Ando * fix lateral distance to publish distance from vehicle side Signed-off-by: Tomohito Ando * modify the location to publish longitudinal distance Signed-off-by: Tomohito Ando * fix calculation of stop index Signed-off-by: Tomohito Ando * use current velocity for stop dicision Signed-off-by: Tomohito Ando * add dynamic parameter for slow down limit Signed-off-by: Tomohito Ando * add debug value to display passing dist Signed-off-by: Tomohito Ando * modify stopping velocity Signed-off-by: Tomohito Ando * update param Signed-off-by: Tomohito Ando * use smoother in planner data Signed-off-by: Tomohito Ando * use path with lane id instead of trajectory Signed-off-by: Tomohito Ando * remove unnecessary data check Signed-off-by: Tomohito Ando * extend path to consider obstacles after the end of the path Signed-off-by: Tomohito Ando * rename public member variables Signed-off-by: Tomohito Ando * remove unused paramter Signed-off-by: Tomohito Ando * create detection area using util Signed-off-by: Tomohito Ando * fix visualization of stop distance marker Signed-off-by: Tomohito Ando * make option for detection method easier to understand Signed-off-by: Tomohito Ando * remove parameter about whether to enable this feature Signed-off-by: Tomohito Ando * calculate and publish debug data in function Signed-off-by: Tomohito Ando * use compare map filtered points Signed-off-by: Tomohito Ando * add comment Signed-off-by: Tomohito Ando * fix visualization of detection area when calculation of stop distance failed Signed-off-by: Tomohito Ando * add option whether to specify the jerk Signed-off-by: Tomohito Ando * fix format Signed-off-by: Tomohito Ando * change parameter name Signed-off-by: Tomohito Ando * remove dynamic reconfigure Signed-off-by: Tomohito Ando * delete unused file Signed-off-by: Tomohito Ando * remove unnecessary comments Signed-off-by: Tomohito Ando * remove unnecessary includes Signed-off-by: Tomohito Ando * add launcher for compare map Signed-off-by: Tomohito Ando * add launch and config for dynamic obstacle stop planner Signed-off-by: Tomohito Ando * fix finding package name Signed-off-by: Tomohito Ando * handle the change of util Signed-off-by: Tomohito Ando * relay points for simulation Signed-off-by: Tomohito Ando * update parameter Signed-off-by: Tomohito Ando * fix position and color of stop line marker Signed-off-by: Tomohito Ando * pre-commit fixes Signed-off-by: Tomohito Ando * remove unnecessary comments Signed-off-by: Tomohito Ando * fix Copyright Co-authored-by: Yukihiro Saito Signed-off-by: Tomohito Ando * fix Copyright Signed-off-by: Tomohito Ando * fix typo Signed-off-by: Tomohito Ando * add documents for dynamic obstacle stop module Signed-off-by: Tomohito Ando * ci(pre-commit): autofix Signed-off-by: Tomohito Ando * update documents Signed-off-by: Tomohito Ando * docs: begin a sentence with a capital letter Signed-off-by: Tomohito Ando * docs: replace predicted with smoothed for path velocity Signed-off-by: Tomohito Ando * create interface class to switch method Signed-off-by: Tomohito Ando * run compare map filter only when points method is used Signed-off-by: Tomohito Ando * delete unused functions Signed-off-by: Tomohito Ando * rename functions for inserting velocity Signed-off-by: Tomohito Ando * rename parameter of path_size to max_prediction_time Signed-off-by: Tomohito Ando * fix(behavior_velocity_planner): dynamic obstacle stop planner docs Signed-off-by: Makoto Kurihara Signed-off-by: Tomohito Ando * fix(behavior_velocity_planner): add ego vehicle description Signed-off-by: Makoto Kurihara Signed-off-by: Tomohito Ando * fix(behavior_velocity_planner): change space to hyphen Signed-off-by: Makoto Kurihara Signed-off-by: Tomohito Ando * change smoothed to expected target velocity Signed-off-by: Tomohito Ando * Start a sentence in figure with a capital letter Signed-off-by: Tomohito Ando * fix typo Signed-off-by: Tomohito Ando * use voxel distance based compare map Signed-off-by: Tomohito Ando * select detection method from param file Signed-off-by: Tomohito Ando * do not launch as default for now Signed-off-by: Tomohito Ando * rename dynamic_obstacle_stop to run_out Signed-off-by: Tomohito Ando * remove unnecessary change Signed-off-by: Tomohito Ando * remove unnecessary changes Signed-off-by: Tomohito Ando * remove unnecessary changes Signed-off-by: Tomohito Ando * fix typo Signed-off-by: Tomohito Ando * change default to false Signed-off-by: Tomohito Ando * update cmake to build run_out module Signed-off-by: Tomohito Ando * add launch_run_out parameter Signed-off-by: Tomohito Ando * Add note for compare map filtered points Signed-off-by: Tomohito Ando * handle the change for virtual wall marker Signed-off-by: Tomohito Ando * rename the parameters for smoother Signed-off-by: Tomohito Ando * fix build error in humble Signed-off-by: Tomohito Ando * fix build error in humble Signed-off-by: Tomohito Ando * launch compare map only when run out module is enabled Signed-off-by: Tomohito Ando * update a document Signed-off-by: Tomohito Ando * add calculation time for modify path Signed-off-by: Tomohito Ando * update a document Signed-off-by: Tomohito Ando Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com> Co-authored-by: Esteve Fernandez Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Tomoya Kimura Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Yukihiro Saito Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com> Co-authored-by: Makoto Kurihara --- .../behavior_velocity_planner.param.yaml | 1 + .../run_out.param.yaml | 45 + .../launch/planning.launch.xml | 6 + .../scenario_planning/lane_driving.launch.xml | 6 + .../behavior_planning.launch.py | 94 ++ .../behavior_planning/compare_map.launch.py | 89 ++ .../scenario_planning.launch.xml | 6 + .../behavior_velocity_planner/CMakeLists.txt | 5 +- planning/behavior_velocity_planner/README.md | 19 +- .../behavior_velocity_planner.param.yaml | 1 + .../config/run_out.param.yaml | 45 + .../calculate_expected_target_velocity.svg | 4 + .../run_out/collision_detection_for_shape.svg | 4 + .../docs/run_out/collision_points.svg | 4 + .../docs/run_out/create_dynamic_obstacle.svg | 4 + .../run_out/create_polygon_on_path_point.svg | 4 + .../exclude_obstacles_by_partition.svg | 4 + .../docs/run_out/insert_velocity.svg | 4 + .../docs/run_out/run_out_overview.svg | 4 + .../include/scene_module/run_out/debug.hpp | 145 +++ .../scene_module/run_out/dynamic_obstacle.hpp | 157 ++++ .../include/scene_module/run_out/manager.hpp | 46 + .../scene_module/run_out/path_utils.hpp | 95 ++ .../include/scene_module/run_out/scene.hpp | 158 ++++ .../include/scene_module/run_out/utils.hpp | 227 +++++ .../run-out-design.md | 197 ++++ .../behavior_velocity_planner/src/node.cpp | 4 + .../src/scene_module/run_out/debug.cpp | 322 +++++++ .../scene_module/run_out/dynamic_obstacle.cpp | 218 +++++ .../src/scene_module/run_out/manager.cpp | 154 +++ .../src/scene_module/run_out/scene.cpp | 879 ++++++++++++++++++ .../src/scene_module/run_out/utils.cpp | 510 ++++++++++ 32 files changed, 3452 insertions(+), 9 deletions(-) create mode 100644 launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml create mode 100644 launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py create mode 100644 planning/behavior_velocity_planner/config/run_out.param.yaml create mode 100644 planning/behavior_velocity_planner/docs/run_out/calculate_expected_target_velocity.svg create mode 100644 planning/behavior_velocity_planner/docs/run_out/collision_detection_for_shape.svg create mode 100644 planning/behavior_velocity_planner/docs/run_out/collision_points.svg create mode 100644 planning/behavior_velocity_planner/docs/run_out/create_dynamic_obstacle.svg create mode 100644 planning/behavior_velocity_planner/docs/run_out/create_polygon_on_path_point.svg create mode 100644 planning/behavior_velocity_planner/docs/run_out/exclude_obstacles_by_partition.svg create mode 100644 planning/behavior_velocity_planner/docs/run_out/insert_velocity.svg create mode 100644 planning/behavior_velocity_planner/docs/run_out/run_out_overview.svg create mode 100644 planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp create mode 100644 planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp create mode 100644 planning/behavior_velocity_planner/include/scene_module/run_out/manager.hpp create mode 100644 planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp create mode 100644 planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp create mode 100644 planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp create mode 100644 planning/behavior_velocity_planner/run-out-design.md create mode 100644 planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp create mode 100644 planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp create mode 100644 planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp create mode 100644 planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp create mode 100644 planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml index d47649bbe7831..1332e422cd3e6 100644 --- a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/behavior_velocity_planner.param.yaml @@ -9,6 +9,7 @@ launch_virtual_traffic_light: true launch_occlusion_spot: true launch_no_stopping_area: true + launch_run_out: false forward_path_length: 1000.0 backward_path_length: 5.0 max_accel: -2.8 diff --git a/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml new file mode 100644 index 0000000000000..8818ed01998da --- /dev/null +++ b/launch/tier4_planning_launch/config/scenario_planning/lane_driving/behavior_planning/behavior_velocity_planner/run_out.param.yaml @@ -0,0 +1,45 @@ +/**: + ros__parameters: + run_out: + detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points + use_partition_lanelet: true # [-] whether to use partition lanelet map data + specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates + stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin + passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin + deceleration_jerk: -0.3 # [m/s^3] ego decelerates with this jerk when stopping for obstacles + obstacle_velocity_kph: 5.0 # [km/h] assumption for obstacle velocity + detection_distance: 45.0 # [m] ahead distance from ego to detect the obstacles + detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time + min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision + + # rectangle detection area for Points method + # this will be replaced with more appropriate detection area + detection_area_size: + dist_ahead: 50.0 # [m] ahead distance from ego position + dist_behind: 5.0 # [m] behind distance from ego position + dist_right: 7.0 # [m] right distance from ego position + dist_left: 7.0 # [m] left distance from ego position + + # parameter to create abstracted dynamic obstacles + dynamic_obstacle: + min_vel_kmph: 0.0 # [km/h] minimum velocity for dynamic obstacles + max_vel_kmph: 5.0 # [km/h] maximum velocity for dynamic obstacles + diameter: 0.1 # [m] diameter of obstacles. used for creating dynamic obstacles from points + height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points + max_prediction_time: 10.0 # [sec] create predicted path until this time + time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path + + # approach if ego has stopped in the front of the obstacle for a certain amount of time + approaching: + enable: false + margin: 0.0 # [m] distance on how close ego approaches the obstacle + limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping + stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping + stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state + dist_thresh: 0.5 # [m] end the approaching state if distance to the obstacle is longer than stop_margin + dist_thresh + + # parameter to avoid sudden stopping + slow_down_limit: + enable: true + max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. + max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. diff --git a/launch/tier4_planning_launch/launch/planning.launch.xml b/launch/tier4_planning_launch/launch/planning.launch.xml index a596f0b408768..94dd7d853776d 100644 --- a/launch/tier4_planning_launch/launch/planning.launch.xml +++ b/launch/tier4_planning_launch/launch/planning.launch.xml @@ -3,6 +3,10 @@ + + + + @@ -16,6 +20,8 @@ + + diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml index 09c282b16a4ff..457abe3a62f4b 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving.launch.xml @@ -3,6 +3,10 @@ + + + + @@ -12,6 +16,8 @@ + + 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 699602bf3e4ad..8b917ccbc2f3e 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 @@ -19,11 +19,14 @@ from launch.actions import DeclareLaunchArgument from launch.actions import ExecuteProcess from launch.actions import GroupAction +from launch.actions import IncludeLaunchDescription from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition +from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration +from launch.substitutions import PythonExpression from launch_ros.actions import ComposableNodeContainer from launch_ros.descriptions import ComposableNode from launch_ros.substitutions import FindPackageShare @@ -188,6 +191,39 @@ def launch_setup(context, *args, **kwargs): extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) + # smoother param + common_param_path = os.path.join( + get_package_share_directory("tier4_planning_launch"), + "config", + "scenario_planning", + "common", + "common.param.yaml", + ) + with open(common_param_path, "r") as f: + common_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + motion_velocity_smoother_param_path = os.path.join( + get_package_share_directory("tier4_planning_launch"), + "config", + "scenario_planning", + "common", + "motion_velocity_smoother", + "motion_velocity_smoother.param.yaml", + ) + with open(motion_velocity_smoother_param_path, "r") as f: + motion_velocity_smoother_param = yaml.safe_load(f)["/**"]["ros__parameters"] + + smoother_type_param_path = os.path.join( + get_package_share_directory("tier4_planning_launch"), + "config", + "scenario_planning", + "common", + "motion_velocity_smoother", + "Analytical.param.yaml", + ) + with open(smoother_type_param_path, "r") as f: + smoother_type_param = yaml.safe_load(f)["/**"]["ros__parameters"] + # behavior velocity planner blind_spot_param_path = os.path.join( get_package_share_directory("tier4_planning_launch"), @@ -297,6 +333,18 @@ def launch_setup(context, *args, **kwargs): with open(no_stopping_area_param_path, "r") as f: no_stopping_area_param = yaml.safe_load(f)["/**"]["ros__parameters"] + run_out_param_path = os.path.join( + get_package_share_directory("tier4_planning_launch"), + "config", + "scenario_planning", + "lane_driving", + "behavior_planning", + "behavior_velocity_planner", + "run_out.param.yaml", + ) + with open(run_out_param_path, "r") as f: + run_out_param = yaml.safe_load(f)["/**"]["ros__parameters"] + behavior_velocity_planner_param_path = os.path.join( get_package_share_directory("tier4_planning_launch"), "config", @@ -323,6 +371,10 @@ def launch_setup(context, *args, **kwargs): "~/input/no_ground_pointcloud", "/perception/obstacle_segmentation/pointcloud", ), + ( + "~/input/compare_map_filtered_pointcloud", + "compare_map_filtered/pointcloud", + ), ( "~/input/traffic_signals", "/perception/traffic_light_recognition/traffic_signals", @@ -331,6 +383,10 @@ def launch_setup(context, *args, **kwargs): "~/input/external_traffic_signals", "/external/traffic_light_recognition/traffic_signals", ), + ( + "~/input/external_velocity_limit_mps", + "/planning/scenario_planning/max_velocity_default", + ), ("~/input/virtual_traffic_light_states", "/awapi/tmp/virtual_traffic_light_states"), ( "~/input/occupancy_grid", @@ -356,6 +412,10 @@ def launch_setup(context, *args, **kwargs): occlusion_spot_param, no_stopping_area_param, vehicle_info_param, + run_out_param, + common_param, + motion_velocity_smoother_param, + smoother_type_param, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ) @@ -372,9 +432,39 @@ def launch_setup(context, *args, **kwargs): output="screen", ) + # load compare map for dynamic obstacle stop module + load_compare_map = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [ + FindPackageShare("tier4_planning_launch"), + "/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py", + ] + ), + launch_arguments={ + "use_pointcloud_container": LaunchConfiguration("use_pointcloud_container"), + "container_name": LaunchConfiguration("container_name"), + "use_multithread": "true", + }.items(), + # launch compare map only when run_out module is enabled and detection method is Points + condition=IfCondition( + PythonExpression( + [ + LaunchConfiguration( + "launch_run_out", default=behavior_velocity_planner_param["launch_run_out"] + ), + " and ", + "'", + run_out_param["run_out"]["detection_method"], + "' == 'Points'", + ] + ) + ), + ) + group = GroupAction( [ container, + load_compare_map, ExecuteProcess( cmd=[ "ros2", @@ -420,6 +510,10 @@ def add_launch_arg(name: str, default_value=None, description=None): add_launch_arg("use_intra_process", "false", "use ROS2 component container communication") add_launch_arg("use_multithread", "false", "use multithread") + # for compare map + add_launch_arg("use_pointcloud_container", "true") + add_launch_arg("container_name", "pointcloud_container") + set_container_executable = SetLaunchConfiguration( "container_executable", "component_container", diff --git a/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py new file mode 100644 index 0000000000000..fe3e347fd2f7d --- /dev/null +++ b/launch/tier4_planning_launch/launch/scenario_planning/lane_driving/behavior_planning/compare_map.launch.py @@ -0,0 +1,89 @@ +# Copyright 2022 TIER IV, Inc. All rights reserved. +# +# 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. + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.actions import SetLaunchConfiguration +from launch.conditions import IfCondition +from launch.conditions import UnlessCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import ComposableNodeContainer +from launch_ros.actions import LoadComposableNodes +from launch_ros.descriptions import ComposableNode + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + + composable_nodes = [ + ComposableNode( + package="compare_map_segmentation", + plugin="compare_map_segmentation::VoxelDistanceBasedCompareMapFilterComponent", + name="voxel_distance_based_compare_map_filter_node", + remappings=[ + ("input", "/perception/obstacle_segmentation/pointcloud"), + ("map", "/map/pointcloud_map"), + ("output", "compare_map_filtered/pointcloud"), + ], + parameters=[ + { + "distance_threshold": 0.7, + } + ], + extra_arguments=[ + {"use_intra_process_comms": False} # this node has QoS of transient local + ], + ), + ] + + compare_map_container = ComposableNodeContainer( + name=LaunchConfiguration("container_name"), + namespace="", + package="rclcpp_components", + executable=LaunchConfiguration("container_executable"), + composable_node_descriptions=composable_nodes, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), + output="screen", + ) + + load_composable_nodes = LoadComposableNodes( + composable_node_descriptions=composable_nodes, + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + return LaunchDescription( + [ + add_launch_arg("use_multithread", "true"), + add_launch_arg("use_pointcloud_container", "true"), + add_launch_arg("container_name", "compare_map_container"), + set_container_executable, + set_container_mt_executable, + compare_map_container, + load_composable_nodes, + ] + ) diff --git a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml index 95126f9789cb3..35f178ab03375 100644 --- a/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml +++ b/launch/tier4_planning_launch/launch/scenario_planning/scenario_planning.launch.xml @@ -3,6 +3,10 @@ + + + + @@ -41,6 +45,8 @@ + + diff --git a/planning/behavior_velocity_planner/CMakeLists.txt b/planning/behavior_velocity_planner/CMakeLists.txt index 827d1caf1e29a..3766c5f631d06 100644 --- a/planning/behavior_velocity_planner/CMakeLists.txt +++ b/planning/behavior_velocity_planner/CMakeLists.txt @@ -8,7 +8,7 @@ autoware_package() find_package(Boost REQUIRED) find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) -find_package(PCL REQUIRED COMPONENTS common) +find_package(PCL REQUIRED COMPONENTS common filters) find_package(OpenCV REQUIRED) set(scene_modules @@ -21,6 +21,7 @@ set(scene_modules no_stopping_area virtual_traffic_light occlusion_spot + run_out ) foreach(scene_module IN LISTS scene_modules) @@ -50,6 +51,8 @@ ament_target_dependencies(behavior_velocity_planner PCL ) +target_link_libraries(behavior_velocity_planner ${PCL_LIBRARIES}) + rclcpp_components_register_node(behavior_velocity_planner PLUGIN "behavior_velocity_planner::BehaviorVelocityPlannerNode" EXECUTABLE behavior_velocity_planner_node diff --git a/planning/behavior_velocity_planner/README.md b/planning/behavior_velocity_planner/README.md index 20ad38140d330..f0801787a8709 100644 --- a/planning/behavior_velocity_planner/README.md +++ b/planning/behavior_velocity_planner/README.md @@ -17,6 +17,7 @@ It consists of several modules. Please refer to the links listed below for detai - [Traffic Light](traffic-light-design.md) - [Occlusion Spot](occlusion-spot-design.md) - [No Stopping Area](no-stopping-area-design.md) +- [Run Out](run-out-design.md) When each module plans velocity, it considers based on `base_link`(center of rear-wheel axis) pose. So for example, in order to stop at a stop line with the vehicles' front on the stop line, it calculates `base_link` position from the distance between `base_link` to front and modifies path velocity from the `base_link` position. @@ -25,14 +26,15 @@ So for example, in order to stop at a stop line with the vehicles' front on the ## Input topics -| Name | Type | Description | -| ----------------------------- | ------------------------------------------------------ | -------------------- | -| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id | -| `~input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | -| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | -| `~input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | -| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | -| `~input/traffic_signals` | autoware_auto_perception_msgs::msg::TrafficSignalArray | traffic light states | +| Name | Type | Description | +| ----------------------------------------- | ------------------------------------------------------ | ------------------------------------------------------------------------------------------------------------------------------- | +| `~input/path_with_lane_id` | autoware_auto_planning_msgs::msg::PathWithLaneId | path with lane_id | +| `~input/vector_map` | autoware_auto_mapping_msgs::msg::HADMapBin | vector map | +| `~input/vehicle_odometry` | nav_msgs::msg::Odometry | vehicle velocity | +| `~input/dynamic_objects` | autoware_auto_perception_msgs::msg::PredictedObjects | dynamic objects | +| `~input/no_ground_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud | +| `~/input/compare_map_filtered_pointcloud` | sensor_msgs::msg::PointCloud2 | obstacle pointcloud filtered by compare map. Note that this is used only when the detection method of run out module is Points. | +| `~input/traffic_signals` | autoware_auto_perception_msgs::msg::TrafficSignalArray | traffic light states | ## Output topics @@ -52,6 +54,7 @@ So for example, in order to stop at a stop line with the vehicles' front on the | `launch_traffic_light` | bool | whether to launch traffic light module | | `launch_stop_line` | bool | whether to launch stop_line module | | `launch_occlusion_spot` | bool | whether to launch occlusion_spot module | +| `launch_run_out` | bool | whether to launch run_out module | | `forward_path_length` | double | forward path length | | `backward_path_length` | double | backward path length | | `max_accel` | double | (to be a global parameter) max acceleration of the vehicle | diff --git a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml index d47649bbe7831..1332e422cd3e6 100644 --- a/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml +++ b/planning/behavior_velocity_planner/config/behavior_velocity_planner.param.yaml @@ -9,6 +9,7 @@ launch_virtual_traffic_light: true launch_occlusion_spot: true launch_no_stopping_area: true + launch_run_out: false forward_path_length: 1000.0 backward_path_length: 5.0 max_accel: -2.8 diff --git a/planning/behavior_velocity_planner/config/run_out.param.yaml b/planning/behavior_velocity_planner/config/run_out.param.yaml new file mode 100644 index 0000000000000..8818ed01998da --- /dev/null +++ b/planning/behavior_velocity_planner/config/run_out.param.yaml @@ -0,0 +1,45 @@ +/**: + ros__parameters: + run_out: + detection_method: "Object" # [-] candidate: Object, ObjectWithoutPath, Points + use_partition_lanelet: true # [-] whether to use partition lanelet map data + specify_decel_jerk: false # [-] whether to specify jerk when ego decelerates + stop_margin: 2.5 # [m] the vehicle decelerates to be able to stop with this margin + passing_margin: 1.1 # [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin + deceleration_jerk: -0.3 # [m/s^3] ego decelerates with this jerk when stopping for obstacles + obstacle_velocity_kph: 5.0 # [km/h] assumption for obstacle velocity + detection_distance: 45.0 # [m] ahead distance from ego to detect the obstacles + detection_span: 1.0 # [m] calculate collision with this span to reduce calculation time + min_vel_ego_kmph: 3.6 # [km/h] min velocity to calculate time to collision + + # rectangle detection area for Points method + # this will be replaced with more appropriate detection area + detection_area_size: + dist_ahead: 50.0 # [m] ahead distance from ego position + dist_behind: 5.0 # [m] behind distance from ego position + dist_right: 7.0 # [m] right distance from ego position + dist_left: 7.0 # [m] left distance from ego position + + # parameter to create abstracted dynamic obstacles + dynamic_obstacle: + min_vel_kmph: 0.0 # [km/h] minimum velocity for dynamic obstacles + max_vel_kmph: 5.0 # [km/h] maximum velocity for dynamic obstacles + diameter: 0.1 # [m] diameter of obstacles. used for creating dynamic obstacles from points + height: 2.0 # [m] height of obstacles. used for creating dynamic obstacles from points + max_prediction_time: 10.0 # [sec] create predicted path until this time + time_step: 0.5 # [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path + + # approach if ego has stopped in the front of the obstacle for a certain amount of time + approaching: + enable: false + margin: 0.0 # [m] distance on how close ego approaches the obstacle + limit_vel_kmph: 3.0 # [km/h] limit velocity for approaching after stopping + stop_thresh: 0.01 # [m/s] threshold to decide if ego is stopping + stop_time_thresh: 3.0 # [sec] threshold for stopping time to transit to approaching state + dist_thresh: 0.5 # [m] end the approaching state if distance to the obstacle is longer than stop_margin + dist_thresh + + # parameter to avoid sudden stopping + slow_down_limit: + enable: true + max_jerk: -0.7 # [m/s^3] minimum jerk deceleration for safe brake. + max_acc : -2.0 # [m/s^2] minimum accel deceleration for safe brake. diff --git a/planning/behavior_velocity_planner/docs/run_out/calculate_expected_target_velocity.svg b/planning/behavior_velocity_planner/docs/run_out/calculate_expected_target_velocity.svg new file mode 100644 index 0000000000000..6c5f2a26e9952 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/run_out/calculate_expected_target_velocity.svg @@ -0,0 +1,4 @@ + + + +
v
v
Expected target velocity
Expected target velocity
Reference velocity
Reference velocity
Current velocity
Current velocity
s
s
d
d
Expected target velocity calculated by smoother
Expected target velocity calculated by smootherText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/run_out/collision_detection_for_shape.svg b/planning/behavior_velocity_planner/docs/run_out/collision_detection_for_shape.svg new file mode 100644 index 0000000000000..ea7fb711310c0 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/run_out/collision_detection_for_shape.svg @@ -0,0 +1,4 @@ + + + +
Predicted position between min velocity and max velocity
(create a rectangle that includes two bounding boxes)
Predicted position between min velocity and max veloci...
Shape: Bounding Box
Shape: Bounding Box
Predicted position between min velocity and max velocity
(create rectangle that includes two circles)
Predicted position between min velocity and max veloci...
Predicted position at minimum velocity
Predicted position at minimum velocity
Predicted position at maximum velocity
Predicted position at maximum velocity
Shape: Cylinder
Shape: Cylinder
Predicted position between min velocity and max velocity
(create convex hull for two polygons)
Predicted position between min velocity and max velocit...
Predicted position at minimum velocity
Predicted position at minimum velocity
Predicted position at maximum velocity
Predicted position at maximum velocity
Shape: Polygon
Shape: Polygon
Predicted position at minimum velocity
Predicted position at minimum velocity
Predicted position at maximum velocity
Predicted position at maximum velocityText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/run_out/collision_points.svg b/planning/behavior_velocity_planner/docs/run_out/collision_points.svg new file mode 100644 index 0000000000000..0c254e224e109 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/run_out/collision_points.svg @@ -0,0 +1,4 @@ + + + +
Collision points
Collision points
Selected collision point
Selected collision pointText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/run_out/create_dynamic_obstacle.svg b/planning/behavior_velocity_planner/docs/run_out/create_dynamic_obstacle.svg new file mode 100644 index 0000000000000..0309680bddb91 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/run_out/create_dynamic_obstacle.svg @@ -0,0 +1,4 @@ + + + +

- classifications
- pose
- predicted_paths
- shape
- min_velocity_mps
- max_velocity_mps
- classifications...

- classifications
- pose
- predicted_paths
- shape
- min_velocity_mps
- max_velocity_mps
- classifications...

- classifications
- pose
  - position
  - orientation
- predicted_paths
- shape
- min_velocity_mps
- max_velocity_mps
- classifications...
Use predicted path 
Use predicted path 
Predicted path is created to run out straight to the path of ego
Predicted path is created to...
Predicted path is created to run out straight to the path of ego
Predicted path is created to...
Object
Object
ObjectWithoutPath
ObjectWithoutPath
Points
Points
Use value of input data
Use value of input data
Assign value in this module
Assign value in this module
Input: Predicted object
Input: Predicted object
Input: Points
Input: PointsText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/run_out/create_polygon_on_path_point.svg b/planning/behavior_velocity_planner/docs/run_out/create_polygon_on_path_point.svg new file mode 100644 index 0000000000000..aa1f6c1ed8e52 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/run_out/create_polygon_on_path_point.svg @@ -0,0 +1,4 @@ + + + +
Polygon is created on path points
Polygon is created on path points
t
t
Travel time
Travel time
s
s
d
d
Travel time calculated by using predicted path
Travel time calculated by using...
t = \sum \frac{...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/run_out/exclude_obstacles_by_partition.svg b/planning/behavior_velocity_planner/docs/run_out/exclude_obstacles_by_partition.svg new file mode 100644 index 0000000000000..4d2b363959bc2 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/run_out/exclude_obstacles_by_partition.svg @@ -0,0 +1,4 @@ + + + +
Partition defined in lanelet
(guardrail, fence, wall)
Partition defined in lanelet...
Obstacles in this area are not detected
Obstacles in this area are not detect...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/run_out/insert_velocity.svg b/planning/behavior_velocity_planner/docs/run_out/insert_velocity.svg new file mode 100644 index 0000000000000..647da6f21f9ef --- /dev/null +++ b/planning/behavior_velocity_planner/docs/run_out/insert_velocity.svg @@ -0,0 +1,4 @@ + + + +
Stop
margin
Stop...
Base link
to front
Base link...

Collision point

Collision point

v

v

x

x

Output velocity

Output velocity

Reference velocity

Reference velocity

Stop point

Stop pointText is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/docs/run_out/run_out_overview.svg b/planning/behavior_velocity_planner/docs/run_out/run_out_overview.svg new file mode 100644 index 0000000000000..18711df5fea23 --- /dev/null +++ b/planning/behavior_velocity_planner/docs/run_out/run_out_overview.svg @@ -0,0 +1,4 @@ + + + +
Predicted vehicle polygon
Predicted vehicle polygon
Collision point
Collision point
Stop point
Stop point
Stop margin + base link to front
Stop margin + base link to fro...
Predicted path of the obstacle
Predicted path of the obstac...
Ignore obstacles outside the partition
Ignore obstacles outside the partit...Text is not SVG - cannot display \ No newline at end of file diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp new file mode 100644 index 0000000000000..c96a3d687e1ef --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/debug.hpp @@ -0,0 +1,145 @@ +// 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 SCENE_MODULE__RUN_OUT__DEBUG_HPP_ +#define SCENE_MODULE__RUN_OUT__DEBUG_HPP_ + +#include "scene_module/run_out/dynamic_obstacle.hpp" + +#include +#include +namespace behavior_velocity_planner +{ +using autoware_auto_planning_msgs::msg::Trajectory; +using tier4_debug_msgs::msg::Float32MultiArrayStamped; +using tier4_debug_msgs::msg::Int32Stamped; + +enum class PointType : int8_t { + Blue = 0, + Red, + Yellow, +}; + +class DebugValues +{ +public: + enum class TYPE { + CALCULATION_TIME = 0, + CALCULATION_TIME_COLLISION_CHECK = 1, + LATERAL_DIST = 2, + LONGITUDINAL_DIST_OBSTACLE = 3, + LONGITUDINAL_DIST_COLLISION = 4, + COLLISION_POS_FROM_EGO_FRONT = 5, + STOP_DISTANCE = 6, + NUM_OBSTACLES = 7, + LATERAL_PASS_DIST = 8, + SIZE, // this is the number of enum elements + }; + + /** + * @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 getValuesIdx(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)> getValues() const { return values_; } + /** + * @brief set the given type to the given value + * @param [in] type TYPE of the value + * @param [in] value value to set + */ + void setValues(const TYPE type, const float val) { values_.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 setValues(const int type, const float val) { values_.at(type) = val; } + +private: + static constexpr int num_debug_values_ = static_cast(TYPE::SIZE); + std::array(TYPE::SIZE)> values_; +}; + +class RunOutDebug +{ +public: + enum class AccelReason { + STOP = 0, + NO_OBSTACLE = 1, + PASS = 2, + LOW_JERK = 3, + }; + + struct TextWithPosition + { + std::string text; + geometry_msgs::msg::Point position; + }; + + explicit RunOutDebug(rclcpp::Node & node); + ~RunOutDebug() {} + + void setDebugValues(const DebugValues::TYPE type, const double val) + { + debug_values_.setValues(type, val); + } + + bool pushObstaclePoint(const geometry_msgs::msg::Point & obstacle_point, const PointType & type); + bool pushObstaclePoint(const pcl::PointXYZ & obstacle_point, const PointType & type); + void pushDebugPoints(const pcl::PointXYZ & debug_point); + void pushDebugPoints(const geometry_msgs::msg::Point & debug_point); + void pushDebugPoints(const std::vector & debug_points); + void pushDebugPoints(const geometry_msgs::msg::Point & debug_point, const PointType point_type); + void pushStopPose(const geometry_msgs::msg::Pose & pose); + void pushDebugLines(const std::vector & debug_line); + void pushDebugPolygons(const std::vector & debug_polygon); + void pushDetectionAreaPolygons(const Polygon2d & debug_polygon); + void pushDebugTexts(const TextWithPosition & debug_text); + void pushDebugTexts( + const std::string text, const geometry_msgs::msg::Pose pose, const float lateral_offset); + void pushDebugTexts(const std::string text, const geometry_msgs::msg::Point position); + void setAccelReason(const AccelReason & accel_reason); + void publishDebugValue(); + void publishDebugTrajectory(const Trajectory & trajectory); + visualization_msgs::msg::MarkerArray createVisualizationMarkerArray(); + visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray(); + +private: + visualization_msgs::msg::MarkerArray createVisualizationMarkerArrayFromDebugData( + const builtin_interfaces::msg::Time & current_time); + void clearDebugMarker(); + + rclcpp::Node & node_; + rclcpp::Publisher::SharedPtr pub_debug_values_; + rclcpp::Publisher::SharedPtr pub_accel_reason_; + rclcpp::Publisher::SharedPtr pub_debug_trajectory_; + std::vector debug_points_; + std::vector debug_points_red_; + std::vector debug_points_yellow_; + std::vector stop_pose_; + std::vector> debug_lines_; + std::vector> debug_polygons_; + std::vector> detection_area_polygons_; + std::vector debug_texts_; + DebugValues debug_values_; + AccelReason accel_reason_; +}; + +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__RUN_OUT__DEBUG_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp new file mode 100644 index 0000000000000..e1360eb36e53b --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/dynamic_obstacle.hpp @@ -0,0 +1,157 @@ +// 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 SCENE_MODULE__RUN_OUT__DYNAMIC_OBSTACLE_HPP_ +#define SCENE_MODULE__RUN_OUT__DYNAMIC_OBSTACLE_HPP_ + +#include "behavior_velocity_planner/planner_data.hpp" +#include "utilization/util.hpp" + +#include + +#include + +#include +#include + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif + +#include + +namespace behavior_velocity_planner +{ +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_perception_msgs::msg::Shape; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using PathPointsWithLaneId = std::vector; + +struct DynamicObstacleParam +{ + float min_vel_kmph{0.0}; + float max_vel_kmph{5.0}; + + // parameter to convert points to dynamic obstacle + float diameter{0.1}; // [m] + float height{2.0}; // [m] + float max_prediction_time{10.0}; // [sec] + float time_step{0.5}; // [sec] +}; + +struct PoseWithRange +{ + geometry_msgs::msg::Pose pose_min; + geometry_msgs::msg::Pose pose_max; +}; + +// since we use the minimum and maximum velocity, +// define the PredictedPath without time_step +struct PredictedPath +{ + std::vector path; + float confidence; +}; + +// abstracted obstacle information +struct DynamicObstacle +{ + geometry_msgs::msg::Pose pose; + std::vector collision_points; + geometry_msgs::msg::Point nearest_collision_point; + float min_velocity_mps; + float max_velocity_mps; + std::vector classifications; + Shape shape; + std::vector predicted_paths; +}; + +struct DynamicObstacleData +{ + PredictedObjects predicted_objects; + pcl::PointCloud compare_map_filtered_pointcloud; + PathWithLaneId path; +}; + +/** + * @brief base class for creating dynamic obstacles from multiple types of input + */ +class DynamicObstacleCreator +{ +public: + explicit DynamicObstacleCreator(rclcpp::Node & node) : node_(node) {} + virtual ~DynamicObstacleCreator() = default; + virtual std::vector createDynamicObstacles() const = 0; + void setParam(const DynamicObstacleParam & param) { param_ = param; } + void setData(const PlannerData & planner_data, const PathWithLaneId & path) + { + // compare map filtered points are subscribed in derived class that needs points + dynamic_obstacle_data_.predicted_objects = *planner_data.predicted_objects; + dynamic_obstacle_data_.path = path; + } + +protected: + DynamicObstacleParam param_; + rclcpp::Node & node_; + DynamicObstacleData dynamic_obstacle_data_; +}; + +/** + * @brief create dynamic obstacles from predicted objects + */ +class DynamicObstacleCreatorForObject : public DynamicObstacleCreator +{ +public: + explicit DynamicObstacleCreatorForObject(rclcpp::Node & node); + std::vector createDynamicObstacles() const override; +}; + +/** + * @brief create dynamic obstacles from predicted objects, but overwrite the path to be normal to + * the path of ego vehicle. + */ +class DynamicObstacleCreatorForObjectWithoutPath : public DynamicObstacleCreator +{ +public: + explicit DynamicObstacleCreatorForObjectWithoutPath(rclcpp::Node & node); + std::vector createDynamicObstacles() const override; +}; + +/** + * @brief create dynamic obstacles from points. + * predicted path is created to be normal to the path of ego vehicle. + */ +class DynamicObstacleCreatorForPoints : public DynamicObstacleCreator +{ +public: + explicit DynamicObstacleCreatorForPoints(rclcpp::Node & node); + std::vector createDynamicObstacles() const override; + +private: + void onCompareMapFilteredPointCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg); + rclcpp::Subscription::SharedPtr + sub_compare_map_filtered_pointcloud_; + + // tf + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; +}; + +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__RUN_OUT__DYNAMIC_OBSTACLE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/manager.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/manager.hpp new file mode 100644 index 0000000000000..7923535065af8 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/manager.hpp @@ -0,0 +1,46 @@ +// 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 SCENE_MODULE__RUN_OUT__MANAGER_HPP_ +#define SCENE_MODULE__RUN_OUT__MANAGER_HPP_ + +#include "scene_module/run_out/scene.hpp" +#include "scene_module/scene_module_interface.hpp" + +#include + +namespace behavior_velocity_planner +{ +class RunOutModuleManager : public SceneModuleManagerInterface +{ +public: + explicit RunOutModuleManager(rclcpp::Node & node); + + const char * getModuleName() override { return "run_out"; } + +private: + run_out_utils::PlannerParam planner_param_; + std::shared_ptr debug_ptr_; + std::unique_ptr dynamic_obstacle_creator_; + + void launchNewModules(const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + std::function &)> getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) override; + + void setDynamicObstacleCreator(rclcpp::Node & node); +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__RUN_OUT__MANAGER_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp new file mode 100644 index 0000000000000..1a769e3139164 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/path_utils.hpp @@ -0,0 +1,95 @@ +// 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 SCENE_MODULE__RUN_OUT__PATH_UTILS_HPP_ +#define SCENE_MODULE__RUN_OUT__PATH_UTILS_HPP_ + +#include + +#include +#include +#include +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace run_out_utils +{ + +template +geometry_msgs::msg::Point findLongitudinalNearestPoint( + const T & points, const geometry_msgs::msg::Point & src_point, + const std::vector & target_points) +{ + float min_dist = std::numeric_limits::max(); + geometry_msgs::msg::Point min_dist_point{}; + + for (const auto & p : target_points) { + const float dist = tier4_autoware_utils::calcSignedArcLength(points, src_point, p); + if (dist < min_dist) { + min_dist = dist; + min_dist_point = p; + } + } + + return min_dist_point; +} + +template +size_t calcIndexByLength( + const T & points, const geometry_msgs::msg::Pose & current_pose, const double target_length) +{ + const size_t nearest_index = + tier4_autoware_utils::findNearestIndex(points, current_pose.position); + if (target_length < 0) { + return nearest_index; + } + + for (size_t i = nearest_index; i < points.size(); i++) { + double length_sum = tier4_autoware_utils::calcSignedArcLength(points, current_pose.position, i); + if (length_sum > target_length) { + return i; + } + } + + // reach the end of the points, so return the last index + return points.size() - 1; +} + +template +size_t calcIndexByLengthReverse( + const T & points, const geometry_msgs::msg::Point & src_point, const float target_length) +{ + const auto nearest_seg_idx = tier4_autoware_utils::findNearestSegmentIndex(points, src_point); + if (nearest_seg_idx == 0) { + return 0; + } + + for (size_t i = nearest_seg_idx; i > 0; i--) { + const auto length_sum = + std::abs(tier4_autoware_utils::calcSignedArcLength(points, src_point, i)); + if (length_sum > target_length) { + return i + 1; + } + } + + // reach the beginning of the path + return 0; +} + +} // namespace run_out_utils +} // namespace behavior_velocity_planner +#endif // SCENE_MODULE__RUN_OUT__PATH_UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp new file mode 100644 index 0000000000000..3785066a9ae7e --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/scene.hpp @@ -0,0 +1,158 @@ +// 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 SCENE_MODULE__RUN_OUT__SCENE_HPP_ +#define SCENE_MODULE__RUN_OUT__SCENE_HPP_ + +#include "scene_module/run_out/debug.hpp" +#include "scene_module/run_out/dynamic_obstacle.hpp" +#include "scene_module/run_out/utils.hpp" +#include "scene_module/scene_module_interface.hpp" + +#include +#include +#include + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::PathPointWithLaneId; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using run_out_utils::DetectionAreaSize; +using run_out_utils::PlannerParam; +using run_out_utils::State; +using tier4_debug_msgs::msg::Float32Stamped; +using BasicPolygons2d = std::vector; + +class RunOutModule : public SceneModuleInterface +{ +public: + RunOutModule( + const int64_t module_id, const std::shared_ptr & planner_data, + const PlannerParam & planner_param, const rclcpp::Logger logger, + std::unique_ptr dynamic_obstacle_creator, + const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock); + + bool modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + tier4_planning_msgs::msg::StopReason * stop_reason) override; + + visualization_msgs::msg::MarkerArray createDebugMarkerArray() override; + visualization_msgs::msg::MarkerArray createVirtualWallMarkerArray() override; + + void setPlannerParam(const PlannerParam & planner_param); + +private: + // Parameter + PlannerParam planner_param_; + + // Variable + State state_{State::GO}; + rclcpp::Time stop_time_; + BasicPolygons2d partition_lanelets_; + std::unique_ptr dynamic_obstacle_creator_; + std::shared_ptr debug_ptr_; + + // Function + pcl::PointCloud extractObstaclePointsWithRectangle( + const pcl::PointCloud & input_points, + const geometry_msgs::msg::Pose & current_pose) const; + + void visualizeDetectionArea(const PathWithLaneId & smoothed_path) const; + + pcl::PointCloud pointsWithinPolygon( + const std::vector & polygon, + const pcl::PointCloud & candidate_points) const; + + boost::optional detectCollision( + const std::vector & dynamic_obstacles, + const PathWithLaneId & path_points) const; + + float calcCollisionPositionOfVehicleSide( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const; + + std::vector createVehiclePolygon( + const geometry_msgs::msg::Pose & base_pose) const; + + std::vector checkCollisionWithObstacles( + const std::vector & dynamic_obstacles, + std::vector poly, const float travel_time) const; + + boost::optional findNearestCollisionObstacle( + const PathWithLaneId & path, const geometry_msgs::msg::Pose & base_pose, + std::vector & dynamic_obstacles) const; + + boost::optional calcPredictedObstaclePose( + const std::vector & predicted_paths, const float travel_time, + const float velocity_mps) const; + + bool checkCollisionWithShape( + const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, + const Shape & shape, std::vector & collision_points) const; + + bool checkCollisionWithCylinder( + const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, + const float radius, std::vector & collision_points) const; + + bool checkCollisionWithBoundingBox( + const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, + const geometry_msgs::msg::Vector3 & dimension, + std::vector & collision_points) const; + + bool checkCollisionWithPolygon() const; + + std::vector createBoundingBoxForRangedPoints( + const PoseWithRange & pose_with_range, const float x_offset, const float y_offset) const; + + boost::optional calcStopPoint( + const boost::optional & dynamic_obstacle, const PathWithLaneId & path, + const geometry_msgs::msg::Pose & current_pose, const float current_vel, + const float current_acc) const; + + void insertStopPoint( + const boost::optional stop_point, + autoware_auto_planning_msgs::msg::PathWithLaneId & path); + + void insertVelocity( + const boost::optional & dynamic_obstacle, + const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, + const PathWithLaneId & smoothed_path, PathWithLaneId & output_path); + + void insertStoppingVelocity( + const boost::optional & dynamic_obstacle, + const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, + const PathWithLaneId & smoothed_path, PathWithLaneId & output_path); + + void insertApproachingVelocity( + const DynamicObstacle & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose, + const float approaching_vel, const float approach_margin, const PathWithLaneId & resampled_path, + PathWithLaneId & output_path); + + void applyMaxJerkLimit( + const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, + PathWithLaneId & path) const; + + std::vector excludeObstaclesOutSideOfPartition( + const std::vector & dynamic_obstacles, const PathWithLaneId & path, + const geometry_msgs::msg::Pose & current_pose) const; + + void publishDebugValue( + const PathWithLaneId & path, const std::vector extracted_obstacles, + const boost::optional & dynamic_obstacle, + const geometry_msgs::msg::Pose & current_pose) const; +}; +} // namespace behavior_velocity_planner + +#endif // SCENE_MODULE__RUN_OUT__SCENE_HPP_ diff --git a/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp b/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp new file mode 100644 index 0000000000000..6922247bc7818 --- /dev/null +++ b/planning/behavior_velocity_planner/include/scene_module/run_out/utils.hpp @@ -0,0 +1,227 @@ +// 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 SCENE_MODULE__RUN_OUT__UTILS_HPP_ +#define SCENE_MODULE__RUN_OUT__UTILS_HPP_ + +#include "scene_module/run_out/dynamic_obstacle.hpp" + +#include + +#include +#include + +namespace behavior_velocity_planner +{ +namespace run_out_utils +{ +namespace bg = boost::geometry; +using autoware_auto_perception_msgs::msg::ObjectClassification; +using autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_auto_planning_msgs::msg::PathWithLaneId; +using tier4_autoware_utils::Box2d; +using tier4_autoware_utils::LineString2d; +using tier4_autoware_utils::Point2d; +using tier4_autoware_utils::Polygon2d; +using tier4_debug_msgs::msg::Float32Stamped; +using vehicle_info_util::VehicleInfo; +using PathPointsWithLaneId = std::vector; +struct CommonParam +{ + double normal_min_jerk; // min jerk limit for mild stop [m/sss] + double normal_min_acc; // min deceleration limit for mild stop [m/ss] + double limit_min_jerk; // min jerk limit [m/sss] + double limit_min_acc; // min deceleration limit [m/ss] +}; +struct RunOutParam +{ + std::string detection_method; + bool use_partition_lanelet; + bool specify_decel_jerk; + double stop_margin; + double passing_margin; + double deceleration_jerk; + double obstacle_velocity_kph; + float detection_distance; + float detection_span; + float min_vel_ego_kmph; +}; + +struct VehicleParam +{ + float base_to_front; + float base_to_rear; + float width; +}; + +struct DetectionAreaSize +{ + float dist_ahead; + float dist_behind; + float dist_right; + float dist_left; +}; + +struct ApproachingParam +{ + bool enable; + float margin; + float limit_vel_kmph; + float stop_thresh; + float stop_time_thresh; + float dist_thresh; +}; + +struct SlowDownLimit +{ + bool enable; + float max_jerk; + float max_acc; +}; + +struct PlannerParam +{ + CommonParam common; + RunOutParam run_out; + VehicleParam vehicle_param; + DetectionAreaSize detection_area; + ApproachingParam approaching; + DynamicObstacleParam dynamic_obstacle; + SlowDownLimit slow_down_limit; +}; + +enum class State { + GO = 0, + APPROACH, + STOP, +}; + +enum class DetectionMethod { + Object = 0, + ObjectWithoutPath, + Points, + Unknown, +}; + +bool validCheckDecelPlan( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin); + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE1) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @param (t_min) duration of constant deceleration [s] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType1( + const double v0, const double vt, const double a0, const double am, const double ja, + const double jd, const double t_min); + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE2) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType2( + const double v0, const double vt, const double a0, const double ja, const double jd); + +/** + * @brief calculate distance until velocity is reached target velocity (TYPE3) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType3( + const double v0, const double vt, const double a0, const double ja); + +boost::optional calcDecelDistWithJerkAndAccConstraints( + const double current_vel, const double target_vel, const double current_acc, const double acc_min, + const double jerk_acc, const double jerk_dec); + +Polygon2d createBoostPolyFromMsg(const std::vector & input_poly); + +std::uint8_t getHighestProbLabel(const std::vector & classification); + +std::vector getHighestConfidencePath( + const std::vector & predicted_paths); + +// apply linear interpolation to position +geometry_msgs::msg::Pose lerpByPose( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const float t); + +std::vector findLateralSameSidePoints( + const std::vector & points, const geometry_msgs::msg::Pose & base_pose, + const geometry_msgs::msg::Point & target_point); + +bool isSamePoint(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2); + +// if path points have the same point as target_point, return the index +boost::optional haveSamePoint( + const PathPointsWithLaneId & path_points, const geometry_msgs::msg::Point & target_point); + +// insert path velocity which doesn't exceed original velocity +void insertPathVelocityFromIndexLimited( + const size_t & start_idx, const float velocity_mps, PathPointsWithLaneId & path_points); + +void insertPathVelocityFromIndex( + const size_t & start_idx, const float velocity_mps, PathPointsWithLaneId & path_points); + +boost::optional findFirstStopPointIdx(PathPointsWithLaneId & path_points); + +LineString2d createLineString2d(const lanelet::BasicPolygon2d & poly); + +std::vector excludeObstaclesOutSideOfLine( + const std::vector & dynamic_obstacles, const PathPointsWithLaneId & path_points, + const lanelet::BasicPolygon2d & partition); + +PathPointsWithLaneId decimatePathPoints( + const PathPointsWithLaneId & input_path_points, const float step); + +// trim path from self_pose to trim_distance +PathWithLaneId trimPathFromSelfPose( + const PathWithLaneId & input, const geometry_msgs::msg::Pose & self_pose, + const double trim_distance); + +std::vector createDetectionAreaPolygon( + const geometry_msgs::msg::Pose & current_pose, const DetectionAreaSize detection_area_size); + +// create polygon for passing lines and deceleration line calculated by stopping jerk +// note that this polygon is not closed +boost::optional> createDetectionAreaPolygon( + const std::vector> & passing_lines, + const size_t deceleration_line_idx); + +// extend path to the pose of goal +PathWithLaneId extendPath(const PathWithLaneId & input, const double extend_distance); +PathPoint createExtendPathPoint(const double extend_distance, const PathPoint & goal_point); + +DetectionMethod toEnum(const std::string & detection_method); +} // namespace run_out_utils +} // namespace behavior_velocity_planner +#endif // SCENE_MODULE__RUN_OUT__UTILS_HPP_ diff --git a/planning/behavior_velocity_planner/run-out-design.md b/planning/behavior_velocity_planner/run-out-design.md new file mode 100644 index 0000000000000..cea0a18c63210 --- /dev/null +++ b/planning/behavior_velocity_planner/run-out-design.md @@ -0,0 +1,197 @@ +## Run Out + +### Role + +`run_out` is the module that decelerates and stops for dynamic obstacles such as pedestrians and bicycles. + +![brief](./docs/run_out/run_out_overview.svg) + +### Activation Timing + +This module is activated if `launch_run_out` becomes true + +### Inner-workings / Algorithms + +#### Flow chart + +```plantuml +@startuml +title modifyPathVelocity +start + +partition "Preprocess path" { +:Calculate the expected target velocity for ego vehicle; +:Extend path; +:Trim path from ego position; +} + +partition "Preprocess obstacles" { +:Create data of abstracted dynamic obstacles; +:Exclude obstacles outside of partition lanelet; +} + +partition "Collision_detection" { +:Detect collision with dynamic obstacles; +} + +partition "Insert velocity" { +:Insert velocity to decelerate for obstacles; + +:Limit velocity with specified jerk and acc limit; +} +stop +@enduml +``` + +#### Preprocess path + +##### Calculate the expected target velocity for ego vehicle + +Calculate the expected target velocity for the ego vehicle path to calculate time to collision with obstacles more precisely. +The expected target velocity is calculated with [motion velocity smoother module](https://github.com/autowarefoundation/autoware.universe/tree/main/planning/motion_velocity_smoother) by using current velocity, current acceleration and velocity limits directed by the map and external API. + +![brief](./docs/run_out/calculate_expected_target_velocity.svg) + +##### Extend the path + +The path is extended by the length of base link to front to consider obstacles after the goal. + +##### Trim path from ego position + +The path is trimmed from ego position to a certain distance to reduce calculation time. +Trimmed distance is specified by parameter of `detection_distance`. + +#### Preprocess obstacles + +##### Create data of abstracted dynamic obstacle + +This module can handle multiple types of obstacles by creating abstracted dynamic obstacle data layer. Currently we have 3 types of detection method (Object, ObjectWithoutPath, Points) to create abstracted obstacle data. + +###### Abstracted dynamic obstacle + +Abstracted obstacle data has following information. + +| Name | Type | Description | +| ---------------- | ----------------------------------------------------------------------- | ---------------------------------------------------------------------------------------------------------------------- | +| pose | `geometry_msgs::msg::Pose` | pose of the obstacle | +| classifications | `std::vector` | classifications with probability | +| shape | `autoware_auto_perception_msgs::msg::Shape` | shape of the obstacle | +| predicted_paths | `std::vector` | predicted paths with confidence. this data doesn't have time step because we use minimum and maximum velocity instead. | +| min_velocity_mps | `float` | minimum velocity of the obstacle. specified by parameter of `dynamic_obstacle.min_vel_kmph` | +| max_velocity_mps | `float` | maximum velocity of the obstacle. specified by parameter of `dynamic_obstacle.max_vel_kmph` | + +Enter the maximum/minimum velocity of the object as a parameter, adding enough margin to the expected velocity. This parameter is used to create polygons for [collision detection](#Collision-detection). + +Future work: Determine the maximum/minimum velocity from the estimated velocity with covariance of the object + +###### 3 types of detection method + +We have 3 types of detection method to meet different safety and availability requirements. The characteristics of them are shown in the table below. +Method of `Object` has high availability (less false positive) because it detects only objects whose predicted path is on the lane. However, sometimes it is not safe because perception may fail to detect obstacles or generate incorrect predicted paths. +On the other hand, method of `Points` has high safety (less false negative) because it uses pointcloud as input. Since points don't have a predicted path, the path that moves in the direction normal to the path of ego vehicle is considered to be the predicted path of abstracted dynamic obstacle data. However, without proper adjustment of filter of points, it may detect a lot of points and it will result in very low availability. +Method of `ObjectWithoutPath` has the characteristics of an intermediate of `Object` and `Points`. + +| Method | Description | +| ----------------- | --------------------------------------------------------------------------------------------------------------------------------------------------------------------- | +| Object | use an object with the predicted path for collision detection. | +| ObjectWithoutPath | use an object but not use the predicted path for collision detection. replace the path assuming that an object jumps out to the lane at specified velocity. | +| Points | use filtered points for collision detection. the path is created assuming that points jump out to the lane. points are regarded as an small circular shaped obstacle. | + +![brief](./docs/run_out/create_dynamic_obstacle.svg) + +##### Exclude obstacles outside of partition + +This module can exclude the obstacles outside of partition such as guardrail, fence, and wall. +We need lanelet map that has the information of partition to use this feature. +By this feature, we can reduce unnecessary deceleration by obstacles that are unlikely to jump out to the lane. +You can choose whether to use this feature by parameter of `use_partition_lanelet`. + +![brief](./docs/run_out/exclude_obstacles_by_partition.svg) + +#### Collision detection + +##### Detect collision with dynamic obstacles + +Along the ego vehicle path, determine the points where collision detection is to be performed for each `detection_span`. + +The travel times to the each points are calculated from [the expected target velocity](#Calculate-the-expected-target-velocity-for-ego-vehicle). + +![brief](./docs/run_out/create_polygon_on_path_point.svg) + +For the each points, collision detection is performed using the footprint polygon of the ego vehicle and the polygon of the predicted location of the obstacles. +The predicted location of the obstacles is described as rectangle or polygon that has the range calculated by min velocity, max velocity and the ego vehicle's travel time to the point. +If the input type of the dynamic obstacle is `Points`, the obstacle shape is defined as a small cylinder. + +![brief](./docs/run_out/collision_detection_for_shape.svg) + +Multiple points are detected as collision points because collision detection is calculated between two polygons. +So we select the point that is on the same side as the obstacle and close to ego vehicle as the collision point. + +![brief](./docs/run_out/collision_points.svg) + +#### Insert velocity + +##### Insert velocity to decelerate for obstacles + +If the collision is detected, stop point is inserted on distance of base link to front + stop margin from the selected collision point. The base link to front means the distance between base_link (center of rear-wheel axis) and front of the car. Stop margin is determined by the parameter of `stop_margin`. + +![brief](./docs/run_out/insert_velocity.svg) + +##### Limit velocity with specified jerk and acc limit + +The maximum slowdown velocity is calculated in order not to slowdown too much. +See the [Occlusion Spot document](./occlusion-spot-design.md#maximum-slowdown-velocity) for more details. +You can choose whether to use this feature by parameter of `slow_down_limit.enable`. + +### Module Parameters + +| Parameter | Type | Description | +| ----------------------- | ------ | ------------------------------------------------------------------------------------------------------------------------ | +| `detection_method` | string | [-] candidate: Object, ObjectWithoutPath, Points | +| `use_partition_lanelet` | bool | [-] whether to use partition lanelet map data | +| `specify_decel_jerk` | bool | [-] whether to specify jerk when ego decelerates | +| `stop_margin` | double | [m] the vehicle decelerates to be able to stop with this margin | +| `passing_margin` | double | [m] the vehicle begins to accelerate if the vehicle's front in predicted position is ahead of the obstacle + this margin | +| `deceleration_jerk` | double | [m/s^3] ego decelerates with this jerk when stopping for obstacles | +| `obstacle_velocity_kph` | double | [km/h] assumption for obstacle velocity | +| `detection_distance` | double | [m] ahead distance from ego to detect the obstacles | +| `detection_span` | double | [m] calculate collision with this span to reduce calculation time | +| `min_vel_ego_kmph` | double | [km/h] min velocity to calculate time to collision | + +| Parameter /detection_area_size | Type | Description | +| ------------------------------ | ------ | ------------------------------------- | +| `dist_ahead` | double | [m] ahead distance from ego position | +| `dist_behind` | double | [m] behind distance from ego position | +| `dist_right` | double | [m] right distance from ego position | +| `dist_left` | double | [m] left distance from ego position | + +| Parameter /dynamic_obstacle | Type | Description | +| --------------------------- | ------ | ----------------------------------------------------------------------------------------------------------- | +| `min_vel_kmph` | double | [km/h] minimum velocity for dynamic obstacles | +| `max_vel_kmph` | double | [km/h] maximum velocity for dynamic obstacles | +| `diameter` | double | [m] diameter of obstacles. used for creating dynamic obstacles from points | +| `height` | double | [m] height of obstacles. used for creating dynamic obstacles from points | +| `max_prediction_time` | double | [sec] create predicted path until this time | +| `time_step` | double | [sec] time step for each path step. used for creating dynamic obstacles from points or objects without path | + +| Parameter /approaching | Type | Description | +| ---------------------- | ------ | -------------------------------------------------------------------------------------------------- | +| `enable` | bool | [-] whether to enable approaching after stopping | +| `margin` | double | [m] distance on how close ego approaches the obstacle | +| `limit_vel_kmph` | double | [km/h] limit velocity for approaching after stopping | +| `stop_thresh` | double | [m/s] threshold to decide if ego is stopping | +| `stop_time_thresh` | double | [sec] threshold for stopping time to transit to approaching state | +| `dist_thresh` | double | [m] end the approaching state if distance to the obstacle is longer than stop_margin + dist_thresh | + +| Parameter /slow_down_limit | Type | Description | +| -------------------------- | ------ | ------------------------------------------------------------- | +| `enable` | bool | [-] whether to enable to limit velocity with max jerk and acc | +| `max_jerk` | double | [m/s^3] minimum jerk deceleration for safe brake. | +| `max_acc` | double | [m/s^2] minimum accel deceleration for safe brake. | + +### Future extensions / Unimplemented parts + +- Calculate obstacle's min velocity and max velocity from covariance +- Detect collisions with polygon object +- Handle the case when the predicted path of obstacles are not straight line + - Currently collision check is calculated based on the assumption that the predicted path of the obstacle is a straight line diff --git a/planning/behavior_velocity_planner/src/node.cpp b/planning/behavior_velocity_planner/src/node.cpp index 1a8dac1aca038..325c7e860a25e 100644 --- a/planning/behavior_velocity_planner/src/node.cpp +++ b/planning/behavior_velocity_planner/src/node.cpp @@ -40,6 +40,7 @@ #include #include #include +#include #include #include #include @@ -195,6 +196,9 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio if (this->declare_parameter("launch_occlusion_spot", true)) { planner_manager_.launchSceneModule(std::make_shared(*this)); } + if (this->declare_parameter("launch_run_out", false)) { + planner_manager_.launchSceneModule(std::make_shared(*this)); + } } // NOTE: argument planner_data must not be referenced for multithreading diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp new file mode 100644 index 0000000000000..3045b01b91d44 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/debug.cpp @@ -0,0 +1,322 @@ +// 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 "scene_module/run_out/debug.hpp" + +#include "scene_module/run_out/scene.hpp" + +using tier4_autoware_utils::appendMarkerArray; +using tier4_autoware_utils::calcOffsetPose; +using tier4_autoware_utils::createDefaultMarker; +using tier4_autoware_utils::createMarkerColor; +using tier4_autoware_utils::createMarkerOrientation; +using tier4_autoware_utils::createMarkerScale; +using tier4_autoware_utils::createPoint; + +namespace behavior_velocity_planner +{ +namespace +{ +RunOutDebug::TextWithPosition createDebugText( + const std::string text, const geometry_msgs::msg::Pose pose, const float lateral_offset) +{ + const auto offset_pose = tier4_autoware_utils::calcOffsetPose(pose, 0, lateral_offset, 0); + + RunOutDebug::TextWithPosition text_with_position; + text_with_position.text = text; + text_with_position.position = offset_pose.position; + + return text_with_position; +} + +RunOutDebug::TextWithPosition createDebugText( + const std::string text, const geometry_msgs::msg::Point position) +{ + RunOutDebug::TextWithPosition text_with_position; + text_with_position.text = text; + text_with_position.position = position; + + return text_with_position; +} + +} // namespace + +RunOutDebug::RunOutDebug(rclcpp::Node & node) : node_(node) +{ + pub_debug_values_ = + node.create_publisher("~/run_out/debug/debug_values", 1); + pub_accel_reason_ = node.create_publisher("~/run_out/debug/accel_reason", 1); + pub_debug_trajectory_ = node.create_publisher("~/run_out/debug/trajectory", 1); +} + +void RunOutDebug::pushDebugPoints(const pcl::PointXYZ & debug_point) +{ + geometry_msgs::msg::Point ros_point; + ros_point.x = debug_point.x; + ros_point.y = debug_point.y; + ros_point.z = debug_point.z; + + debug_points_.push_back(ros_point); +} + +void RunOutDebug::pushDebugPoints(const geometry_msgs::msg::Point & debug_point) +{ + debug_points_.push_back(debug_point); +} + +void RunOutDebug::pushDebugPoints(const std::vector & debug_points) +{ + for (const auto & p : debug_points) { + debug_points_.push_back(p); + } +} + +void RunOutDebug::pushDebugPoints( + const geometry_msgs::msg::Point & debug_point, const PointType point_type) +{ + switch (point_type) { + case PointType::Blue: + debug_points_.push_back(debug_point); + break; + + case PointType::Red: + debug_points_red_.push_back(debug_point); + break; + + case PointType::Yellow: + debug_points_yellow_.push_back(debug_point); + break; + + default: + break; + } +} + +void RunOutDebug::pushStopPose(const geometry_msgs::msg::Pose & pose) +{ + stop_pose_.push_back(pose); +} + +void RunOutDebug::pushDebugLines(const std::vector & debug_line) +{ + debug_lines_.push_back(debug_line); +} + +void RunOutDebug::pushDebugPolygons(const std::vector & debug_polygon) +{ + debug_polygons_.push_back(debug_polygon); +} + +void RunOutDebug::pushDetectionAreaPolygons(const Polygon2d & debug_polygon) +{ + std::vector ros_points; + for (const auto & p : debug_polygon.outer()) { + ros_points.push_back(tier4_autoware_utils::createPoint(p.x(), p.y(), 0.0)); + } + + detection_area_polygons_.push_back(ros_points); +} + +void RunOutDebug::pushDebugTexts(const TextWithPosition & debug_text) +{ + debug_texts_.push_back(debug_text); +} + +void RunOutDebug::pushDebugTexts( + const std::string text, const geometry_msgs::msg::Pose pose, const float lateral_offset) +{ + debug_texts_.push_back(createDebugText(text, pose, lateral_offset)); +} + +void RunOutDebug::pushDebugTexts(const std::string text, const geometry_msgs::msg::Point position) +{ + debug_texts_.push_back(createDebugText(text, position)); +} + +void RunOutDebug::clearDebugMarker() +{ + debug_points_.clear(); + debug_points_red_.clear(); + debug_points_yellow_.clear(); + debug_lines_.clear(); + debug_polygons_.clear(); + detection_area_polygons_.clear(); + debug_texts_.clear(); +} + +visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArray() +{ + rclcpp::Time current_time = node_.now(); + + // create marker array from debug data + auto visualization_marker_array = createVisualizationMarkerArrayFromDebugData(current_time); + + // clear debug data + clearDebugMarker(); + + return visualization_marker_array; +} + +visualization_msgs::msg::MarkerArray RunOutDebug::createVirtualWallMarkerArray() +{ + visualization_msgs::msg::MarkerArray wall_marker; + rclcpp::Time now = node_.now(); + size_t id = 0; + + for (const auto & p : stop_pose_) { + appendMarkerArray( + tier4_autoware_utils::createStopVirtualWallMarker(p, "run_out", now, id++), &wall_marker); + } + + stop_pose_.clear(); + + return wall_marker; +} + +visualization_msgs::msg::MarkerArray RunOutDebug::createVisualizationMarkerArrayFromDebugData( + const builtin_interfaces::msg::Time & current_time) +{ + visualization_msgs::msg::MarkerArray msg; + + if (!debug_points_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "debug_points", 0, visualization_msgs::msg::Marker::SPHERE_LIST, + createMarkerScale(0.5, 0.5, 0.5), createMarkerColor(0, 0.0, 1.0, 0.999)); + for (const auto & p : debug_points_) { + marker.points.push_back(p); + } + msg.markers.push_back(marker); + } + + if (!debug_points_red_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "debug_points_red", 0, visualization_msgs::msg::Marker::SPHERE_LIST, + createMarkerScale(1.0, 1.0, 1.0), createMarkerColor(1.0, 0, 0, 0.999)); + for (const auto & p : debug_points_red_) { + marker.points.push_back(p); + } + msg.markers.push_back(marker); + } + + if (!debug_points_yellow_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "debug_points_yellow", 0, visualization_msgs::msg::Marker::SPHERE_LIST, + createMarkerScale(0.7, 0.7, 0.7), createMarkerColor(1.0, 1.0, 0, 0.999)); + for (const auto & p : debug_points_yellow_) { + marker.points.push_back(p); + } + msg.markers.push_back(marker); + } + + if (!debug_lines_.empty()) { + int32_t marker_id = 0; + for (const auto & line : debug_lines_) { + auto marker = createDefaultMarker( + "map", current_time, "debug_lines", marker_id, visualization_msgs::msg::Marker::LINE_STRIP, + createMarkerScale(0.1, 0.0, 0.0), createMarkerColor(0, 0, 1.0, 0.999)); + for (const auto & p : line) { + marker.points.push_back(p); + } + msg.markers.push_back(marker); + marker_id++; + } + } + + if (!debug_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "debug_polygons", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(1.0, 1.0, 0.0, 0.999)); + + for (const auto & poly : debug_polygons_) { + for (size_t i = 0; i < poly.size() - 1; i++) { + marker.points.push_back(poly.at(i)); + marker.points.push_back(poly.at(i + 1)); + } + marker.points.push_back(poly.back()); + marker.points.push_back(poly.front()); + } + + msg.markers.push_back(marker); + } + + if (!detection_area_polygons_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "detection_area_polygon", 0, visualization_msgs::msg::Marker::LINE_LIST, + createMarkerScale(0.05, 0.0, 0.0), createMarkerColor(0.0, 0.0, 1.0, 0.999)); + + for (const auto & poly : detection_area_polygons_) { + for (size_t i = 0; i < poly.size() - 1; i++) { + marker.points.push_back(poly.at(i)); + marker.points.push_back(poly.at(i + 1)); + } + marker.points.push_back(poly.back()); + marker.points.push_back(poly.front()); + } + + msg.markers.push_back(marker); + } + + if (!debug_texts_.empty()) { + auto marker = createDefaultMarker( + "map", current_time, "debug_texts", 0, visualization_msgs::msg::Marker::TEXT_VIEW_FACING, + createMarkerScale(0.0, 0.0, 0.8), createMarkerColor(1.0, 1.0, 1.0, 0.999)); + + constexpr float height_offset = 2.0; + for (const auto & text : debug_texts_) { + marker.pose.position = text.position; + marker.pose.position.z += height_offset; + marker.text = text.text; + + msg.markers.push_back(marker); + marker.id++; + } + } + + return msg; +} +void RunOutDebug::setAccelReason(const AccelReason & accel_reason) { accel_reason_ = accel_reason; } + +void RunOutDebug::publishDebugValue() +{ + // publish debug values + tier4_debug_msgs::msg::Float32MultiArrayStamped debug_msg{}; + debug_msg.stamp = node_.now(); + for (const auto & v : debug_values_.getValues()) { + debug_msg.data.push_back(v); + } + pub_debug_values_->publish(debug_msg); + + Int32Stamped accel_reason; + accel_reason.stamp = node_.now(); + accel_reason.data = static_cast(accel_reason_); + pub_accel_reason_->publish(accel_reason); +} + +void RunOutDebug::publishDebugTrajectory(const Trajectory & trajectory) +{ + pub_debug_trajectory_->publish(trajectory); +} + +// scene module +visualization_msgs::msg::MarkerArray RunOutModule::createDebugMarkerArray() +{ + return debug_ptr_->createVisualizationMarkerArray(); +} + +visualization_msgs::msg::MarkerArray RunOutModule::createVirtualWallMarkerArray() +{ + return debug_ptr_->createVirtualWallMarkerArray(); +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp new file mode 100644 index 0000000000000..afe2994ed4cf5 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/dynamic_obstacle.cpp @@ -0,0 +1,218 @@ +// 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 "scene_module/run_out/dynamic_obstacle.hpp" + +#include + +namespace behavior_velocity_planner +{ +namespace +{ +// create quaternion facing to the nearest trajectory point +geometry_msgs::msg::Quaternion createQuaternionFacingToTrajectory( + const PathPointsWithLaneId & path_points, const geometry_msgs::msg::Point & point) +{ + const auto nearest_idx = tier4_autoware_utils::findNearestIndex(path_points, point); + const auto & nearest_pose = path_points.at(nearest_idx).point.pose; + + const auto longitudinal_offset = + tier4_autoware_utils::calcLongitudinalDeviation(nearest_pose, point); + const auto vertical_point = + tier4_autoware_utils::calcOffsetPose(nearest_pose, longitudinal_offset, 0, 0).position; + const auto azimuth_angle = tier4_autoware_utils::calcAzimuthAngle(point, vertical_point); + + return tier4_autoware_utils::createQuaternionFromYaw(azimuth_angle); +} + +// create predicted path assuming that obstacles move with constant velocity +std::vector createPredictedPath( + const geometry_msgs::msg::Pose & initial_pose, const float time_step, + const float max_velocity_mps, const float max_prediction_time) +{ + const size_t path_size = max_prediction_time / time_step; + std::vector path_points; + for (size_t i = 0; i < path_size; i++) { + const float travel_dist = max_velocity_mps * time_step * i; + const auto predicted_pose = + tier4_autoware_utils::calcOffsetPose(initial_pose, travel_dist, 0, 0); + path_points.emplace_back(predicted_pose); + } + + return path_points; +} + +pcl::PointCloud applyVoxelGridFilter( + const pcl::PointCloud::ConstPtr & input_points) +{ + auto no_height_points = *input_points; + for (auto & p : no_height_points) { + p.z = 0.0; + } + + // use boost::makeshared instead of std beacause filter.setInputCloud requires boost shared ptr + pcl::VoxelGrid filter; + filter.setInputCloud(pcl::make_shared>(no_height_points)); + filter.setLeafSize(0.05f, 0.05f, 100000.0f); + + pcl::PointCloud output_points; + filter.filter(output_points); + + return output_points; +} +} // namespace + +DynamicObstacleCreatorForObject::DynamicObstacleCreatorForObject(rclcpp::Node & node) +: DynamicObstacleCreator(node) +{ +} + +std::vector DynamicObstacleCreatorForObject::createDynamicObstacles() const +{ + // create dynamic obstacles from predicted objects + std::vector dynamic_obstacles; + for (const auto & predicted_object : dynamic_obstacle_data_.predicted_objects.objects) { + DynamicObstacle dynamic_obstacle; + dynamic_obstacle.pose = predicted_object.kinematics.initial_pose_with_covariance.pose; + + // TODO(Tomohito Ando): calculate velocity from covariance of predicted_object + dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph); + dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph); + dynamic_obstacle.classifications = predicted_object.classification; + dynamic_obstacle.shape = predicted_object.shape; + + // get predicted paths of predicted_objects + for (const auto & path : predicted_object.kinematics.predicted_paths) { + PredictedPath predicted_path; + predicted_path.confidence = path.confidence; + predicted_path.path.resize(path.path.size()); + std::copy(path.path.cbegin(), path.path.cend(), predicted_path.path.begin()); + + dynamic_obstacle.predicted_paths.emplace_back(predicted_path); + } + + dynamic_obstacles.emplace_back(dynamic_obstacle); + } + + return dynamic_obstacles; +} + +DynamicObstacleCreatorForObjectWithoutPath::DynamicObstacleCreatorForObjectWithoutPath( + rclcpp::Node & node) +: DynamicObstacleCreator(node) +{ +} + +std::vector DynamicObstacleCreatorForObjectWithoutPath::createDynamicObstacles() + const +{ + std::vector dynamic_obstacles; + + for (const auto & predicted_object : dynamic_obstacle_data_.predicted_objects.objects) { + DynamicObstacle dynamic_obstacle; + dynamic_obstacle.pose.position = + predicted_object.kinematics.initial_pose_with_covariance.pose.position; + dynamic_obstacle.pose.orientation = createQuaternionFacingToTrajectory( + dynamic_obstacle_data_.path.points, dynamic_obstacle.pose.position); + + dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph); + dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph); + dynamic_obstacle.classifications = predicted_object.classification; + dynamic_obstacle.shape = predicted_object.shape; + + // replace predicted path with path that runs straight to lane + PredictedPath predicted_path; + predicted_path.path = createPredictedPath( + dynamic_obstacle.pose, param_.time_step, dynamic_obstacle.max_velocity_mps, + param_.max_prediction_time); + predicted_path.confidence = 1.0; + dynamic_obstacle.predicted_paths.emplace_back(predicted_path); + + dynamic_obstacles.emplace_back(dynamic_obstacle); + } + + return dynamic_obstacles; +} + +DynamicObstacleCreatorForPoints::DynamicObstacleCreatorForPoints(rclcpp::Node & node) +: DynamicObstacleCreator(node), tf_buffer_(node.get_clock()), tf_listener_(tf_buffer_) +{ + using std::placeholders::_1; + sub_compare_map_filtered_pointcloud_ = node.create_subscription( + "~/input/compare_map_filtered_pointcloud", rclcpp::SensorDataQoS(), + std::bind(&DynamicObstacleCreatorForPoints::onCompareMapFilteredPointCloud, this, _1)); +} + +std::vector DynamicObstacleCreatorForPoints::createDynamicObstacles() const +{ + std::vector dynamic_obstacles; + for (const auto & point : dynamic_obstacle_data_.compare_map_filtered_pointcloud) { + DynamicObstacle dynamic_obstacle; + + // create pose facing the direction of the lane + dynamic_obstacle.pose.position = tier4_autoware_utils::createPoint(point.x, point.y, point.z); + dynamic_obstacle.pose.orientation = createQuaternionFacingToTrajectory( + dynamic_obstacle_data_.path.points, dynamic_obstacle.pose.position); + + dynamic_obstacle.min_velocity_mps = tier4_autoware_utils::kmph2mps(param_.min_vel_kmph); + dynamic_obstacle.max_velocity_mps = tier4_autoware_utils::kmph2mps(param_.max_vel_kmph); + + // create classification of points as pedestrian + ObjectClassification classification; + classification.label = ObjectClassification::PEDESTRIAN; + classification.probability = 1.0; + dynamic_obstacle.classifications.emplace_back(classification); + + // create shape of points as cylinder + dynamic_obstacle.shape.type = Shape::CYLINDER; + dynamic_obstacle.shape.dimensions.x = param_.diameter; + dynamic_obstacle.shape.dimensions.y = param_.diameter; + dynamic_obstacle.shape.dimensions.z = param_.height; + + // create predicted path of points + PredictedPath predicted_path; + predicted_path.path = createPredictedPath( + dynamic_obstacle.pose, param_.time_step, dynamic_obstacle.max_velocity_mps, + param_.max_prediction_time); + predicted_path.confidence = 1.0; + dynamic_obstacle.predicted_paths.emplace_back(predicted_path); + + dynamic_obstacles.emplace_back(dynamic_obstacle); + } + + return dynamic_obstacles; +} + +void DynamicObstacleCreatorForPoints::onCompareMapFilteredPointCloud( + const sensor_msgs::msg::PointCloud2::ConstSharedPtr msg) +{ + geometry_msgs::msg::TransformStamped transform; + try { + transform = tf_buffer_.lookupTransform( + "map", msg->header.frame_id, msg->header.stamp, rclcpp::Duration::from_seconds(0.1)); + } catch (tf2::TransformException & e) { + RCLCPP_WARN(node_.get_logger(), "no transform found for no_ground_pointcloud: %s", e.what()); + return; + } + + pcl::PointCloud pc; + pcl::fromROSMsg(*msg, pc); + + Eigen::Affine3f affine = tf2::transformToEigen(transform.transform).cast(); + pcl::PointCloud::Ptr pc_transformed(new pcl::PointCloud); + pcl::transformPointCloud(pc, *pc_transformed, affine); + + dynamic_obstacle_data_.compare_map_filtered_pointcloud = applyVoxelGridFilter(pc_transformed); +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp new file mode 100644 index 0000000000000..5b80ac2c0537e --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/manager.cpp @@ -0,0 +1,154 @@ +// 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 "scene_module/run_out/manager.hpp" + +namespace behavior_velocity_planner +{ +namespace +{ +} // namespace + +RunOutModuleManager::RunOutModuleManager(rclcpp::Node & node) +: SceneModuleManagerInterface(node, getModuleName()) +{ + // Vehicle Parameters + { + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo(); + auto & p = planner_param_.vehicle_param; + p.base_to_front = vehicle_info.wheel_base_m + vehicle_info.front_overhang_m; + p.base_to_rear = vehicle_info.rear_overhang_m; + p.width = vehicle_info.vehicle_width_m; + } + + const std::string ns(getModuleName()); + + { + auto & p = planner_param_.common; + p.normal_min_jerk = node.declare_parameter(".normal.min_jerk", -0.3); + p.normal_min_acc = node.declare_parameter(".normal.min_acc", -1.0); + p.limit_min_jerk = node.declare_parameter(".limit.min_jerk", -1.5); + p.limit_min_acc = node.declare_parameter(".limit.min_acc", -2.5); + } + + { + auto & p = planner_param_.run_out; + p.detection_method = node.declare_parameter(ns + ".detection_method", "Object"); + p.use_partition_lanelet = node.declare_parameter(ns + ".use_partition_lanelet", true); + p.specify_decel_jerk = node.declare_parameter(ns + ".specify_decel_jerk", false); + p.stop_margin = node.declare_parameter(ns + ".stop_margin", 2.5); + p.passing_margin = node.declare_parameter(ns + ".passing_margin", 1.0); + p.deceleration_jerk = node.declare_parameter(ns + ".deceleration_jerk", -0.3); + p.obstacle_velocity_kph = node.declare_parameter(ns + ".obstacle_velocity_kph", 5.0); + p.detection_distance = node.declare_parameter(ns + ".detection_distance", 45.0); + p.detection_span = node.declare_parameter(ns + ".detection_span", 1.0); + p.min_vel_ego_kmph = node.declare_parameter(ns + ".min_vel_ego_kmph", 5.0); + } + + { + auto & p = planner_param_.detection_area; + const std::string ns_da = ns + ".detection_area_size"; + p.dist_ahead = node.declare_parameter(ns_da + ".dist_ahead", 50.0); + p.dist_behind = node.declare_parameter(ns_da + ".dist_behind", 5.0); + p.dist_right = node.declare_parameter(ns_da + ".dist_right", 10.0); + p.dist_left = node.declare_parameter(ns_da + ".dist_left", 10.0); + } + + { + auto & p = planner_param_.dynamic_obstacle; + const std::string ns_do = ns + ".dynamic_obstacle"; + p.min_vel_kmph = node.declare_parameter(ns_do + ".min_vel_kmph", 0.0); + p.max_vel_kmph = node.declare_parameter(ns_do + ".max_vel_kmph", 5.0); + p.diameter = node.declare_parameter(ns_do + ".diameter", 0.1); + p.height = node.declare_parameter(ns_do + ".height", 2.0); + p.max_prediction_time = node.declare_parameter(ns_do + ".max_prediction_time", 10.0); + p.time_step = node.declare_parameter(ns_do + ".time_step", 0.5); + } + + { + auto & p = planner_param_.approaching; + const std::string ns_a = ns + ".approaching"; + p.enable = node.declare_parameter(ns_a + ".enable", false); + p.margin = node.declare_parameter(ns_a + ".margin", 0.0); + p.limit_vel_kmph = node.declare_parameter(ns_a + ".limit_vel_kmph", 5.0); + p.stop_thresh = node.declare_parameter(ns_a + ".stop_thresh", 0.01); + p.stop_time_thresh = node.declare_parameter(ns_a + ".stop_time_thresh", 3.0); + p.dist_thresh = node.declare_parameter(ns_a + ".dist_thresh", 0.5); + } + + { + auto & p = planner_param_.slow_down_limit; + const std::string ns_m = ns + ".slow_down_limit"; + p.enable = node.declare_parameter(ns_m + ".enable", true); + p.max_jerk = node.declare_parameter(ns_m + ".max_jerk", -0.7); + p.max_acc = node.declare_parameter(ns_m + ".max_acc", -2.0); + } + + debug_ptr_ = std::make_shared(node); + setDynamicObstacleCreator(node); +} + +void RunOutModuleManager::launchNewModules( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + if (path.points.empty()) { + return; + } + + constexpr int64_t module_id = 0; + if (!isModuleRegistered(module_id)) { + registerModule(std::make_shared( + module_id, planner_data_, planner_param_, logger_.get_child("run_out_module"), + std::move(dynamic_obstacle_creator_), debug_ptr_, clock_)); + } +} + +std::function &)> +RunOutModuleManager::getModuleExpiredFunction( + const autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + return + [&path]([[maybe_unused]] const std::shared_ptr & scene_module) -> bool { + return false; + }; +} + +void RunOutModuleManager::setDynamicObstacleCreator(rclcpp::Node & node) +{ + using run_out_utils::DetectionMethod; + + const auto detection_method_enum = run_out_utils::toEnum(planner_param_.run_out.detection_method); + switch (detection_method_enum) { + case DetectionMethod::Object: + dynamic_obstacle_creator_ = std::make_unique(node); + break; + + case DetectionMethod::ObjectWithoutPath: + dynamic_obstacle_creator_ = + std::make_unique(node); + break; + + case DetectionMethod::Points: + dynamic_obstacle_creator_ = std::make_unique(node); + break; + + default: + RCLCPP_WARN_STREAM(logger_, "detection method is invalid. use default method (Object)."); + dynamic_obstacle_creator_ = std::make_unique(node); + break; + } + + dynamic_obstacle_creator_->setParam(planner_param_.dynamic_obstacle); +} +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp new file mode 100644 index 0000000000000..71bb6ee12a29b --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/scene.cpp @@ -0,0 +1,879 @@ +// 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 "scene_module/run_out/scene.hpp" + +#include "scene_module/run_out/path_utils.hpp" +#include "utilization/trajectory_utils.hpp" +#include "utilization/util.hpp" + +namespace behavior_velocity_planner +{ +namespace bg = boost::geometry; + +RunOutModule::RunOutModule( + const int64_t module_id, const std::shared_ptr & planner_data, + const PlannerParam & planner_param, const rclcpp::Logger logger, + std::unique_ptr dynamic_obstacle_creator, + const std::shared_ptr & debug_ptr, const rclcpp::Clock::SharedPtr clock) +: SceneModuleInterface(module_id, logger, clock), + planner_param_(planner_param), + dynamic_obstacle_creator_(std::move(dynamic_obstacle_creator)), + debug_ptr_(debug_ptr) +{ + if (planner_param.run_out.use_partition_lanelet) { + const lanelet::LaneletMapConstPtr & ll = planner_data->route_handler_->getLaneletMapPtr(); + planning_utils::getAllPartitionLanelets(ll, partition_lanelets_); + } +} + +void RunOutModule::setPlannerParam(const PlannerParam & planner_param) +{ + planner_param_ = planner_param; +} + +bool RunOutModule::modifyPathVelocity( + autoware_auto_planning_msgs::msg::PathWithLaneId * path, + [[maybe_unused]] tier4_planning_msgs::msg::StopReason * stop_reason) +{ + // timer starts + const auto t1_modify_path = std::chrono::system_clock::now(); + + // set planner data + const auto current_vel = planner_data_->current_velocity->twist.linear.x; + const auto current_acc = planner_data_->current_accel.get(); + const auto & current_pose = planner_data_->current_pose.pose; + + // smooth velocity of the path to calcute time to collision accurately + PathWithLaneId smoothed_path; + if (!smoothPath(*path, smoothed_path, planner_data_)) { + return true; + } + + // extend path to consider obstacles after the goal + const auto extended_smoothed_path = + run_out_utils::extendPath(smoothed_path, planner_param_.vehicle_param.base_to_front); + + // trim path ahead of the base_link to make calculation easier + const double trim_distance = planner_param_.run_out.detection_distance; + const auto trim_smoothed_path = + run_out_utils::trimPathFromSelfPose(extended_smoothed_path, current_pose, trim_distance); + + // create abstracted dynamic obstacles from objects or points + dynamic_obstacle_creator_->setData(*planner_data_, *path); + const auto dynamic_obstacles = dynamic_obstacle_creator_->createDynamicObstacles(); + debug_ptr_->setDebugValues(DebugValues::TYPE::NUM_OBSTACLES, dynamic_obstacles.size()); + + // extract obstacles using lanelet information + const auto partition_excluded_obstacles = + excludeObstaclesOutSideOfPartition(dynamic_obstacles, trim_smoothed_path, current_pose); + + // timer starts + const auto t1_collision_check = std::chrono::system_clock::now(); + + // detect collision with dynamic obstacles using velocity planning of ego + const auto dynamic_obstacle = detectCollision(partition_excluded_obstacles, trim_smoothed_path); + + // timer ends + const auto t2_collision_check = std::chrono::system_clock::now(); + const auto elapsed_collision_check = + std::chrono::duration_cast(t2_collision_check - t1_collision_check); + debug_ptr_->setDebugValues( + DebugValues::TYPE::CALCULATION_TIME_COLLISION_CHECK, elapsed_collision_check.count() / 1000.0); + + // insert stop point for the detected obstacle + if (planner_param_.approaching.enable) { + // after a certain amount of time has elapsed since the ego stopped, + // approach the obstacle with slow velocity + insertVelocity( + dynamic_obstacle, current_pose, current_vel, current_acc, trim_smoothed_path, *path); + } else { + // just insert zero velocity for stopping + insertStoppingVelocity( + dynamic_obstacle, current_pose, current_vel, current_acc, trim_smoothed_path, *path); + } + + // apply max jerk limit if the ego can't stop with specified max jerk and acc + if (planner_param_.slow_down_limit.enable) { + applyMaxJerkLimit(current_pose, current_vel, current_acc, *path); + } + + visualizeDetectionArea(trim_smoothed_path); + publishDebugValue( + trim_smoothed_path, partition_excluded_obstacles, dynamic_obstacle, current_pose); + + // timer ends + const auto t2_modify_path = std::chrono::system_clock::now(); + const auto elapsed_modify_path = + std::chrono::duration_cast(t2_modify_path - t1_modify_path); + debug_ptr_->setDebugValues( + DebugValues::TYPE::CALCULATION_TIME, elapsed_modify_path.count() / 1000.0); + + return true; +} + +pcl::PointCloud RunOutModule::extractObstaclePointsWithRectangle( + const pcl::PointCloud & input_points, + const geometry_msgs::msg::Pose & current_pose) const +{ + const auto detection_area_polygon = + createDetectionAreaPolygon(current_pose, planner_param_.detection_area); + + debug_ptr_->pushDebugPolygons(detection_area_polygon); + + const auto extracted_points = pointsWithinPolygon(detection_area_polygon, input_points); + + return extracted_points; +} + +void RunOutModule::visualizeDetectionArea(const PathWithLaneId & smoothed_path) const +{ + // calculate distance needed to stop with jerk and acc constraints + const float initial_vel = planner_data_->current_velocity->twist.linear.x; + const float initial_acc = planner_data_->current_accel.get(); + const float target_vel = 0.0; + const float jerk_dec = planner_param_.run_out.deceleration_jerk; + const float jerk_acc = std::abs(jerk_dec); + const float planning_dec = jerk_dec < planner_param_.common.normal_min_jerk + ? planner_param_.common.limit_min_acc + : planner_param_.common.normal_min_acc; + auto stop_dist = run_out_utils::calcDecelDistWithJerkAndAccConstraints( + initial_vel, target_vel, initial_acc, planning_dec, jerk_acc, jerk_dec); + + if (!stop_dist) { + return; + } + + DetectionRange da_range; + const float obstacle_vel_mps = planner_param_.dynamic_obstacle.max_vel_kmph / 3.6; + da_range.interval = planner_param_.run_out.detection_distance; + da_range.min_longitudinal_distance = planner_param_.vehicle_param.base_to_front; + da_range.max_longitudinal_distance = *stop_dist + planner_param_.run_out.stop_margin; + da_range.min_lateral_distance = planner_param_.vehicle_param.width / 2.0; + da_range.max_lateral_distance = + obstacle_vel_mps * planner_param_.dynamic_obstacle.max_prediction_time; + Polygons2d detection_area_poly; + planning_utils::createDetectionAreaPolygons( + detection_area_poly, smoothed_path, planner_data_->current_pose.pose, da_range, + planner_param_.dynamic_obstacle.max_vel_kmph / 3.6); + + for (const auto & poly : detection_area_poly) { + debug_ptr_->pushDetectionAreaPolygons(poly); + } +} + +pcl::PointCloud RunOutModule::pointsWithinPolygon( + const std::vector & polygon, + const pcl::PointCloud & candidate_points) const +{ + // convert to boost type + const tier4_autoware_utils::Polygon2d bg_poly = run_out_utils::createBoostPolyFromMsg(polygon); + + // find points in detection area + pcl::PointCloud within_points; + for (const auto & p : candidate_points) { + tier4_autoware_utils::Point2d point(p.x, p.y); + + if (!bg::covered_by(point, bg_poly)) { + continue; + } + + within_points.push_back(p); + } + + return within_points; +} + +boost::optional RunOutModule::detectCollision( + const std::vector & dynamic_obstacles, const PathWithLaneId & path) const +{ + if (path.points.size() < 2) { + RCLCPP_WARN_STREAM(logger_, "path doesn't have enough points."); + return {}; + } + + // detect collision with obstacles from the nearest path point to the end + // ignore the travel time from current pose to nearest path point? + float travel_time = 0.0; + float dist_sum = 0.0; + for (size_t idx = 1; idx < path.points.size(); idx++) { + const auto & p1 = path.points.at(idx - 1).point; + const auto & p2 = path.points.at(idx).point; + const float prev_vel = + std::max(p1.longitudinal_velocity_mps, planner_param_.run_out.min_vel_ego_kmph / 3.6f); + const float ds = tier4_autoware_utils::calcDistance2d(p1, p2); + + // calculate travel time from nearest point to p2 + travel_time += ds / prev_vel; + dist_sum += ds; + + // skip collision detection to reduce calculation time + if (idx != 1 && dist_sum < planner_param_.run_out.detection_span) { + continue; + } + dist_sum = 0.0; + + const auto vehicle_poly = createVehiclePolygon(p2.pose); + + // debug + { + debug_ptr_->pushDebugPolygons(vehicle_poly); + std::stringstream sstream; + sstream << std::setprecision(4) << travel_time << "s"; + debug_ptr_->pushDebugTexts(sstream.str(), p2.pose, /* lateral_offset */ 3.0); + } + + auto obstacles_collision = + checkCollisionWithObstacles(dynamic_obstacles, vehicle_poly, travel_time); + if (obstacles_collision.empty()) { + continue; + } + + const auto obstacle_selected = findNearestCollisionObstacle(path, p2.pose, obstacles_collision); + if (!obstacle_selected) { + continue; + } + + // debug + { + std::stringstream sstream; + sstream << std::setprecision(4) << "ttc: " << std::to_string(travel_time) << "s"; + debug_ptr_->pushDebugTexts(sstream.str(), obstacle_selected->nearest_collision_point); + debug_ptr_->pushDebugPoints(obstacle_selected->collision_points); + debug_ptr_->pushDebugPoints(obstacle_selected->nearest_collision_point, PointType::Red); + } + + return obstacle_selected; + } + + // no collision detected + return {}; +} + +boost::optional RunOutModule::findNearestCollisionObstacle( + const PathWithLaneId & path, const geometry_msgs::msg::Pose & base_pose, + std::vector & dynamic_obstacles) const +{ + // sort obstacles with distance from ego + std::sort( + dynamic_obstacles.begin(), dynamic_obstacles.end(), + [&path, &base_pose](const auto & lhs, const auto & rhs) -> bool { + const auto dist_lhs = tier4_autoware_utils::calcSignedArcLength( + path.points, base_pose.position, lhs.pose.position); + const auto dist_rhs = tier4_autoware_utils::calcSignedArcLength( + path.points, base_pose.position, rhs.pose.position); + + return dist_lhs < dist_rhs; + }); + + // select obstacle to decelerate from the nearest obstacle + DynamicObstacle obstacle_collision; + for (const auto & obstacle : dynamic_obstacles) { + const auto obstacle_same_side_points = run_out_utils::findLateralSameSidePoints( + obstacle.collision_points, base_pose, obstacle.pose.position); + + const auto nearest_collision_point = run_out_utils::findLongitudinalNearestPoint( + path.points, base_pose.position, obstacle_same_side_points); + + const auto collision_position_from_ego_front = + calcCollisionPositionOfVehicleSide(nearest_collision_point, base_pose); + + // if position of collision on ego side is less than passing margin, + // which is considered to be collision + // TODO(Tomohito Ando): calculate collision position more precisely + if (collision_position_from_ego_front < planner_param_.run_out.passing_margin) { + debug_ptr_->setDebugValues( + DebugValues::TYPE::COLLISION_POS_FROM_EGO_FRONT, collision_position_from_ego_front); + + obstacle_collision = obstacle; + obstacle_collision.nearest_collision_point = nearest_collision_point; + return obstacle_collision; + } + + // the obstacle is considered to be able to pass + debug_ptr_->setAccelReason(RunOutDebug::AccelReason::PASS); + } + + // no collision points + return {}; +} + +// calculate longitudinal offset of collision point from vehicle front +float RunOutModule::calcCollisionPositionOfVehicleSide( + const geometry_msgs::msg::Point & point, const geometry_msgs::msg::Pose & base_pose) const +{ + const auto vehicle_front_pose = tier4_autoware_utils::calcOffsetPose( + base_pose, planner_param_.vehicle_param.base_to_front, 0, 0); + const auto longitudinal_offset_from_front = + std::abs(tier4_autoware_utils::calcLongitudinalDeviation(vehicle_front_pose, point)); + + return longitudinal_offset_from_front; +} + +/** + * p4 p1 + * +----------------+ + * | ↑-> (base) | + * +----------------+ + * p3 p2 + */ +std::vector RunOutModule::createVehiclePolygon( + const geometry_msgs::msg::Pose & base_pose) const +{ + const float base_to_rear = planner_param_.vehicle_param.base_to_rear; + const float base_to_front = planner_param_.vehicle_param.base_to_front; + const float half_width = planner_param_.vehicle_param.width / 2.0; + + const auto p1 = tier4_autoware_utils::calcOffsetPose(base_pose, base_to_front, half_width, 0.0); + const auto p2 = tier4_autoware_utils::calcOffsetPose(base_pose, base_to_front, -half_width, 0.0); + const auto p3 = tier4_autoware_utils::calcOffsetPose(base_pose, -base_to_rear, -half_width, 0.0); + const auto p4 = tier4_autoware_utils::calcOffsetPose(base_pose, -base_to_rear, half_width, 0.0); + + std::vector vehicle_poly; + vehicle_poly.push_back(p1.position); + vehicle_poly.push_back(p2.position); + vehicle_poly.push_back(p3.position); + vehicle_poly.push_back(p4.position); + + return vehicle_poly; +} + +std::vector RunOutModule::checkCollisionWithObstacles( + const std::vector & dynamic_obstacles, + std::vector poly, const float travel_time) const +{ + const auto bg_poly_vehicle = run_out_utils::createBoostPolyFromMsg(poly); + + // check collision for each objects + std::vector obstacles_collision; + for (const auto & obstacle : dynamic_obstacles) { + // get classification that has highest probability + const auto classification = run_out_utils::getHighestProbLabel(obstacle.classifications); + + // detect only pedestrian and bicycle + if ( + classification != ObjectClassification::PEDESTRIAN && + classification != ObjectClassification::BICYCLE) { + continue; + } + + // calculate predicted obstacle pose for min velocity and max velocity + const auto predicted_obstacle_pose_min_vel = + calcPredictedObstaclePose(obstacle.predicted_paths, travel_time, obstacle.min_velocity_mps); + const auto predicted_obstacle_pose_max_vel = + calcPredictedObstaclePose(obstacle.predicted_paths, travel_time, obstacle.max_velocity_mps); + if (!predicted_obstacle_pose_min_vel || !predicted_obstacle_pose_max_vel) { + continue; + } + const PoseWithRange pose_with_range = { + *predicted_obstacle_pose_min_vel, *predicted_obstacle_pose_max_vel}; + + std::vector collision_points; + const bool collision_detected = + checkCollisionWithShape(bg_poly_vehicle, pose_with_range, obstacle.shape, collision_points); + + if (!collision_detected) { + continue; + } + + DynamicObstacle obstacle_collision = obstacle; + obstacle_collision.collision_points = collision_points; + obstacles_collision.emplace_back(obstacle_collision); + } + + return obstacles_collision; +} + +// calculate the predicted pose of the obstacle on the predicted path with given travel time +// assume that the obstacle moves with constant velocity +boost::optional RunOutModule::calcPredictedObstaclePose( + const std::vector & predicted_paths, const float travel_time, + const float velocity_mps) const +{ + // use the path that has highest confidence for now + const auto predicted_path = run_out_utils::getHighestConfidencePath(predicted_paths); + + if (predicted_path.size() < 2) { + RCLCPP_WARN_STREAM(logger_, "predicted path doesn't have enough points"); + return {}; + } + + if ( + travel_time < std::numeric_limits::epsilon() || + velocity_mps < std::numeric_limits::epsilon()) { + return predicted_path.at(0); + } + + // calculate predicted pose + float time_sum = 0.0; + for (size_t i = 1; i < predicted_path.size(); i++) { + const auto & p1 = predicted_path.at(i - 1); + const auto & p2 = predicted_path.at(i); + + const float ds = tier4_autoware_utils::calcDistance2d(p1, p2); + const float dt = ds / velocity_mps; + + // apply linear interpolation between the predicted path points + if (time_sum + dt > travel_time) { + const float time_remaining = travel_time - time_sum; + const float ratio = time_remaining / dt; + return run_out_utils::lerpByPose(p1, p2, ratio); + } + + time_sum += dt; + } + + // reach the end of the predicted path + return predicted_path.back(); +} + +bool RunOutModule::checkCollisionWithShape( + const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, + const Shape & shape, std::vector & collision_points) const +{ + bool collision_detected = false; + switch (shape.type) { + case Shape::CYLINDER: + collision_detected = checkCollisionWithCylinder( + vehicle_polygon, pose_with_range, shape.dimensions.x / 2.0, collision_points); + break; + + case Shape::BOUNDING_BOX: + collision_detected = checkCollisionWithBoundingBox( + vehicle_polygon, pose_with_range, shape.dimensions, collision_points); + break; + + case Shape::POLYGON: + collision_detected = checkCollisionWithPolygon(); + break; + + default: + break; + } + + return collision_detected; +} + +bool RunOutModule::checkCollisionWithCylinder( + const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, + const float radius, std::vector & collision_points) const +{ + // create bounding box for min and max velocity point + const auto bounding_box_for_points = + createBoundingBoxForRangedPoints(pose_with_range, radius, radius); + const auto bg_bounding_box_for_points = + run_out_utils::createBoostPolyFromMsg(bounding_box_for_points); + + // debug + debug_ptr_->pushDebugPolygons(bounding_box_for_points); + + // check collision with 2d polygon + std::vector collision_points_bg; + bg::intersection(vehicle_polygon, bg_bounding_box_for_points, collision_points_bg); + + // no collision detected + if (collision_points_bg.empty()) { + return false; + } + + for (const auto & p : collision_points_bg) { + const auto p_msg = + tier4_autoware_utils::createPoint(p.x(), p.y(), pose_with_range.pose_min.position.z); + collision_points.emplace_back(p_msg); + } + + return true; +} + +// create 2D bounding box for two points +// Box is better to reduce calculation cost? +std::vector RunOutModule::createBoundingBoxForRangedPoints( + const PoseWithRange & pose_with_range, const float x_offset, const float y_offset) const +{ + const auto dist_p1_p2 = + tier4_autoware_utils::calcDistance2d(pose_with_range.pose_min, pose_with_range.pose_max); + + geometry_msgs::msg::Pose p_min_to_p_max; + const auto azimuth_angle = tier4_autoware_utils::calcAzimuthAngle( + pose_with_range.pose_min.position, pose_with_range.pose_max.position); + p_min_to_p_max.position = pose_with_range.pose_min.position; + p_min_to_p_max.orientation = tier4_autoware_utils::createQuaternionFromYaw(azimuth_angle); + + std::vector poly; + poly.emplace_back( + tier4_autoware_utils::calcOffsetPose(p_min_to_p_max, dist_p1_p2 + x_offset, y_offset, 0.0) + .position); + poly.emplace_back( + tier4_autoware_utils::calcOffsetPose(p_min_to_p_max, dist_p1_p2 + x_offset, -y_offset, 0.0) + .position); + poly.emplace_back( + tier4_autoware_utils::calcOffsetPose(p_min_to_p_max, -x_offset, -y_offset, 0.0).position); + poly.emplace_back( + tier4_autoware_utils::calcOffsetPose(p_min_to_p_max, -x_offset, y_offset, 0.0).position); + + return poly; +} + +bool RunOutModule::checkCollisionWithBoundingBox( + const tier4_autoware_utils::Polygon2d & vehicle_polygon, const PoseWithRange pose_with_range, + const geometry_msgs::msg::Vector3 & dimension, + std::vector & collision_points) const +{ + // create bounding box for min and max velocity point + const auto bounding_box = + createBoundingBoxForRangedPoints(pose_with_range, dimension.x / 2.0, dimension.y / 2.0); + const auto bg_bounding_box = run_out_utils::createBoostPolyFromMsg(bounding_box); + + // debug + debug_ptr_->pushDebugPolygons(bounding_box); + + // check collision with 2d polygon + std::vector collision_points_bg; + bg::intersection(vehicle_polygon, bg_bounding_box, collision_points_bg); + + // no collision detected + if (collision_points_bg.empty()) { + return false; + } + + for (const auto & p : collision_points_bg) { + const auto p_msg = + tier4_autoware_utils::createPoint(p.x(), p.y(), pose_with_range.pose_min.position.z); + collision_points.emplace_back(p_msg); + } + + return true; +} + +bool RunOutModule::checkCollisionWithPolygon() const +{ + RCLCPP_WARN_STREAM(logger_, "detection for POLYGON type is not implemented yet."); + + return false; +} + +boost::optional RunOutModule::calcStopPoint( + const boost::optional & dynamic_obstacle, const PathWithLaneId & path, + const geometry_msgs::msg::Pose & current_pose, const float current_vel, + const float current_acc) const +{ + // no obstacles + if (!dynamic_obstacle) { + return {}; + } + + // calculate distance to collision with the obstacle + const float dist_to_collision_point = tier4_autoware_utils::calcSignedArcLength( + path.points, current_pose.position, dynamic_obstacle->nearest_collision_point); + const float dist_to_collision = + dist_to_collision_point - planner_param_.vehicle_param.base_to_front; + + // insert the stop point without considering the distance from the obstacle + // smoother will calculate appropriate jerk for deceleration + if (!planner_param_.run_out.specify_decel_jerk) { + // calculate index of stop point + const float base_to_collision_point = + planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front; + const size_t stop_index = run_out_utils::calcIndexByLengthReverse( + path.points, dynamic_obstacle->nearest_collision_point, base_to_collision_point); + const auto & stop_point = path.points.at(stop_index).point.pose; + + // debug + debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); + debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( + stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); + + return stop_point; + } + + // calculate distance needed to stop with jerk and acc constraints + const float target_vel = 0.0; + const float jerk_dec = planner_param_.run_out.deceleration_jerk; + const float jerk_acc = std::abs(jerk_dec); + const float planning_dec = jerk_dec < planner_param_.common.normal_min_jerk + ? planner_param_.common.limit_min_acc + : planner_param_.common.normal_min_acc; + auto stop_dist = run_out_utils::calcDecelDistWithJerkAndAccConstraints( + current_vel, target_vel, current_acc, planning_dec, jerk_acc, jerk_dec); + if (!stop_dist) { + RCLCPP_WARN_STREAM(logger_, "failed to calculate stop distance."); + + // force to insert zero velocity + stop_dist = boost::make_optional(dist_to_collision); + } + + // debug + { + const float base_to_obstacle = + planner_param_.vehicle_param.base_to_front + planner_param_.run_out.stop_margin; + const auto vehicle_stop_idx = run_out_utils::calcIndexByLength( + path.points, current_pose, stop_dist.get() + base_to_obstacle); + const auto & p = path.points.at(vehicle_stop_idx).point.pose.position; + debug_ptr_->pushDebugPoints(p, PointType::Yellow); + debug_ptr_->setDebugValues(DebugValues::TYPE::STOP_DISTANCE, *stop_dist); + } + + // vehicle have to decelerate if there is not enough distance with deceleration_jerk + const bool deceleration_needed = + *stop_dist > dist_to_collision - planner_param_.run_out.stop_margin; + // avoid acceleration when ego is decelerating + // TODO(Tomohito Ando): replace with more appropriate method + constexpr float epsilon = 1.0e-2; + constexpr float stopping_vel_mps = 2.5 / 3.6; + const bool is_stopping = current_vel < stopping_vel_mps && current_acc < epsilon; + if (!deceleration_needed && !is_stopping) { + debug_ptr_->setAccelReason(RunOutDebug::AccelReason::LOW_JERK); + return {}; + } + + // calculate index of stop point + const float base_to_collision_point = + planner_param_.run_out.stop_margin + planner_param_.vehicle_param.base_to_front; + const size_t stop_index = run_out_utils::calcIndexByLengthReverse( + path.points, dynamic_obstacle->nearest_collision_point, base_to_collision_point); + const auto & stop_point = path.points.at(stop_index).point.pose; + + // debug + debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); + debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( + stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); + + return stop_point; +} + +void RunOutModule::insertStopPoint( + const boost::optional stop_point, + autoware_auto_planning_msgs::msg::PathWithLaneId & path) +{ + // no stop point + if (!stop_point) { + RCLCPP_DEBUG_STREAM(logger_, "already has same point"); + return; + } + + // find nearest point index behind the stop point + const auto nearest_seg_idx = + tier4_autoware_utils::findNearestSegmentIndex(path.points, stop_point->position); + auto insert_idx = nearest_seg_idx + 1; + + // to PathPointWithLaneId + autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + stop_point_with_lane_id = path.points.at(nearest_seg_idx); + stop_point_with_lane_id.point.pose = *stop_point; + + planning_utils::insertVelocity(path, stop_point_with_lane_id, 0.0, insert_idx); +} + +void RunOutModule::insertVelocity( + const boost::optional & dynamic_obstacle, + const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, + const PathWithLaneId & smoothed_path, PathWithLaneId & output_path) +{ + // no obstacles + if (!dynamic_obstacle) { + state_ = State::GO; + return; + } + + const auto longitudinal_offset_to_collision_point = + tier4_autoware_utils::calcSignedArcLength( + smoothed_path.points, current_pose.position, dynamic_obstacle->nearest_collision_point) - + planner_param_.vehicle_param.base_to_front; + // enough distance to the obstacle + if ( + longitudinal_offset_to_collision_point > + planner_param_.run_out.stop_margin + planner_param_.approaching.dist_thresh) { + state_ = State::GO; + } + + switch (state_) { + case State::GO: { + if ( + planner_data_->current_velocity->twist.linear.x < planner_param_.approaching.stop_thresh) { + RCLCPP_DEBUG_STREAM(logger_, "transit to STOP state."); + stop_time_ = clock_->now(); + state_ = State::STOP; + } + + insertStoppingVelocity( + dynamic_obstacle, current_pose, current_vel, current_acc, smoothed_path, output_path); + break; + } + + case State::STOP: { + RCLCPP_DEBUG_STREAM(logger_, "STOP state"); + const auto elapsed_time = (clock_->now() - stop_time_).seconds(); + state_ = + elapsed_time > planner_param_.approaching.stop_time_thresh ? State::APPROACH : State::STOP; + RCLCPP_DEBUG_STREAM(logger_, "elapsed time: " << elapsed_time); + + insertStoppingVelocity( + dynamic_obstacle, current_pose, current_vel, current_acc, smoothed_path, output_path); + break; + } + + case State::APPROACH: { + RCLCPP_DEBUG_STREAM(logger_, "APPROACH state"); + insertApproachingVelocity( + *dynamic_obstacle, current_pose, planner_param_.approaching.limit_vel_kmph / 3.6, + planner_param_.approaching.margin, smoothed_path, output_path); + debug_ptr_->setAccelReason(RunOutDebug::AccelReason::STOP); + break; + } + + default: { + RCLCPP_WARN_STREAM(logger_, "invalid state"); + break; + } + } +} + +void RunOutModule::insertStoppingVelocity( + const boost::optional & dynamic_obstacle, + const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, + const PathWithLaneId & smoothed_path, PathWithLaneId & output_path) +{ + const auto stop_point = + calcStopPoint(dynamic_obstacle, smoothed_path, current_pose, current_vel, current_acc); + insertStopPoint(stop_point, output_path); +} + +void RunOutModule::insertApproachingVelocity( + const DynamicObstacle & dynamic_obstacle, const geometry_msgs::msg::Pose & current_pose, + const float approaching_vel, const float approach_margin, const PathWithLaneId & resampled_path, + PathWithLaneId & output_path) +{ + // insert slow down velocity from nearest segment point + const auto nearest_seg_idx = + tier4_autoware_utils::findNearestSegmentIndex(output_path.points, current_pose.position); + run_out_utils::insertPathVelocityFromIndexLimited( + nearest_seg_idx, approaching_vel, output_path.points); + + // debug + debug_ptr_->pushDebugPoints( + output_path.points.at(nearest_seg_idx).point.pose.position, PointType::Yellow); + + // calculate stop point to insert 0 velocity + const float base_to_collision_point = + approach_margin + planner_param_.vehicle_param.base_to_front; + const auto stop_idx = run_out_utils::calcIndexByLengthReverse( + resampled_path.points, dynamic_obstacle.nearest_collision_point, base_to_collision_point); + const auto & stop_point = resampled_path.points.at(stop_idx).point.pose; + + // debug + debug_ptr_->pushStopPose(tier4_autoware_utils::calcOffsetPose( + stop_point, planner_param_.vehicle_param.base_to_front, 0, 0)); + + const auto nearest_seg_idx_stop = + tier4_autoware_utils::findNearestSegmentIndex(output_path.points, stop_point.position); + auto insert_idx_stop = nearest_seg_idx_stop + 1; + + // to PathPointWithLaneId + // use lane id of point behind inserted point + autoware_auto_planning_msgs::msg::PathPointWithLaneId stop_point_with_lane_id; + stop_point_with_lane_id = output_path.points.at(nearest_seg_idx_stop); + stop_point_with_lane_id.point.pose = stop_point; + + planning_utils::insertVelocity(output_path, stop_point_with_lane_id, 0.0, insert_idx_stop); +} + +void RunOutModule::applyMaxJerkLimit( + const geometry_msgs::msg::Pose & current_pose, const float current_vel, const float current_acc, + PathWithLaneId & path) const +{ + const auto stop_point_idx = run_out_utils::findFirstStopPointIdx(path.points); + if (!stop_point_idx) { + return; + } + + const auto stop_point = path.points.at(stop_point_idx.get()).point.pose.position; + const auto dist_to_stop_point = + tier4_autoware_utils::calcSignedArcLength(path.points, current_pose.position, stop_point); + + // calculate desired velocity with limited jerk + const auto jerk_limited_vel = planning_utils::calcDecelerationVelocityFromDistanceToTarget( + planner_param_.slow_down_limit.max_jerk, planner_param_.slow_down_limit.max_acc, current_acc, + current_vel, dist_to_stop_point); + + // overwrite velocity with limited velocity + run_out_utils::insertPathVelocityFromIndex(stop_point_idx.get(), jerk_limited_vel, path.points); +} + +std::vector RunOutModule::excludeObstaclesOutSideOfPartition( + const std::vector & dynamic_obstacles, const PathWithLaneId & path, + const geometry_msgs::msg::Pose & current_pose) const +{ + if (!planner_param_.run_out.use_partition_lanelet || partition_lanelets_.empty()) { + return dynamic_obstacles; + } + + // extract partitions within detection distance + BasicPolygons2d close_partitions; + planning_utils::extractClosePartition( + current_pose.position, partition_lanelets_, close_partitions, + planner_param_.run_out.detection_distance); + + // decimate trajectory to reduce calculation time + constexpr float decimate_step = 1.0; + const auto decimate_path_points = run_out_utils::decimatePathPoints(path.points, decimate_step); + + // exclude obstacles outside of partition + std::vector extracted_obstacles = dynamic_obstacles; + for (const auto & partition : close_partitions) { + extracted_obstacles = run_out_utils::excludeObstaclesOutSideOfLine( + extracted_obstacles, decimate_path_points, partition); + } + + return extracted_obstacles; +} + +void RunOutModule::publishDebugValue( + const PathWithLaneId & path, const std::vector extracted_obstacles, + const boost::optional & dynamic_obstacle, + const geometry_msgs::msg::Pose & current_pose) const +{ + if (dynamic_obstacle) { + const auto lateral_dist = std::abs(tier4_autoware_utils::calcLateralOffset( + path.points, dynamic_obstacle->pose.position)) - + planner_param_.vehicle_param.width / 2.0; + const auto longitudinal_dist_to_obstacle = + tier4_autoware_utils::calcSignedArcLength( + path.points, current_pose.position, dynamic_obstacle->pose.position) - + planner_param_.vehicle_param.base_to_front; + + const float dist_to_collision_point = tier4_autoware_utils::calcSignedArcLength( + path.points, current_pose.position, dynamic_obstacle->nearest_collision_point); + const auto dist_to_collision = + dist_to_collision_point - planner_param_.vehicle_param.base_to_front; + + debug_ptr_->setDebugValues(DebugValues::TYPE::LONGITUDINAL_DIST_COLLISION, dist_to_collision); + debug_ptr_->setDebugValues(DebugValues::TYPE::LATERAL_DIST, lateral_dist); + debug_ptr_->setDebugValues( + DebugValues::TYPE::LONGITUDINAL_DIST_OBSTACLE, longitudinal_dist_to_obstacle); + } else { + // max value + constexpr float max_val = 50.0; + debug_ptr_->setDebugValues(DebugValues::TYPE::LATERAL_DIST, max_val); + debug_ptr_->setDebugValues(DebugValues::TYPE::LONGITUDINAL_DIST_COLLISION, max_val); + debug_ptr_->setDebugValues(DebugValues::TYPE::LONGITUDINAL_DIST_OBSTACLE, max_val); + } + + if (extracted_obstacles.empty()) { + debug_ptr_->setAccelReason(RunOutDebug::AccelReason::NO_OBSTACLE); + } + + debug_ptr_->publishDebugValue(); +} + +} // namespace behavior_velocity_planner diff --git a/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp new file mode 100644 index 0000000000000..1f7f6fa72a0a5 --- /dev/null +++ b/planning/behavior_velocity_planner/src/scene_module/run_out/utils.cpp @@ -0,0 +1,510 @@ +// 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 "scene_module/run_out/utils.hpp" + +namespace behavior_velocity_planner +{ +namespace run_out_utils +{ +bool validCheckDecelPlan( + const double v_end, const double a_end, const double v_target, const double a_target, + const double v_margin, const double a_margin) +{ + const double v_min = v_target - std::abs(v_margin); + const double v_max = v_target + std::abs(v_margin); + const double a_min = a_target - std::abs(a_margin); + const double a_max = a_target + std::abs(a_margin); + + if (v_end < v_min || v_max < v_end) { + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("validCheckDecelPlan"), + "[validCheckDecelPlan] valid check error! v_target = " << v_target << ", v_end = " << v_end); + return false; + } + if (a_end < a_min || a_max < a_end) { + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("validCheckDecelPlan"), + "[validCheckDecelPlan] valid check error! a_target = " << a_target << ", a_end = " << a_end); + return false; + } + + return true; +} +/** + * @brief calculate distance until velocity is reached target velocity (TYPE1) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @param (t_min) duration of constant deceleration [s] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType1( + const double v0, const double vt, const double a0, const double am, const double ja, + const double jd, const double t_min) +{ + constexpr double epsilon = 1e-3; + + const double j1 = am < a0 ? jd : ja; + const double t1 = epsilon < (am - a0) / j1 ? (am - a0) / j1 : 0.0; + const double a1 = a0 + j1 * t1; + const double v1 = v0 + a0 * t1 + 0.5 * j1 * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * j1 * t1 * t1 * t1; + + const double t2 = epsilon < t_min ? t_min : 0.0; + const double a2 = a1; + const double v2 = v1 + a1 * t2; + const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2; + + const double t3 = epsilon < (0.0 - am) / ja ? (0.0 - am) / ja : 0.0; + const double a3 = a2 + ja * t3; + const double v3 = v2 + a2 * t3 + 0.5 * ja * t3 * t3; + const double x3 = x2 + v2 * t3 + 0.5 * a2 * t3 * t3 + (1.0 / 6.0) * ja * t3 * t3 * t3; + + const double a_target = 0.0; + const double v_margin = 0.3; // [m/s] + const double a_margin = 0.1; // [m/s^2] + + if (!validCheckDecelPlan(v3, a3, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x3; +} +/** + * @brief calculate distance until velocity is reached target velocity (TYPE2) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (am) minimum deceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @param (jd) minimum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType2( + const double v0, const double vt, const double a0, const double ja, const double jd) +{ + constexpr double epsilon = 1e-3; + + const double a1_square = (vt - v0 - 0.5 * (0.0 - a0) / jd * a0) * (2.0 * ja * jd / (ja - jd)); + const double a1 = -std::sqrt(a1_square); + + const double t1 = epsilon < (a1 - a0) / jd ? (a1 - a0) / jd : 0.0; + const double v1 = v0 + a0 * t1 + 0.5 * jd * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * jd * t1 * t1 * t1; + + const double t2 = epsilon < (0.0 - a1) / ja ? (0.0 - a1) / ja : 0.0; + const double a2 = a1 + ja * t2; + const double v2 = v1 + a1 * t2 + 0.5 * ja * t2 * t2; + const double x2 = x1 + v1 * t2 + 0.5 * a1 * t2 * t2 + (1.0 / 6.0) * ja * t2 * t2 * t2; + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v2, a2, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x2; +} +/** + * @brief calculate distance until velocity is reached target velocity (TYPE3) + * @param (v0) current velocity [m/s] + * @param (vt) target velocity [m/s] + * @param (a0) current acceleration [m/ss] + * @param (ja) maximum jerk [m/sss] + * @return moving distance until velocity is reached vt [m] + * @detail TODO(Satoshi Ota) + */ +boost::optional calcDecelDistPlanType3( + const double v0, const double vt, const double a0, const double ja) +{ + constexpr double epsilon = 1e-3; + + const double t_acc = (0.0 - a0) / ja; + + const double t1 = epsilon < t_acc ? t_acc : 0.0; + const double a1 = a0 + ja * t1; + const double v1 = v0 + a0 * t1 + 0.5 * ja * t1 * t1; + const double x1 = v0 * t1 + 0.5 * a0 * t1 * t1 + (1.0 / 6.0) * ja * t1 * t1 * t1; + + const double a_target = 0.0; + const double v_margin = 0.3; + const double a_margin = 0.1; + + if (!validCheckDecelPlan(v1, a1, vt, a_target, v_margin, a_margin)) { + return {}; + } + + return x1; +} +boost::optional calcDecelDistWithJerkAndAccConstraints( + const double current_vel, const double target_vel, const double current_acc, const double acc_min, + const double jerk_acc, const double jerk_dec) +{ + constexpr double epsilon = 1e-3; + const double t_dec = + acc_min < current_acc ? (acc_min - current_acc) / jerk_dec : (acc_min - current_acc) / jerk_acc; + const double t_acc = (0.0 - acc_min) / jerk_acc; + const double t_min = (target_vel - current_vel - current_acc * t_dec - + 0.5 * jerk_dec * t_dec * t_dec - 0.5 * acc_min * t_acc) / + acc_min; + + // check if it is possible to decelerate to the target velocity + // by simply bringing the current acceleration to zero. + const auto is_decel_needed = + 0.5 * (0.0 - current_acc) / jerk_acc * current_acc > target_vel - current_vel; + + if (t_min > epsilon) { + return calcDecelDistPlanType1( + current_vel, target_vel, current_acc, acc_min, jerk_acc, jerk_dec, t_min); + } else if (is_decel_needed || current_acc > epsilon) { + return calcDecelDistPlanType2(current_vel, target_vel, current_acc, jerk_acc, jerk_dec); + } else { + return calcDecelDistPlanType3(current_vel, target_vel, current_acc, jerk_acc); + } + + return {}; +} + +Polygon2d createBoostPolyFromMsg(const std::vector & input_poly) +{ + Polygon2d bg_poly; + for (const auto & p : input_poly) { + bg_poly.outer().emplace_back(bg::make(p.x, p.y)); + } + + // one more point to close polygon + const auto & first_point = input_poly.front(); + bg_poly.outer().emplace_back(bg::make(first_point.x, first_point.y)); + + return bg_poly; +} + +std::uint8_t getHighestProbLabel(const std::vector & classification) +{ + std::uint8_t label = autoware_auto_perception_msgs::msg::ObjectClassification::UNKNOWN; + float highest_prob = 0.0; + for (const auto & _class : classification) { + if (highest_prob < _class.probability) { + highest_prob = _class.probability; + label = _class.label; + } + } + return label; +} + +std::vector getHighestConfidencePath( + const std::vector & predicted_paths) +{ + std::vector predicted_path{}; + float highest_confidence = 0.0; + for (const auto & path : predicted_paths) { + if (path.confidence > highest_confidence) { + highest_confidence = path.confidence; + predicted_path = path.path; + } + } + + return predicted_path; +} + +// apply linear interpolation to position +geometry_msgs::msg::Pose lerpByPose( + const geometry_msgs::msg::Pose & p1, const geometry_msgs::msg::Pose & p2, const float t) +{ + tf2::Transform tf_transform1, tf_transform2; + tf2::fromMsg(p1, tf_transform1); + tf2::fromMsg(p2, tf_transform2); + const auto & tf_point = tf2::lerp(tf_transform1.getOrigin(), tf_transform2.getOrigin(), t); + + geometry_msgs::msg::Pose pose; + pose.position.x = tf_point.getX(); + pose.position.y = tf_point.getY(); + pose.position.z = tf_point.getZ(); + pose.orientation = p1.orientation; + return pose; +} + +std::vector findLateralSameSidePoints( + const std::vector & points, const geometry_msgs::msg::Pose & base_pose, + const geometry_msgs::msg::Point & target_point) +{ + const auto signed_deviation = tier4_autoware_utils::calcLateralDeviation(base_pose, target_point); + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("findLateralSameSidePoints"), "signed dev of target: " << signed_deviation); + + std::vector same_side_points; + for (const auto & p : points) { + const auto signed_deviation_of_point = tier4_autoware_utils::calcLateralDeviation(base_pose, p); + RCLCPP_DEBUG_STREAM( + rclcpp::get_logger("findLateralSameSidePoints"), + "signed dev of point: " << signed_deviation_of_point); + if (signed_deviation * signed_deviation_of_point > 0) { + same_side_points.emplace_back(p); + } + } + + if (same_side_points.empty()) { + return points; + } + + return same_side_points; +} + +bool isSamePoint(const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2) +{ + if (tier4_autoware_utils::calcDistance2d(p1, p2) < std::numeric_limits::epsilon()) { + return true; + } + + return false; +} + +// if path points have the same point as target_point, return the index +boost::optional haveSamePoint( + const PathPointsWithLaneId & path_points, const geometry_msgs::msg::Point & target_point) +{ + for (size_t i = 0; i < path_points.size(); i++) { + const auto & path_point = path_points.at(i).point.pose.position; + if (isSamePoint(path_point, target_point)) { + return i; + } + } + + return {}; +} + +// insert path velocity which doesn't exceed original velocity +void insertPathVelocityFromIndexLimited( + const size_t & start_idx, const float velocity_mps, PathPointsWithLaneId & path_points) +{ + for (size_t i = start_idx; i < path_points.size(); i++) { + const auto current_path_vel = path_points.at(i).point.longitudinal_velocity_mps; + path_points.at(i).point.longitudinal_velocity_mps = std::min(velocity_mps, current_path_vel); + } +} + +void insertPathVelocityFromIndex( + const size_t & start_idx, const float velocity_mps, PathPointsWithLaneId & path_points) +{ + for (size_t i = start_idx; i < path_points.size(); i++) { + path_points.at(i).point.longitudinal_velocity_mps = velocity_mps; + } +} + +boost::optional findFirstStopPointIdx(PathPointsWithLaneId & path_points) +{ + for (size_t i = 0; i < path_points.size(); i++) { + const auto vel = path_points.at(i).point.longitudinal_velocity_mps; + if (vel < std::numeric_limits::epsilon()) { + return i; + } + } + + return {}; +} + +LineString2d createLineString2d(const lanelet::BasicPolygon2d & poly) +{ + LineString2d line_string; + for (const auto & p : poly) { + Point2d bg_point{p.x(), p.y()}; + line_string.push_back(bg_point); + } + + return line_string; +} + +std::vector excludeObstaclesOutSideOfLine( + const std::vector & dynamic_obstacles, const PathPointsWithLaneId & path_points, + const lanelet::BasicPolygon2d & partition) +{ + std::vector extracted_dynamic_obstacle; + for (const auto & obstacle : dynamic_obstacles) { + const auto obstacle_nearest_idx = + tier4_autoware_utils::findNearestIndex(path_points, obstacle.pose.position); + const auto & obstacle_nearest_path_point = + path_points.at(obstacle_nearest_idx).point.pose.position; + + // create linestring from traj point to obstacle + const LineString2d path_point_to_obstacle{ + {obstacle_nearest_path_point.x, obstacle_nearest_path_point.y}, + {obstacle.pose.position.x, obstacle.pose.position.y}}; + + // create linestring for partition + const LineString2d partition_bg = createLineString2d(partition); + + // ignore obstacle outside of partition + if (bg::intersects(path_point_to_obstacle, partition_bg)) { + continue; + } + extracted_dynamic_obstacle.emplace_back(obstacle); + } + + return extracted_dynamic_obstacle; +} + +PathPointsWithLaneId decimatePathPoints( + const PathPointsWithLaneId & input_path_points, const float step) +{ + if (input_path_points.empty()) { + return PathPointsWithLaneId(); + } + + float dist_sum = 0.0; + PathPointsWithLaneId decimate_path_points; + // push first point + decimate_path_points.emplace_back(input_path_points.front()); + + for (size_t i = 1; i < input_path_points.size(); i++) { + const auto p1 = input_path_points.at(i - 1); + const auto p2 = input_path_points.at(i); + const auto dist = tier4_autoware_utils::calcDistance2d(p1, p2); + dist_sum += dist; + + if (dist_sum > step) { + decimate_path_points.emplace_back(p2); + dist_sum = 0.0; + } + } + + return decimate_path_points; +} + +// trim path from self_pose to trim_distance +PathWithLaneId trimPathFromSelfPose( + const PathWithLaneId & input, const geometry_msgs::msg::Pose & self_pose, + const double trim_distance) +{ + const size_t nearest_idx = + tier4_autoware_utils::findNearestIndex(input.points, self_pose.position); + + PathWithLaneId output{}; + output.header = input.header; + output.drivable_area = input.drivable_area; + double dist_sum = 0; + for (size_t i = nearest_idx; i < input.points.size(); ++i) { + output.points.push_back(input.points.at(i)); + + if (i != nearest_idx) { + dist_sum += tier4_autoware_utils::calcDistance2d(input.points.at(i - 1), input.points.at(i)); + } + + if (dist_sum > trim_distance) { + break; + } + } + + return output; +} + +std::vector createDetectionAreaPolygon( + const geometry_msgs::msg::Pose & current_pose, const DetectionAreaSize detection_area_size) +{ + const auto & d = detection_area_size; + const auto p1 = tier4_autoware_utils::calcOffsetPose(current_pose, d.dist_ahead, d.dist_left, 0); + const auto p2 = + tier4_autoware_utils::calcOffsetPose(current_pose, d.dist_ahead, -d.dist_right, 0); + const auto p3 = + tier4_autoware_utils::calcOffsetPose(current_pose, -d.dist_behind, -d.dist_right, 0); + const auto p4 = + tier4_autoware_utils::calcOffsetPose(current_pose, -d.dist_behind, d.dist_left, 0); + + std::vector detection_area; + detection_area.emplace_back(p1.position); + detection_area.emplace_back(p2.position); + detection_area.emplace_back(p3.position); + detection_area.emplace_back(p4.position); + + return detection_area; +} + +// create polygon for passing lines and deceleration line calculated by stopping jerk +// note that this polygon is not closed +boost::optional> createDetectionAreaPolygon( + const std::vector> & passing_lines, + const size_t deceleration_line_idx) +{ + if (passing_lines.size() != 2) { + return {}; + } + + std::vector detection_area_polygon; + const auto & line1 = passing_lines.at(0); + const int poly_corner_idx = std::min(deceleration_line_idx, line1.size() - 1); + for (int i = 0; i <= poly_corner_idx; i++) { + const auto & p = line1.at(i); + detection_area_polygon.push_back(p); + } + + // push points from the end to create the polygon + const auto & line2 = passing_lines.at(1); + for (int i = poly_corner_idx; i >= 0; i--) { + const auto & p = line2.at(i); + detection_area_polygon.push_back(p); + } + + return detection_area_polygon; +} + +PathPointWithLaneId createExtendPathPoint( + const double extend_distance, const PathPointWithLaneId & goal_point) +{ + PathPointWithLaneId extend_path_point = goal_point; + extend_path_point.point.pose = + tier4_autoware_utils::calcOffsetPose(goal_point.point.pose, extend_distance, 0.0, 0.0); + return extend_path_point; +} + +PathWithLaneId extendPath(const PathWithLaneId & input, const double extend_distance) +{ + PathWithLaneId output = input; + if (extend_distance < std::numeric_limits::epsilon() || input.points.empty()) { + return output; + } + + const auto goal_point = input.points.back(); + constexpr double interpolation_interval = 0.1; + double extend_sum = interpolation_interval; + while (extend_sum < extend_distance) { + const auto extend_path_point = createExtendPathPoint(extend_sum, goal_point); + output.points.push_back(extend_path_point); + extend_sum += interpolation_interval; + } + + return output; +} + +DetectionMethod toEnum(const std::string & detection_method) +{ + if (detection_method == "Object") { + return DetectionMethod::Object; + } else if (detection_method == "ObjectWithoutPath") { + return DetectionMethod::ObjectWithoutPath; + } else if (detection_method == "Points") { + return DetectionMethod::Points; + } else { + return DetectionMethod::Unknown; + } +} + +} // namespace run_out_utils +} // namespace behavior_velocity_planner